@woosh/meep-engine 2.110.7 → 2.110.9

package/src/engine/physics/inverse_kinematics/fabrik/fabrik3d_solve_primitive.js

@@ -0,0 +1,277 @@
+import { v3_displace_in_direction_array } from "../../../../core/geom/vec3/v3_displace_in_direction_array.js";
+import { v3_distance } from "../../../../core/geom/vec3/v3_distance.js";
+import { v3_distance_sqr } from "../../../../core/geom/vec3/v3_distance_sqr.js";
+import { v3_length_sqr } from "../../../../core/geom/vec3/v3_length_sqr.js";
+import { v3_normalize_array } from "../../../../core/geom/vec3/v3_normalize_array.js";
+
+const scratch_direction = new Float32Array(3);
+
+/**
+ * Implementation of FABRIK algorithm, "Forward And Backward Reaching Inverse Kinematics"
+ * This implementation deals with single chain at a time, without support for multiple effectors
+ * see "FABRIK: a fast, iterative solver for inverse kinematics"
+ * @param {number} size number of points in the chain
+ * @param {Float32Array|number[]} positions
+ * @param {Float32Array|number[]} lengths
+ * @param {number} origin_x
+ * @param {number} origin_y
+ * @param {number} origin_z
+ * @param {number} target_x
+ * @param {number} target_y
+ * @param {number} target_z
+ * @param {number} [max_iterations] More steps will lead to higher accuracy, but at the cost of computation. Generally solution will be reached in just a few iteration so this is just a ceiling
+ * @param {number} [distance_tolerance] Minimum squared distance to be achieved to the target, used to terminate earlier before maximum number of iterations is reached
+ */
+export function fabrik3d_solve_primitive(
+    size,
+    positions,
+    lengths,
+    origin_x, origin_y, origin_z,
+    target_x, target_y, target_z,
+    max_iterations = 4,
+    distance_tolerance = 1e-7
+) {
+
+    if (size === 0) {
+        // nothing to solve, chain has insufficient number of joints
+        return;
+    }
+
+    if (
+        !is_reachable(
+            size, lengths,
+            origin_x, origin_y, origin_z,
+            target_x, target_y, target_z)
+    ) {
+        // target is not reachable, so stretch out the chain towards it instead
+        reach_out(
+            size,
+            positions,
+            lengths,
+            origin_x, origin_y, origin_z,
+            target_x, target_y, target_z
+        );
+
+        return;
+    }
+
+
+    for (let i = 0; i < max_iterations; i++) {
+
+        solve_backward(size, positions, lengths, target_x, target_y, target_z);
+        solve_forward(size, positions, lengths, origin_x, origin_y, origin_z);
+
+        const last_join_address = (size - 1) * 3;
+
+        const last_joint_x = positions[last_join_address];
+        const last_joint_y = positions[last_join_address + 1];
+        const last_joint_z = positions[last_join_address + 2];
+
+        if (v3_distance_sqr(last_joint_x, last_joint_y, last_joint_z, target_x, target_y, target_z) <= distance_tolerance) {
+            // close enough
+            break;
+        }
+
+    }
+
+}
+
+/**
+ * @param {number} size number of points in the chain
+ * @param {Float32Array|number[]} positions
+ * @param {Float32Array|number[]} lengths
+ * @param {number} origin_x
+ * @param {number} origin_y
+ * @param {number} origin_z
+ * @param {number} target_x
+ * @param {number} target_y
+ * @param {number} target_z
+ */
+function reach_out(
+    size,
+    positions,
+    lengths,
+    origin_x, origin_y, origin_z,
+    target_x, target_y, target_z,
+) {
+
+    positions[0] = origin_x;
+    positions[1] = origin_y;
+    positions[2] = origin_z;
+
+    scratch_direction[0] = target_x - origin_x;
+    scratch_direction[1] = target_y - origin_y;
+    scratch_direction[2] = target_z - origin_z;
+
+    v3_normalize_array(scratch_direction, 0, scratch_direction, 0);
+
+    for (let i = 1; i < size; i++) {
+        const current_offset = i * 3;
+
+        const previous_offset = current_offset - 3;
+
+        const previous_x = positions[previous_offset];
+        const previous_y = positions[previous_offset + 1];
+        const previous_z = positions[previous_offset + 2];
+
+        const length = lengths[i - 1];
+
+        positions[current_offset] = previous_x + scratch_direction[0] * length;
+        positions[current_offset + 1] = previous_y + scratch_direction[1] * length;
+        positions[current_offset + 2] = previous_z + scratch_direction[2] * length;
+    }
+
+}
+
+/**
+ *
+ * @param {number} size
+ * @param {number[]} lengths
+ * @param {number} origin_x
+ * @param {number} origin_y
+ * @param {number} origin_z
+ * @param {number} target_x
+ * @param {number} target_y
+ * @param {number} target_z
+ */
+function is_reachable(
+    size, lengths,
+    origin_x, origin_y, origin_z,
+    target_x, target_y, target_z
+) {
+
+    let total_length = 0;
+
+    const limit = size - 1;
+
+    for (let i = 0; i < limit; i++) {
+        const x = lengths[i];
+
+        total_length += x;
+    }
+
+    const distance_to_target = v3_distance(origin_x, origin_y, origin_z, target_x, target_y, target_z);
+
+    return distance_to_target <= total_length;
+}
+
+/**
+ * Produced vector normalize(B-A)
+ * @param result
+ * @param result_offset
+ * @param positions
+ * @param offsetA
+ * @param offsetB
+ */
+function compute_direction(
+    result, result_offset,
+    positions, offsetA, offsetB
+) {
+
+    const ax = positions[offsetA];
+    const ay = positions[offsetA + 1];
+    const az = positions[offsetA + 2];
+
+    const bx = positions[offsetB];
+    const by = positions[offsetB + 1];
+    const bz = positions[offsetB + 2];
+
+    const dx = bx - ax;
+    const dy = by - ay;
+    const dz = bz - az;
+
+    const length_sqr = v3_length_sqr(dx, dy, dz);
+
+    if (length_sqr === 0) {
+        // TODO use random point on a sphere to prevent pathology
+
+        // points are on top of each other, set arbitrary direction
+        result[result_offset + 0] = 0;
+        result[result_offset + 1] = 0;
+        result[result_offset + 2] = 1;
+
+        return;
+    }
+
+    const m = 1 / Math.sqrt(length_sqr);
+
+    result[result_offset + 0] = dx * m;
+    result[result_offset + 1] = dy * m;
+    result[result_offset + 2] = dz * m;
+}
+
+
+/**
+ *
+ * @param {number} size
+ * @param {Float32Array} positions
+ * @param {Float32Array|number[]} lengths
+ * @param {number} origin_x
+ * @param {number} origin_y
+ * @param {number} origin_z
+ */
+function solve_forward(size, positions, lengths, origin_x, origin_y, origin_z) {
+
+    // move first point to origin
+    positions[0] = origin_x;
+    positions[1] = origin_y;
+    positions[2] = origin_z;
+
+    for (let i = 1; i < size; i++) {
+        const current_address = i * 3;
+        const previous_address = current_address - 3;
+
+        compute_direction(
+            scratch_direction, 0,
+            positions, previous_address, current_address
+        );
+
+        const length = lengths[i - 1];
+
+        v3_displace_in_direction_array(
+            positions, current_address,
+            positions, previous_address,
+            scratch_direction, 0,
+            length
+        );
+    }
+
+}
+
+/**
+ *
+ * @param {number} size
+ * @param {Float32Array} positions
+ * @param {Float32Array|number[]} lengths
+ * @param {number} target_x
+ * @param {number} target_y
+ * @param {number} target_z
+ */
+function solve_backward(size, positions, lengths, target_x, target_y, target_z) {
+
+    const last_index = size - 1;
+
+    // move last point to target
+    const last_address = last_index * 3;
+
+    positions[last_address] = target_x;
+    positions[last_address + 1] = target_y;
+    positions[last_address + 2] = target_z;
+
+    for (let i = last_index - 1; i >= 0; i--) {
+        const current_address = i * 3;
+        const next_address = current_address + 3;
+
+        compute_direction(
+            scratch_direction, 0,
+            positions, next_address, current_address
+        );
+
+        v3_displace_in_direction_array(
+            positions, current_address,
+            positions, next_address,
+            scratch_direction, 0,
+            lengths[i]
+        )
+    }
+}

