@woosh/meep-engine 3.40.0 → 3.41.0

package/src/engine/physics/inverse_kinematics/frame_fabrik/FrameFabrikProblem.js

@@ -0,0 +1,245 @@
+import { assert } from "../../../../core/assert.js";
+
+/**
+ * One tree for {@link frame_fabrik_solve} to pose: its joints, as the animation left them, the goals
+ * pulling on them, and, once solved, where they went.
+ *
+ * Every array is sized once, at construction, for the most joints and goals the problem will ever
+ * hold; a solve allocates nothing. Joints are indices into the problem, not entities: the problem
+ * knows nothing about where its tree came from.
+ *
+ * The caller fills the inputs and {@link joint_count} / {@link goal_count}; the fields under "solver
+ * state" belong to the solver, which overwrites them on every solve.
+ *
+ * @author Alex Goldring
+ * @copyright Company Named Limited (c) 2026
+ */
+export class FrameFabrikProblem {
+
+    /**
+     * @param {number} joint_capacity the most joints a solve will be given
+     * @param {number} goal_capacity the most goals a solve will be given
+     */
+    constructor(joint_capacity, goal_capacity) {
+        assert.isPositiveInteger(joint_capacity, 'joint_capacity');
+        assert.isPositiveInteger(goal_capacity, 'goal_capacity');
+
+        /**
+         * @readonly
+         * @type {number}
+         */
+        this.joint_capacity = joint_capacity;
+
+        /**
+         * @readonly
+         * @type {number}
+         */
+        this.goal_capacity = goal_capacity;
+
+        /**
+         * How many joints this solve poses.
+         * @type {number}
+         */
+        this.joint_count = 0;
+
+        /**
+         * How many goals pull on them.
+         * @type {number}
+         */
+        this.goal_count = 0;
+
+        /**
+         * Each joint's parent, as an index into this problem, or -1 for a joint whose parent the solve
+         * does not pose: its parent's frame is fixed, as animated. Every parent comes before its
+         * children, `joint_parent[i] < i`.
+         * @type {Int32Array}
+         */
+        this.joint_parent = new Int32Array(joint_capacity);
+
+        /**
+         * Where each joint is as animated, in world. Packed `[x, y, z]` per joint.
+         * @type {Float64Array}
+         */
+        this.base_position = new Float64Array(joint_capacity * 3);
+
+        /**
+         * How each joint is turned as animated, in world, a unit quaternion. Packed `[x, y, z, w]` per
+         * joint.
+         * @type {Float64Array}
+         */
+        this.base_rotation = new Float64Array(joint_capacity * 4);
+
+        /**
+         * How each joint is turned relative to its parent as animated, a unit quaternion: the parent's
+         * {@link base_rotation} composed with it is the joint's. For a joint whose parent is not posed,
+         * relative to that parent, whose frame it gives the solve. Packed `[x, y, z, w]` per joint.
+         * @type {Float64Array}
+         */
+        this.base_local_rotation = new Float64Array(joint_capacity * 4);
+
+        /**
+         * 1 for a joint whose turn relative to its parent is limited, by {@link limit_basis_a},
+         * {@link limit_basis_b}, {@link limit_lower} and {@link limit_upper}; 0 for one that turns
+         * freely.
+         * @type {Uint8Array}
+         */
+        this.joint_limited = new Uint8Array(joint_capacity);
+
+        /**
+         * The limit's frame in the parent's, a unit quaternion: a physics `Joint`'s `localBasisA`.
+         * Packed `[x, y, z, w]` per joint.
+         * @type {Float64Array}
+         */
+        this.limit_basis_a = new Float64Array(joint_capacity * 4);
+
+        /**
+         * The limit's frame in the joint's own, a unit quaternion: a physics `Joint`'s `localBasisB`.
+         * Packed `[x, y, z, w]` per joint.
+         * @type {Float64Array}
+         */
+        this.limit_basis_b = new Float64Array(joint_capacity * 4);
+
+        /**
+         * Lower bounds of the turn of frame B relative to frame A, as `swing_twist_error_array`
+         * measures it: `[twist about X, swing Y, swing Z]` per joint, radians. `-Infinity` for an
+         * axis that turns freely, 0 for a locked one.
+         * @type {Float64Array}
+         */
+        this.limit_lower = new Float64Array(joint_capacity * 3);
+
+        /**
+         * Upper bounds, as {@link limit_lower}.
+         * @type {Float64Array}
+         */
+        this.limit_upper = new Float64Array(joint_capacity * 3);
+
+        /**
+         * The joint each goal pulls on.
+         * @type {Int32Array}
+         */
+        this.goal_effector = new Int32Array(goal_capacity);
+
+        /**
+         * The highest joint each goal turns: its effector or an ancestor of it. The goal's path runs
+         * from the effector up to here. This joint turns and never moves.
+         * @type {Int32Array}
+         */
+        this.goal_root = new Int32Array(goal_capacity);
+
+        /**
+         * How hard each goal pulls, 0..1. 0 gives the pose the solve gives without the goal, and the
+         * pose is continuous in it.
+         * @type {Float64Array}
+         */
+        this.goal_weight = new Float64Array(goal_capacity);
+
+        /**
+         * The joint each goal's target hangs below, or -1 for a target the solve does not move. A
+         * target hanging below a joint is carried by that joint's frame as the solve turns it, read
+         * afresh on every iteration.
+         * @type {Int32Array}
+         */
+        this.goal_target_joint = new Int32Array(goal_capacity);
+
+        /**
+         * Where each goal wants its effector, in world, as animated: the frame of
+         * {@link goal_target_joint} carries it from there. Packed `[x, y, z]` per goal.
+         * @type {Float64Array}
+         */
+        this.goal_target = new Float64Array(goal_capacity * 3);
+
+        /**
+         * The solved pose: where each joint went, in world. Packed `[x, y, z]` per joint.
+         * @type {Float64Array}
+         */
+        this.solved_position = new Float64Array(joint_capacity * 3);
+
+        /**
+         * The solved pose: how each joint is turned, in world. Packed `[x, y, z, w]` per joint.
+         * @type {Float64Array}
+         */
+        this.solved_rotation = new Float64Array(joint_capacity * 4);
+
+        // --- solver state ---
+
+        /**
+         * Each joint's place in its parent's frame, as animated: the rigid offset the solve keeps.
+         * @type {Float64Array}
+         */
+        this._offset = new Float64Array(joint_capacity * 3);
+
+        /**
+         * The weight of the link from each joint's parent to it: the heaviest goal whose path holds
+         * both ends. 0 for a link no path holds.
+         * @type {Float64Array}
+         */
+        this._link_weight = new Float64Array(joint_capacity);
+
+        /**
+         * Children of joint `i` are `_child[_child_start[i] .. _child_start[i + 1]]`.
+         * @type {Int32Array}
+         */
+        this._child_start = new Int32Array(joint_capacity + 1);
+
+        /**
+         * @type {Int32Array}
+         */
+        this._child = new Int32Array(joint_capacity);
+
+        /**
+         * How many goals of non-zero weight have the joint on their path.
+         * @type {Int32Array}
+         */
+        this._path_count = new Int32Array(joint_capacity);
+
+        /**
+         * 1 for a joint some goal's path holds below that goal's root.
+         * @type {Uint8Array}
+         */
+        this._interior = new Uint8Array(joint_capacity);
+
+        /**
+         * 1 for a joint that turns and never moves: a goal's root no path holds below its own root.
+         * @type {Uint8Array}
+         */
+        this._root_only = new Uint8Array(joint_capacity);
+
+        /**
+         * The goal whose effector the joint is, or -1.
+         * @type {Int32Array}
+         */
+        this._effector_goal = new Int32Array(joint_capacity);
+
+        /**
+         * The joint whose frame carries each goal's effector when the goal does not pull: the deepest
+         * joint of its path another goal's path holds too, else its root's parent (-1 when that is
+         * not posed).
+         * @type {Int32Array}
+         */
+        this._goal_anchor = new Int32Array(goal_capacity);
+
+        /**
+         * The effector in its anchor's frame, as animated. Packed `[x, y, z]` per goal.
+         * @type {Float64Array}
+         */
+        this._goal_anchor_offset = new Float64Array(goal_capacity * 3);
+
+        /**
+         * Each target in its target joint's frame, as animated. Packed `[x, y, z]` per goal.
+         * @type {Float64Array}
+         */
+        this._goal_target_local = new Float64Array(goal_capacity * 3);
+
+        /**
+         * This iteration's target for each effector. Packed `[x, y, z]` per goal.
+         * @type {Float64Array}
+         */
+        this._target = new Float64Array(goal_capacity * 3);
+
+        /**
+         * Where each joint was before this iteration, to measure how far the iteration moved it.
+         * @type {Float64Array}
+         */
+        this._previous = new Float64Array(joint_capacity * 3);
+    }
+}

