@woosh/meep-engine 3.40.0 → 3.41.0

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

@@ -0,0 +1,874 @@
+import { assert } from "../../../../core/assert.js";
+import { quat3_multiply_array } from "../../../../core/geom/3d/quaternion/quat3_multiply.js";
+import { quat3_shortest_arc } from "../../../../core/geom/3d/quaternion/quat3_shortest_arc.js";
+import { swing_twist_clamp_array } from "../../../../core/geom/3d/quaternion/swing_twist_clamp.js";
+import { v3_quaternion_apply_array } from "../../../../core/geom/vec3/v3_quaternion_apply.js";
+
+/**
+ * Power-iteration steps a branch joint's fit takes from its carried rotation, per pass.
+ *
+ * The steps are part of the mechanism, not a precision setting. A branch whose children lie nearly
+ * along one line (a chest, its shoulders either side) barely determines its turn about that line,
+ * and a fit started from the carried rotation that takes a few steps leaves that turn where the
+ * carried rotation had it. An exact fit (Jacobi) answers it from rounding: at convergence it bent
+ * the chest joint 40-175 degrees over the probe cases this one bends 0-54.
+ *
+ * @type {number}
+ */
+const FIT_STEPS = 8;
+
+/**
+ * Below this fraction of its bone's length, a direction to fit a joint toward is no direction: the
+ * child sits on the joint, and a turn toward it divides 0/0.
+ *
+ * @type {number}
+ */
+const DEGENERATE_DIRECTION = 1e-12;
+
+/**
+ * The rotation a joint would have if the solve left it alone relative to its parent.
+ * @type {Float64Array}
+ */
+const carried = new Float64Array(4);
+
+/**
+ * `[x, y, z]` scratch.
+ * @type {Float64Array}
+ */
+const v3 = new Float64Array(3);
+
+/**
+ * Turn and position scratch for one fit.
+ * @type {Float64Array}
+ */
+const turn = new Float64Array(4);
+
+/**
+ * The frame above a limited joint, and the joint's turn in it.
+ * @type {Float64Array}
+ */
+const above = new Float64Array(4);
+
+/**
+ * @type {Float64Array}
+ */
+const relative = new Float64Array(4);
+
+/**
+ * Fit the world rotation of joint `i` so its weighted children's offsets point where the children
+ * are, starting from `carried`, and write it to `rotation[i]`.
+ *
+ * - No weighted child: the carried rotation.
+ * - One: the shortest arc from where the carried rotation points the child onto where it is, so the
+ *   joint's turn about the bone stays the carried one. This is the paper's pull.
+ * - More: Horn's quaternion fit of the offsets onto the directions, weighted by link and by both
+ *   lengths, by {@link FIT_STEPS} steps of power iteration from the carried rotation.
+ *
+ * A child sitting on the joint has no direction and contributes nothing; with none left, the carried
+ * rotation stands.
+ *
+ * @param {FrameFabrikProblem} problem
+ * @param {number} i
+ * @returns {boolean} whether the joint was fitted, rather than left at the carried rotation
+ */
+function fit_rotation(problem, i) {
+    const position = problem.solved_position;
+    const rotation = problem.solved_rotation;
+    const offset = problem._offset;
+    const link_weight = problem._link_weight;
+    const child = problem._child;
+
+    const child_end = problem._child_start[i + 1];
+
+    const i3 = i * 3;
+    const i4 = i * 4;
+
+    const px = position[i3], py = position[i3 + 1], pz = position[i3 + 2];
+
+    let weighted = 0;
+    let single = -1;
+
+    for (let c = problem._child_start[i]; c < child_end; c++) {
+        const k = child[c];
+
+        if (link_weight[k] > 0) {
+            weighted++;
+            single = k;
+        }
+    }
+
+    if (weighted === 1) {
+        const k3 = single * 3;
+
+        const ox = offset[k3], oy = offset[k3 + 1], oz = offset[k3 + 2];
+
+        const dx = position[k3] - px, dy = position[k3 + 1] - py, dz = position[k3 + 2] - pz;
+
+        const o_sqr = ox * ox + oy * oy + oz * oz;
+        const d_sqr = dx * dx + dy * dy + dz * dz;
+
+        if (d_sqr > DEGENERATE_DIRECTION * DEGENERATE_DIRECTION * o_sqr) {
+            // where the carried rotation points the child
+            v3_quaternion_apply_array(v3, 0, offset, k3, carried, 0);
+
+            const u_inverse = 1 / Math.sqrt(o_sqr);
+            const d_inverse = 1 / Math.sqrt(d_sqr);
+
+            quat3_shortest_arc(
+                turn, 0,
+                v3[0] * u_inverse, v3[1] * u_inverse, v3[2] * u_inverse,
+                dx * d_inverse, dy * d_inverse, dz * d_inverse
+            );
+
+            quat3_multiply_array(rotation, i4, turn, 0, carried, 0);
+
+            return true;
+        }
+
+        weighted = 0;
+    }
+
+    if (weighted === 0) {
+        rotation[i4] = carried[0];
+        rotation[i4 + 1] = carried[1];
+        rotation[i4 + 2] = carried[2];
+        rotation[i4 + 3] = carried[3];
+
+        return false;
+    }
+
+    // Horn (1987): S = sum w b a^T, with a the direction to the child and b its offset
+    let Sxx = 0, Sxy = 0, Sxz = 0;
+    let Syx = 0, Syy = 0, Syz = 0;
+    let Szx = 0, Szy = 0, Szz = 0;
+
+    // bounds N's spectrum, |lambda| <= s, so N + sI has the fit as its dominant eigenvector
+    let s = 0;
+
+    for (let c = problem._child_start[i]; c < child_end; c++) {
+        const k = child[c];
+
+        const w = link_weight[k];
+
+        if (w <= 0) {
+            continue;
+        }
+
+        const k3 = k * 3;
+
+        const bx = offset[k3], by = offset[k3 + 1], bz = offset[k3 + 2];
+
+        const ax = position[k3] - px, ay = position[k3 + 1] - py, az = position[k3 + 2] - pz;
+
+        Sxx += w * bx * ax;
+        Sxy += w * bx * ay;
+        Sxz += w * bx * az;
+        Syx += w * by * ax;
+        Syy += w * by * ay;
+        Syz += w * by * az;
+        Szx += w * bz * ax;
+        Szy += w * bz * ay;
+        Szz += w * bz * az;
+
+        s += w * Math.sqrt((ax * ax + ay * ay + az * az) * (bx * bx + by * by + bz * bz));
+    }
+
+    // scalar-last (x, y, z, w), as v3_rigid_align_paired_unit_vectors orders it, shifted by s
+    const Nxx = Sxx - Syy - Szz + s;
+    const Nyy = -Sxx + Syy - Szz + s;
+    const Nzz = -Sxx - Syy + Szz + s;
+    const Nww = Sxx + Syy + Szz + s;
+    const Nxy = Sxy + Syx;
+    const Nxz = Szx + Sxz;
+    const Nxw = Syz - Szy;
+    const Nyz = Syz + Szy;
+    const Nyw = Szx - Sxz;
+    const Nzw = Sxy - Syx;
+
+    let x = carried[0], y = carried[1], z = carried[2], w = carried[3];
+
+    for (let step = 0; step < FIT_STEPS; step++) {
+        const rx = Nxx * x + Nxy * y + Nxz * z + Nxw * w;
+        const ry = Nxy * x + Nyy * y + Nyz * z + Nyw * w;
+        const rz = Nxz * x + Nyz * y + Nzz * z + Nzw * w;
+        const rw = Nxw * x + Nyw * y + Nzw * z + Nww * w;
+
+        const length = Math.sqrt(rx * rx + ry * ry + rz * rz + rw * rw);
+
+        if (!(length > 0)) {
+            // every child sits on the joint: nothing to fit
+            break;
+        }
+
+        const inverse = 1 / length;
+
+        x = rx * inverse;
+        y = ry * inverse;
+        z = rz * inverse;
+        w = rw * inverse;
+    }
+
+    rotation[i4] = x;
+    rotation[i4 + 1] = y;
+    rotation[i4 + 2] = z;
+    rotation[i4 + 3] = w;
+
+    return true;
+}
+
+/**
+ * Project joint `i`'s fitted rotation into its limits: its turn relative to its parent, taken
+ * between the limit's frames as a physics `Joint` measures it (`conj(basis_a) * local * basis_b`),
+ * clamped into the box (`swing_twist_clamp_array`). The parent's frame is its current one; a joint
+ * whose parent is not posed is limited against that parent's fixed frame.
+ *
+ * @param {FrameFabrikProblem} problem
+ * @param {number} i
+ */
+function limit_rotation(problem, i) {
+    const rotation = problem.solved_rotation;
+    const basis_a = problem.limit_basis_a;
+    const basis_b = problem.limit_basis_b;
+
+    const i4 = i * 4;
+    const parent = problem.joint_parent[i];
+
+    if (parent < 0) {
+        // the parent's fixed frame: R_i = above * L_i, as animated
+        const local = problem.base_local_rotation;
+
+        turn[0] = -local[i4];
+        turn[1] = -local[i4 + 1];
+        turn[2] = -local[i4 + 2];
+        turn[3] = local[i4 + 3];
+
+        quat3_multiply_array(above, 0, problem.base_rotation, i4, turn, 0);
+    } else {
+        const p4 = parent * 4;
+
+        above[0] = rotation[p4];
+        above[1] = rotation[p4 + 1];
+        above[2] = rotation[p4 + 2];
+        above[3] = rotation[p4 + 3];
+    }
+
+    // relative = conj(basis_a) * conj(above) * R_i * basis_b
+    turn[0] = -above[0];
+    turn[1] = -above[1];
+    turn[2] = -above[2];
+    turn[3] = above[3];
+
+    quat3_multiply_array(relative, 0, turn, 0, rotation, i4);
+    quat3_multiply_array(relative, 0, relative, 0, basis_b, i4);
+
+    turn[0] = -basis_a[i4];
+    turn[1] = -basis_a[i4 + 1];
+    turn[2] = -basis_a[i4 + 2];
+    turn[3] = basis_a[i4 + 3];
+
+    quat3_multiply_array(relative, 0, turn, 0, relative, 0);
+
+    if (!swing_twist_clamp_array(relative, 0, relative, 0, problem.limit_lower, problem.limit_upper, i * 3)) {
+        return;
+    }
+
+    // R_i = above * basis_a * relative * conj(basis_b)
+    quat3_multiply_array(relative, 0, basis_a, i4, relative, 0);
+
+    turn[0] = -basis_b[i4];
+    turn[1] = -basis_b[i4 + 1];
+    turn[2] = -basis_b[i4 + 2];
+    turn[3] = basis_b[i4 + 3];
+
+    quat3_multiply_array(relative, 0, relative, 0, turn, 0);
+
+    quat3_multiply_array(rotation, i4, above, 0, relative, 0);
+}
+
+/**
+ * The rotation joint `i` carries from its parent's current frame: the parent's rotation composed with
+ * the joint's own as animated. A joint whose parent is not posed carries its animated rotation.
+ *
+ * @param {FrameFabrikProblem} problem
+ * @param {number} i
+ */
+function load_carried(problem, i) {
+    const parent = problem.joint_parent[i];
+
+    if (parent < 0) {
+        const r = problem.base_rotation;
+        const i4 = i * 4;
+
+        carried[0] = r[i4];
+        carried[1] = r[i4 + 1];
+        carried[2] = r[i4 + 2];
+        carried[3] = r[i4 + 3];
+    } else {
+        quat3_multiply_array(carried, 0, problem.solved_rotation, parent * 4, problem.base_local_rotation, i * 4);
+    }
+}
+
+/**
+ * What a solve derives once from the tree, the goals and the animated pose: offsets, children, link
+ * weights, which joints only turn, each goal's anchor.
+ *
+ * @param {FrameFabrikProblem} problem
+ */
+function prepare(problem) {
+    const n = problem.joint_count;
+    const goal_count = problem.goal_count;
+
+    const parent = problem.joint_parent;
+    const base_position = problem.base_position;
+    const base_rotation = problem.base_rotation;
+
+    const offset = problem._offset;
+    const child_start = problem._child_start;
+    const child = problem._child;
+    const link_weight = problem._link_weight;
+    const path_count = problem._path_count;
+    const interior = problem._interior;
+    const root_only = problem._root_only;
+    const effector_goal = problem._effector_goal;
+
+    // offsets, and the solve starts from the animated pose
+    for (let i = 0; i < n; i++) {
+        const i3 = i * 3;
+        const i4 = i * 4;
+
+        problem.solved_position[i3] = base_position[i3];
+        problem.solved_position[i3 + 1] = base_position[i3 + 1];
+        problem.solved_position[i3 + 2] = base_position[i3 + 2];
+
+        problem.solved_rotation[i4] = base_rotation[i4];
+        problem.solved_rotation[i4 + 1] = base_rotation[i4 + 1];
+        problem.solved_rotation[i4 + 2] = base_rotation[i4 + 2];
+        problem.solved_rotation[i4 + 3] = base_rotation[i4 + 3];
+
+        child_start[i] = 0;
+        link_weight[i] = 0;
+        path_count[i] = 0;
+        interior[i] = 0;
+        root_only[i] = 0;
+        effector_goal[i] = -1;
+
+        const p = parent[i];
+
+        if (p < 0) {
+            continue;
+        }
+
+        const p3 = p * 3;
+        const p4 = p * 4;
+
+        // conj(parent) applied to the world offset
+        v3[0] = base_position[i3] - base_position[p3];
+        v3[1] = base_position[i3 + 1] - base_position[p3 + 1];
+        v3[2] = base_position[i3 + 2] - base_position[p3 + 2];
+
+        turn[0] = -base_rotation[p4];
+        turn[1] = -base_rotation[p4 + 1];
+        turn[2] = -base_rotation[p4 + 2];
+        turn[3] = base_rotation[p4 + 3];
+
+        v3_quaternion_apply_array(offset, i3, v3, 0, turn, 0);
+    }
+
+    // children, compressed: counted one slot up, summed into starts, filled with the start as a
+    // cursor (which leaves each start where the next one began), then shifted back down
+    child_start[n] = 0;
+
+    for (let i = 0; i < n; i++) {
+        const p = parent[i];
+
+        if (p >= 0) {
+            child_start[p + 1]++;
+        }
+    }
+
+    for (let i = 1; i <= n; i++) {
+        child_start[i] += child_start[i - 1];
+    }
+
+    for (let i = 0; i < n; i++) {
+        const p = parent[i];
+
+        if (p >= 0) {
+            child[child_start[p]++] = i;
+        }
+    }
+
+    for (let i = n; i > 0; i--) {
+        child_start[i] = child_start[i - 1];
+    }
+
+    child_start[0] = 0;
+
+    // paths: link weights, how many pulling goals hold each joint, which joints are interior
+    for (let g = 0; g < goal_count; g++) {
+        const effector = problem.goal_effector[g];
+        const root = problem.goal_root[g];
+        const weight = problem.goal_weight[g];
+
+        effector_goal[effector] = g;
+
+        for (let j = effector; ; j = parent[j]) {
+            if (weight > 0) {
+                path_count[j]++;
+            }
+
+            if (j === root) {
+                break;
+            }
+
+            interior[j] = 1;
+
+            if (weight > link_weight[j]) {
+                link_weight[j] = weight;
+            }
+        }
+    }
+
+    for (let g = 0; g < goal_count; g++) {
+        const root = problem.goal_root[g];
+
+        if (interior[root] === 0) {
+            root_only[root] = 1;
+        }
+    }
+
+    for (let i = 0; i < n; i++) {
+        if (parent[i] < 0) {
+            // its parent's frame is fixed, so nothing the solve does moves it
+            root_only[i] = 1;
+        }
+    }
+
+    // anchors, and each target in its joint's frame
+    for (let g = 0; g < goal_count; g++) {
+        const effector = problem.goal_effector[g];
+        const root = problem.goal_root[g];
+        const own = problem.goal_weight[g] > 0 ? 1 : 0;
+
+        let anchor = parent[root];
+
+        for (let j = effector; ; j = parent[j]) {
+            if (path_count[j] - own > 0) {
+                anchor = j;
+                break;
+            }
+
+            if (j === root) {
+                break;
+            }
+        }
+
+        problem._goal_anchor[g] = anchor;
+
+        if (anchor >= 0) {
+            to_frame(problem._goal_anchor_offset, g * 3, base_position, effector * 3, base_position, base_rotation, anchor);
+        }
+
+        const target_joint = problem.goal_target_joint[g];
+
+        if (target_joint >= 0) {
+            to_frame(problem._goal_target_local, g * 3, problem.goal_target, g * 3, base_position, base_rotation, target_joint);
+        }
+    }
+}
+
+/**
+ * `out = conj(R_j) (point - P_j)` with the frame of joint `j` as animated.
+ *
+ * @param {Float64Array} out
+ * @param {number} out_offset
+ * @param {Float64Array} point
+ * @param {number} point_offset
+ * @param {Float64Array} base_position
+ * @param {Float64Array} base_rotation
+ * @param {number} j
+ */
+function to_frame(out, out_offset, point, point_offset, base_position, base_rotation, j) {
+    const j3 = j * 3;
+    const j4 = j * 4;
+
+    v3[0] = point[point_offset] - base_position[j3];
+    v3[1] = point[point_offset + 1] - base_position[j3 + 1];
+    v3[2] = point[point_offset + 2] - base_position[j3 + 2];
+
+    turn[0] = -base_rotation[j4];
+    turn[1] = -base_rotation[j4 + 1];
+    turn[2] = -base_rotation[j4 + 2];
+    turn[3] = base_rotation[j4 + 3];
+
+    v3_quaternion_apply_array(out, out_offset, v3, 0, turn, 0);
+}
+
+/**
+ * `out = P_j + R_j local` with the frame of joint `j` as solved so far.
+ *
+ * @param {Float64Array} out
+ * @param {number} out_offset
+ * @param {FrameFabrikProblem} problem
+ * @param {Float64Array} local
+ * @param {number} local_offset
+ * @param {number} j
+ */
+function from_frame(out, out_offset, problem, local, local_offset, j) {
+    const j3 = j * 3;
+
+    v3_quaternion_apply_array(v3, 0, local, local_offset, problem.solved_rotation, j * 4);
+
+    out[out_offset] = problem.solved_position[j3] + v3[0];
+    out[out_offset + 1] = problem.solved_position[j3 + 1] + v3[1];
+    out[out_offset + 2] = problem.solved_position[j3 + 2] + v3[2];
+}
+
+/**
+ * This iteration's target for every effector: `lerp(carried, goal, w)`.
+ *
+ * Carried is where the effector would be if the goal did not pull, in the frame of its anchor (see
+ * {@link FrameFabrikProblem#_goal_anchor}), so a goal of weight 0 asks for exactly where the other
+ * goals take it, and raising its weight moves it continuously from there.
+ *
+ * @param {FrameFabrikProblem} problem
+ */
+function load_targets(problem) {
+    const goal_count = problem.goal_count;
+    const target = problem._target;
+
+    for (let g = 0; g < goal_count; g++) {
+        const g3 = g * 3;
+
+        const anchor = problem._goal_anchor[g];
+
+        let cx, cy, cz;
+
+        if (anchor < 0) {
+            const e3 = problem.goal_effector[g] * 3;
+
+            cx = problem.base_position[e3];
+            cy = problem.base_position[e3 + 1];
+            cz = problem.base_position[e3 + 2];
+        } else {
+            from_frame(target, g3, problem, problem._goal_anchor_offset, g3, anchor);
+
+            cx = target[g3];
+            cy = target[g3 + 1];
+            cz = target[g3 + 2];
+        }
+
+        const target_joint = problem.goal_target_joint[g];
+
+        let gx, gy, gz;
+
+        if (target_joint < 0) {
+            gx = problem.goal_target[g3];
+            gy = problem.goal_target[g3 + 1];
+            gz = problem.goal_target[g3 + 2];
+        } else {
+            from_frame(target, g3, problem, problem._goal_target_local, g3, target_joint);
+
+            gx = target[g3];
+            gy = target[g3 + 1];
+            gz = target[g3 + 2];
+        }
+
+        const w = problem.goal_weight[g];
+
+        target[g3] = cx + (gx - cx) * w;
+        target[g3 + 1] = cy + (gy - cy) * w;
+        target[g3 + 2] = cz + (gz - cz) * w;
+    }
+}
+
+/**
+ * Inward: from the leaves to the roots, each effector onto its target, and each joint with pulling
+ * children turned toward them from where it was, then moved to the weighted mean of where each child
+ * puts it rigidly. An effector with pulling children of its own, inside another goal's path, takes
+ * its target into that mean as one more proposal, weighted by its goal's weight. A joint that only
+ * turns is left for the outward pass.
+ *
+ * @param {FrameFabrikProblem} problem
+ */
+function pass_inward(problem) {
+    const n = problem.joint_count;
+
+    const position = problem.solved_position;
+    const rotation = problem.solved_rotation;
+    const offset = problem._offset;
+    const link_weight = problem._link_weight;
+    const child = problem._child;
+    const child_start = problem._child_start;
+    const root_only = problem._root_only;
+    const effector_goal = problem._effector_goal;
+    const target = problem._target;
+
+    for (let i = n - 1; i >= 0; i--) {
+        if (root_only[i] !== 0) {
+            continue;
+        }
+
+        const i3 = i * 3;
+
+        const goal = effector_goal[i];
+
+        const child_end = child_start[i + 1];
+
+        let pulled = false;
+
+        for (let c = child_start[i]; c < child_end; c++) {
+            if (link_weight[child[c]] > 0) {
+                pulled = true;
+                break;
+            }
+        }
+
+        if (!pulled) {
+            if (goal >= 0) {
+                // an effector at the end of its paths goes onto its target
+                const g3 = goal * 3;
+
+                position[i3] = target[g3];
+                position[i3 + 1] = target[g3 + 1];
+                position[i3 + 2] = target[g3 + 2];
+            }
+
+            continue;
+        }
+
+        load_carried(problem, i);
+
+        if (fit_rotation(problem, i) && problem.joint_limited[i] !== 0) {
+            limit_rotation(problem, i);
+        }
+
+        let x = 0, y = 0, z = 0, total = 0;
+
+        for (let c = child_start[i]; c < child_end; c++) {
+            const k = child[c];
+
+            const w = link_weight[k];
+
+            if (w <= 0) {
+                continue;
+            }
+
+            const k3 = k * 3;
+
+            v3_quaternion_apply_array(v3, 0, offset, k3, rotation, i * 4);
+
+            x += w * (position[k3] - v3[0]);
+            y += w * (position[k3 + 1] - v3[1]);
+            z += w * (position[k3 + 2] - v3[2]);
+
+            total += w;
+        }
+
+        if (goal >= 0) {
+            // an effector inside another goal's path: its target is one more proposal, as strong as
+            // its goal, so weight 0 leaves the joint to its children and the pose is continuous
+            const w = problem.goal_weight[goal];
+            const g3 = goal * 3;
+
+            x += w * target[g3];
+            y += w * target[g3 + 1];
+            z += w * target[g3 + 2];
+
+            total += w;
+        }
+
+        const inverse = 1 / total;
+
+        position[i3] = x * inverse;
+        position[i3 + 1] = y * inverse;
+        position[i3 + 2] = z * inverse;
+    }
+}
+
+/**
+ * Outward: from the roots to the leaves, each joint turned toward where its pulling children went on
+ * the way in, from where its parent has now put it, and every child placed rigidly.
+ *
+ * @param {FrameFabrikProblem} problem
+ */
+function pass_outward(problem) {
+    const n = problem.joint_count;
+
+    const position = problem.solved_position;
+    const rotation = problem.solved_rotation;
+    const offset = problem._offset;
+    const child = problem._child;
+    const child_start = problem._child_start;
+
+    for (let i = 0; i < n; i++) {
+        load_carried(problem, i);
+
+        if (fit_rotation(problem, i) && problem.joint_limited[i] !== 0) {
+            limit_rotation(problem, i);
+        }
+
+        const i3 = i * 3;
+        const child_end = child_start[i + 1];
+
+        for (let c = child_start[i]; c < child_end; c++) {
+            const k3 = child[c] * 3;
+
+            v3_quaternion_apply_array(v3, 0, offset, k3, rotation, i * 4);
+
+            position[k3] = position[i3] + v3[0];
+            position[k3 + 1] = position[i3 + 1] + v3[1];
+            position[k3 + 2] = position[i3 + 2] + v3[2];
+        }
+    }
+}
+
+/**
+ * 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, max_iterations, tolerance) {
+    assert.defined(problem, 'problem');
+    assert.isNonNegativeInteger(max_iterations, 'max_iterations');
+    assert.isNumber(tolerance, 'tolerance');
+    assert.greaterThanOrEqual(tolerance, 0, 'tolerance');
+
+    const n = problem.joint_count;
+    const goal_count = problem.goal_count;
+
+    assert.isNonNegativeInteger(n, 'joint_count');
+    assert.lessThanOrEqual(n, problem.joint_capacity, 'joint_count');
+    assert.isNonNegativeInteger(goal_count, 'goal_count');
+    assert.lessThanOrEqual(goal_count, problem.goal_capacity, 'goal_count');
+
+    validate(problem);
+
+    prepare(problem);
+
+    const position = problem.solved_position;
+    const previous = problem._previous;
+
+    let iteration = 0;
+
+    while (iteration < max_iterations) {
+        for (let i = 0; i < n * 3; i++) {
+            previous[i] = position[i];
+        }
+
+        load_targets(problem);
+
+        pass_inward(problem);
+
+        pass_outward(problem);
+
+        iteration++;
+
+        let moved = 0;
+
+        for (let i = 0; i < n; i++) {
+            const i3 = i * 3;
+
+            const dx = position[i3] - previous[i3];
+            const dy = position[i3 + 1] - previous[i3 + 1];
+            const dz = position[i3 + 2] - previous[i3 + 2];
+
+            const d = dx * dx + dy * dy + dz * dz;
+
+            if (d > moved) {
+                moved = d;
+            }
+        }
+
+        if (moved < tolerance * tolerance) {
+            break;
+        }
+    }
+
+    return iteration;
+}
+
+/**
+ * The problem's shape, asserted: parents first, one goal per effector, every root on its effector's
+ * way up, weights in 0..1.
+ *
+ * @param {FrameFabrikProblem} problem
+ */
+function validate(problem) {
+    const n = problem.joint_count;
+    const parent = problem.joint_parent;
+
+    for (let i = 0; i < n; i++) {
+        assert.ok(parent[i] >= -1 && parent[i] < i, 'every parent comes before its children');
+
+        if (problem.joint_limited[i] !== 0) {
+            for (let k = 0; k < 3; k++) {
+                assert.ok(problem.limit_lower[i * 3 + k] <= problem.limit_upper[i * 3 + k], 'limit_lower is no greater than limit_upper');
+            }
+        }
+
+        const p = parent[i];
+
+        if (p < 0) {
+            continue;
+        }
+
+        // base_rotation[i] = base_rotation[p] * base_local_rotation[i]
+        quat3_multiply_array(above, 0, problem.base_rotation, p * 4, problem.base_local_rotation, i * 4);
+
+        const i4 = i * 4;
+        const dot = above[0] * problem.base_rotation[i4] + above[1] * problem.base_rotation[i4 + 1]
+            + above[2] * problem.base_rotation[i4 + 2] + above[3] * problem.base_rotation[i4 + 3];
+
+        assert.ok(Math.abs(dot) > 1 - 1e-9, 'base_local_rotation composes with the parent\'s base_rotation to the joint\'s');
+    }
+
+    for (let g = 0; g < problem.goal_count; g++) {
+        const effector = problem.goal_effector[g];
+        const root = problem.goal_root[g];
+        const weight = problem.goal_weight[g];
+        const target_joint = problem.goal_target_joint[g];
+
+        assert.ok(effector >= 0 && effector < n, 'goal_effector is a joint of the problem');
+        assert.ok(weight >= 0 && weight <= 1, 'goal_weight is within 0..1');
+        assert.ok(target_joint >= -1 && target_joint < n, 'goal_target_joint is a joint of the problem, or -1');
+
+        let j = effector;
+
+        while (j !== root && j >= 0) {
+            j = parent[j];
+        }
+
+        assert.equal(j, root, 'goal_root is the effector or one of its ancestors');
+
+        for (let h = 0; h < g; h++) {
+            assert.notEqual(problem.goal_effector[h], effector, 'one goal per effector');
+        }
+    }
+}