package/src/engine/physics/inverse_kinematics/fabrik/fabrik3d_solve_primitive.spec.d.ts

@@ -0,0 +1,2 @@
+export {};
+//# sourceMappingURL=fabrik3d_solve_primitive.spec.d.ts.map

package/src/engine/physics/inverse_kinematics/fabrik/fabrik3d_solve_primitive.spec.d.ts.map

@@ -0,0 +1 @@
+{"version":3,"file":"fabrik3d_solve_primitive.spec.d.ts","sourceRoot":"","sources":["../../../../../../src/engine/physics/inverse_kinematics/fabrik/fabrik3d_solve_primitive.spec.js"],"names":[],"mappings":""}

package/src/engine/physics/inverse_kinematics/fabrik/fabrik3d_solve_primitive.spec.js

@@ -0,0 +1,43 @@
+import { fabrik3d_solve_primitive } from "./fabrik3d_solve_primitive.js";
+
+test("0 size chain solve should succeed", () => {
+
+    fabrik3d_solve_primitive(
+        0, [], [],
+        0, 0, 0,
+        0, 0, 1
+    );
+
+});
+
+test("1 joint chain", () => {
+
+    const positions = [-1, -1, -1];
+
+    fabrik3d_solve_primitive(
+        1, positions, [1],
+        1, 2, 3,
+        0, 0, 1
+    );
+
+    expect(positions).toEqual([1, 2, 3]);
+});
+
+test("2 joint chain", () => {
+    const positions = [
+        -1, -1, -1,
+        -1, -1, -1,
+    ];
+
+    fabrik3d_solve_primitive(
+        2, positions, [1, 1],
+        1, 2, 3,
+        2, 2, 3
+    );
+
+    expect(positions).toEqual([
+        1, 2, 3,
+        2, 2, 3
+    ]);
+
+});