package/src/engine/physics/inverse_kinematics/frame_fabrik/frame_fabrik_limit_from_joint.d.ts

@@ -0,0 +1,21 @@
+/**
+ * Limit joint `i` of a {@link FrameFabrikProblem} by the physics {@link Joint} on its bone:
+ * `entityA` the parent bone, `entityB` the bone itself.
+ *
+ * The limits are the Joint's angular ones, in its frames (`localBasisA` in the parent's,
+ * `localBasisB` in the bone's), as the joint solver measures them with `swingTwist` on: the twist
+ * about frame X and the swing over Y and Z, each its own range, a box. A LOCKED axis is held at 0,
+ * a LIMITED one kept in `[lower, upper]`, and a FREE one left alone; so is a SPRING or a MOTOR,
+ * which drive and do not limit. Anchors and the linear axes are ignored: IK turns bones and moves
+ * none.
+ *
+ * `swingTwist` has to be on. Without it physics measures an angle as `2 sin(θ/2)`, so a limit
+ * above 114.6 degrees never engages there, and physics and IK would disagree on what the joint
+ * allows.
+ *
+ * @param {FrameFabrikProblem} problem
+ * @param {number} i
+ * @param {Joint} joint
+ */
+export function frame_fabrik_limit_from_joint(problem: FrameFabrikProblem, i: number, joint: Joint): void;
+//# sourceMappingURL=frame_fabrik_limit_from_joint.d.ts.map