package/src/engine/physics/inverse_kinematics/fabrik/fabrik_solve.d.ts

@@ -0,0 +1,12 @@
+/**
+ *
+ * @param {Transform[]} joints
+ * @param {number[]} lengths distance to next bone
+ * @param {Vector3} origin where should the first joint be placed at
+ * @param {Vector3} target where should the last joint be placed at
+ * @param {number} [max_iterations] More steps will lead to higher accuracy, but at the cost of computation. Generally solution will be reached in just a few iteration so this is just a ceiling
+ * @param {number} [distance_tolerance] Minimum squared distance to be achieved to the target, used to terminate earlier before maximum number of iterations is reached
+ */
+export function fabrik_solve(joints: Transform[], lengths: number[], origin: Vector3, target: Vector3, max_iterations?: number, distance_tolerance?: number): void;
+import Vector3 from "../../../../core/geom/Vector3.js";
+//# sourceMappingURL=fabrik_solve.d.ts.map

package/src/engine/physics/inverse_kinematics/fabrik/fabrik_solve.d.ts.map

@@ -0,0 +1 @@
+{"version":3,"file":"fabrik_solve.d.ts","sourceRoot":"","sources":["../../../../../../src/engine/physics/inverse_kinematics/fabrik/fabrik_solve.js"],"names":[],"mappings":"AAWA;;;;;;;;GAQG;AACH,qCAPW,WAAW,WACX,MAAM,EAAE,UACR,OAAO,UACP,OAAO,mBACP,MAAM,uBACN,MAAM,QAgFhB;oBAjGmB,kCAAkC"}

package/src/engine/physics/inverse_kinematics/fabrik/fabrik_solve.js

@@ -0,0 +1,100 @@
+import Quaternion from "../../../../core/geom/Quaternion.js";
+import Vector3 from "../../../../core/geom/Vector3.js";
+import { fabrik3d_solve_primitive } from "./fabrik3d_solve_primitive.js";
+
+const scratch_positions_count = 64;
+const scratch_positions = new Float32Array(scratch_positions_count * 3);
+
+const scratch_v3_0 = new Vector3();
+const scratch_v3_1 = new Vector3();
+const scratch_quat = new Quaternion();
+
+/**
+ *
+ * @param {Transform[]} joints
+ * @param {number[]} lengths distance to next bone
+ * @param {Vector3} origin where should the first joint be placed at
+ * @param {Vector3} target where should the last joint be placed at
+ * @param {number} [max_iterations] More steps will lead to higher accuracy, but at the cost of computation. Generally solution will be reached in just a few iteration so this is just a ceiling
+ * @param {number} [distance_tolerance] Minimum squared distance to be achieved to the target, used to terminate earlier before maximum number of iterations is reached
+ */
+export function fabrik_solve(
+    joints,
+    lengths,
+    origin,
+    target,
+    max_iterations = 4,
+    distance_tolerance = 1e-7
+) {
+
+    const joint_count = joints.length;
+
+    let positions;
+    if (joint_count <= scratch_positions_count) {
+        positions = scratch_positions;
+    } else {
+        positions = new Float32Array(joint_count * 3);
+    }
+
+    for (let i = 0; i < joint_count; i++) {
+        const offset = i * 3;
+        const joint = joints[i];
+
+        joint.position.toArray(positions, offset);
+    }
+
+    fabrik3d_solve_primitive(
+        joint_count,
+        positions,
+        lengths,
+        origin.x, origin.y, origin.z,
+        target.x, target.y, target.z,
+        max_iterations, distance_tolerance
+    );
+
+    // apply rotation
+    for (let i = 1; i < joint_count; i++) {
+        const current_joint = joints[i];
+        const previous_joint = joints[i - 1];
+
+        const i3 = i * 3;
+
+        const previous_x = positions[i3 - 3];
+        const previous_y = positions[i3 - 2];
+        const previous_z = positions[i3 - 1];
+
+        const current_x = positions[i3];
+        const current_y = positions[i3 + 1];
+        const current_z = positions[i3 + 2];
+
+        scratch_v3_0.copy(current_joint.position);
+        scratch_v3_0.sub(previous_joint.position);
+
+        if (scratch_v3_0.lengthSqr() === 0) {
+            continue;
+        }
+
+        scratch_v3_1.set(
+            current_x - previous_x,
+            current_y - previous_y,
+            current_z - previous_z,
+        );
+
+        if (scratch_v3_1.lengthSqr() === 0) {
+            continue;
+        }
+
+        scratch_v3_0.normalize();
+        scratch_v3_1.normalize();
+
+        scratch_quat.fromUnitVectors(scratch_v3_0, scratch_v3_1);
+
+        previous_joint.rotation.multiplyQuaternions(scratch_quat, previous_joint.rotation);
+    }
+
+    // apply positions
+    for (let i = 0; i < joint_count; i++) {
+        joints[i].position.fromArray(positions, i * 3);
+    }
+}
+

package/src/engine/physics/inverse_kinematics/fabrik/prototype.d.ts

@@ -0,0 +1,2 @@
+export {};
+//# sourceMappingURL=prototype.d.ts.map

package/src/engine/physics/inverse_kinematics/fabrik/prototype.d.ts.map

@@ -0,0 +1 @@
+{"version":3,"file":"prototype.d.ts","sourceRoot":"","sources":["../../../../../../src/engine/physics/inverse_kinematics/fabrik/prototype.js"],"names":[],"mappings":""}

package/src/engine/physics/inverse_kinematics/fabrik/prototype.js