package/src/engine/physics/inverse_kinematics/frame_fabrik/frame_fabrik_limit_from_joint.d.ts.map

@@ -0,0 +1 @@
+{"version":3,"file":"frame_fabrik_limit_from_joint.d.ts","sourceRoot":"","sources":["../../../../../../src/engine/physics/inverse_kinematics/frame_fabrik/frame_fabrik_limit_from_joint.js"],"names":[],"mappings":"AAGA;;;;;;;;;;;;;;;;;;GAkBG;AACH,8EAHW,MAAM,sBA2ChB"}

package/src/engine/physics/inverse_kinematics/frame_fabrik/frame_fabrik_limit_from_joint.js

@@ -0,0 +1,63 @@
+import { assert } from "../../../../core/assert.js";
+import { DofMode } from "../../constraint/DofMode.js";
+
+/**
+ * Limit joint `i` of a {@link FrameFabrikProblem} by the physics {@link Joint} on its bone:
+ * `entityA` the parent bone, `entityB` the bone itself.
+ *
+ * The limits are the Joint's angular ones, in its frames (`localBasisA` in the parent's,
+ * `localBasisB` in the bone's), as the joint solver measures them with `swingTwist` on: the twist
+ * about frame X and the swing over Y and Z, each its own range, a box. A LOCKED axis is held at 0,
+ * a LIMITED one kept in `[lower, upper]`, and a FREE one left alone; so is a SPRING or a MOTOR,
+ * which drive and do not limit. Anchors and the linear axes are ignored: IK turns bones and moves
+ * none.
+ *
+ * `swingTwist` has to be on. Without it physics measures an angle as `2 sin(θ/2)`, so a limit
+ * above 114.6 degrees never engages there, and physics and IK would disagree on what the joint
+ * allows.
+ *
+ * @param {FrameFabrikProblem} problem
+ * @param {number} i
+ * @param {Joint} joint
+ */
+export function frame_fabrik_limit_from_joint(problem, i, joint) {
+    assert.ok(i >= 0 && i < problem.joint_capacity, 'i is a joint of the problem');
+    assert.ok(joint.swingTwist === true, 'the Joint measures its angles by swing and twist (swingTwist), as IK clamps them');
+
+    const mode = joint.dofMode;
+
+    for (let k = 0; k < 3; k++) {
+        const m = mode[3 + k];
+
+        let lower, upper;
+
+        if (m === DofMode.LOCKED) {
+            lower = 0;
+            upper = 0;
+        } else if (m === DofMode.LIMITED) {
+            lower = joint.dofLowerLimit[3 + k];
+            upper = joint.dofUpperLimit[3 + k];
+        } else {
+            lower = -Infinity;
+            upper = Infinity;
+        }
+
+        problem.limit_lower[i * 3 + k] = lower;
+        problem.limit_upper[i * 3 + k] = upper;
+    }
+
+    const a = joint.localBasisA;
+    const b = joint.localBasisB;
+
+    problem.limit_basis_a[i * 4] = a[0];
+    problem.limit_basis_a[i * 4 + 1] = a[1];
+    problem.limit_basis_a[i * 4 + 2] = a[2];
+    problem.limit_basis_a[i * 4 + 3] = a[3];
+
+    problem.limit_basis_b[i * 4] = b[0];
+    problem.limit_basis_b[i * 4 + 1] = b[1];
+    problem.limit_basis_b[i * 4 + 2] = b[2];
+    problem.limit_basis_b[i * 4 + 3] = b[3];
+
+    problem.joint_limited[i] = 1;
+}

package/src/engine/physics/inverse_kinematics/frame_fabrik/frame_fabrik_solve.d.ts

@@ -0,0 +1,33 @@
+/**
+ * Pose a tree of joints so its effectors reach their goals: frame FABRIK.
+ *
+ * Every posed joint is a frame, a position and a rotation, with its children at rigid offsets in it,
+ * so a branch keeps every child's offset and the solved pose is one the hierarchy composes exactly.
+ * Each iteration runs:
+ * - **inward**, leaves to roots: each effector onto its target; every other joint with a pulling child
+ *   fitted toward its pulling children from its old position, then moved to the weighted mean of
+ *   the positions its children put it at rigidly;
+ * - **outward**, roots to leaves: each joint refitted from where its parent now puts it, and its
+ *   children placed rigidly.
+ *
+ * A limited joint's fitted turn is projected into its limits in both passes, so the solved pose keeps
+ * them (Aristidou, Chrysanthou and Lasenby 2016); a joint the solve only carries keeps its animated
+ * turn, inside its limits or not, as it would with no goal.
+ *
+ * With one pulling child that is the paper's pull (Aristidou and Lasenby 2011). The fits start from
+ * the rotation the joint carries from its parent, never from its own last answer, so nothing drifts
+ * with the iteration count. A goal's weight scales the links of its path, and its effector's target
+ * is `lerp(carried, goal, weight)`: weight 0 gives the pose the solve gives without the goal, and the
+ * pose is continuous in the weight. A goal's root turns and never moves.
+ *
+ * The solve stops when no joint moved by `tolerance` in an iteration, or after `max_iterations`. Not
+ * on the distance to the targets: an unreachable target never comes within any distance, and a
+ * reachable one comes close while the joints above it are still settling.
+ *
+ * @param {FrameFabrikProblem} problem filled; the solved pose lands in its `solved_position` and `solved_rotation`
+ * @param {number} max_iterations
+ * @param {number} tolerance in world units; the solve stops once an iteration moves no joint this far
+ * @returns {number} how many iterations ran
+ */
+export function frame_fabrik_solve(problem: FrameFabrikProblem, max_iterations: number, tolerance: number): number;
+//# sourceMappingURL=frame_fabrik_solve.d.ts.map

package/src/engine/physics/inverse_kinematics/frame_fabrik/frame_fabrik_solve.d.ts.map

@@ -0,0 +1 @@
+{"version":3,"file":"frame_fabrik_solve.d.ts","sourceRoot":"","sources":["../../../../../../src/engine/physics/inverse_kinematics/frame_fabrik/frame_fabrik_solve.js"],"names":[],"mappings":"AAqtBA;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;GA8BG;AACH,gFAJW,MAAM,aACN,MAAM,GACJ,MAAM,CA4DlB"}