@@ -0,0 +1,112 @@
+import { CylinderBufferGeometry, MeshStandardMaterial } from "three";
+import { TransformControls } from "../../../../../editor/tools/v2/TransformControls.js";
+import Vector3 from "../../../../core/geom/Vector3.js";
+import { DEG_TO_RAD } from "../../../../core/math/DEG_TO_RAD.js";
+import Entity from "../../../ecs/Entity.js";
+import { TransformAttachmentSystem } from "../../../ecs/transform-attachment/TransformAttachmentSystem.js";
+import { Transform } from "../../../ecs/transform/Transform.js";
+import { EngineHarness } from "../../../EngineHarness.js";
+import { ShadedGeometry } from "../../../graphics/ecs/mesh-v2/ShadedGeometry.js";
+import { ShadedGeometrySystem } from "../../../graphics/ecs/mesh-v2/ShadedGeometrySystem.js";
+import InputController from "../../../input/ecs/components/InputController.js";
+import InputControllerSystem from "../../../input/ecs/systems/InputControllerSystem.js";
+import { BehaviorComponent } from "../../../intelligence/behavior/ecs/BehaviorComponent.js";
+import { BehaviorSystem } from "../../../intelligence/behavior/ecs/BehaviorSystem.js";
+import { fabrik_solve } from "./fabrik_solve.js";
+
+EngineHarness.bootstrap({
+    configuration(config, engine) {
+        config.addSystem(new ShadedGeometrySystem(engine));
+        config.addSystem(new InputControllerSystem(engine.devices));
+        config.addSystem(new TransformAttachmentSystem());
+        config.addSystem(new BehaviorSystem(engine));
+    }
+})
+    .then(async engine => {
+
+        const ecd = engine.entityManager.dataset;
+
+        await EngineHarness.buildBasics({
+            engine,
+            cameraController: false
+        });
+
+
+        const controls = new TransformControls(engine.graphics.camera, engine.viewStack.el);
+
+        /**
+         *
+         * @type {Entity[]}
+         */
+        const chain = [];
+
+
+        const link_geometry = new CylinderBufferGeometry(1, 1, 2);
+        link_geometry.rotateX(90 * DEG_TO_RAD);
+
+        for (let i = 0; i < 50; i++) {
+
+            const entity = new Entity()
+                .add(Transform.fromJSON({
+                    position: {
+                        x: 10,
+                        y: 0,
+                        z: 10
+                    },
+                    scale: 0.1
+                }));
+
+                entity.add(ShadedGeometry.from(link_geometry, new MeshStandardMaterial()));
+
+            chain.push(entity);
+
+            entity.build(ecd);
+        }
+
+        controls.build(ecd);
+        controls.attach(chain[chain.length - 1].id);
+
+        const joints = chain.map(e => e.getComponent(Transform));
+        const lengths = chain.map(x => 0.1);
+
+        new Entity()
+            .add(BehaviorComponent.looping_function(timeDelta => {
+                const last = joints[joints.length - 1];
+                const target = last.position.clone();
+
+                for (let i = 0; i < joints.length; i++) {
+                    // gravity
+                    joints[i].position._add(0, -9 * timeDelta, 0);
+                }
+
+
+                fabrik_solve(
+                    joints,
+                    lengths,
+                    new Vector3(10, 2, 10),
+                    target
+                );
+
+                last.position.copy(target);
+            }))
+            .build(ecd);
+
+        new Entity()
+            .add(new InputController([{
+                path: 'keyboard/keys/w/down',
+                listener() {
+                    controls.mode = "translate"
+                }
+            }, {
+                path: 'keyboard/keys/e/down',
+                listener() {
+                    controls.mode = "scale"
+                }
+            }, {
+                path: 'keyboard/keys/r/down',
+                listener() {
+                    controls.mode = "rotate"
+                }
+            }]))
+            .build(ecd);
+    });

package/src/engine/physics/inverse_kinematics/two_joint_ik.d.ts

@@ -0,0 +1,16 @@
+/**
+ * Based on http://theorangeduck.com/page/simple-two-joint
+ * @param {Vector3} a Root bone position
+ * @param {Vector3} b Second bone position
+ * @param {Vector3} c Effector position
+ * @param {Vector3} t Target position
+ * @param {number} eps EPSILON value, small value for rounding error compensation
+ * @param {Quaternion} a_gr Global rotation of root bone
+ * @param {Quaternion} b_gr Global rotation of second bone
+ * @param {Quaternion} a_lr local rotation for root bone, this will be updated as a result
+ * @param {Quaternion} b_lr local rotation for second bone, this will be updated as a result
+ */
+export function two_joint_ik(a: Vector3, b: Vector3, c: Vector3, t: Vector3, eps: number, a_gr: Quaternion, b_gr: Quaternion, a_lr: Quaternion, b_lr: Quaternion): void;
+import Vector3 from "../../../core/geom/Vector3.js";
+import Quaternion from "../../../core/geom/Quaternion.js";
+//# sourceMappingURL=two_joint_ik.d.ts.map

package/src/engine/physics/inverse_kinematics/two_joint_ik.d.ts.map

@@ -0,0 +1 @@
+{"version":3,"file":"two_joint_ik.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/inverse_kinematics/two_joint_ik.js"],"names":[],"mappings":"AAgDA;;;;;;;;;;;GAWG;AACH,gCAVW,OAAO,KACP,OAAO,KACP,OAAO,KACP,OAAO,OACP,MAAM,QACN,UAAU,QACV,UAAU,QACV,UAAU,QACV,UAAU,QAoEpB;oBA5HmB,+BAA+B;uBAF5B,kCAAkC"}