@woosh/meep-engine 3.22.0 → 3.23.0

package/src/engine/physics/cloth/ecs/cloth_write_back.js

@@ -1,449 +1,212 @@
-import { quat3_multiply } from "../../../../core/geom/3d/quaternion/quat3_multiply.js";
-import { quat3_nlerp } from "../../../../core/geom/3d/quaternion/quat3_nlerp.js";
-import { quat3_shortest_arc } from "../../../../core/geom/3d/quaternion/quat3_shortest_arc.js";
-import { v3_quaternion_apply } from "../../../../core/geom/vec3/v3_quaternion_apply.js";
-import {
-    v3_quaternion_apply_inverse
-} from "../../../../core/geom/vec3/v3_quaternion_apply_inverse.js";
-import { t64_announce_change } from "../../../ecs/transform/t64_announce_change.js";
-import {
-    TRANSFORM64_ROTATION_OFFSET,
-    TRANSFORM64_TRANSLATION_OFFSET
-} from "../../../ecs/transform/Transform64.js";
-
-/**
- * # Putting the answer back — §11.1
- *
- * Position directly, rotation derived from the direction to the child,
- * interpolated between the parent-driven and the child-driven orientation —
- * which is what keeps a two-link chain from looking hinged.
- *
- * ## Rotation is a swing on top of the animated orientation
- *
- * A particle has a position and no orientation, so an entity's rotation has to
- * be reconstructed. The reconstruction here is the minimal rotation carrying
- * the link's **animated** direction onto its **simulated** one, composed onto
- * the animated orientation:
- *
- *     q_node = swing(d_animated → d_simulated) ⊗ q_animated
- *
- * Twist about the link is therefore inherited from the animation rather than
- * invented, which is right: a chain of particles carries no twist degree of
- * freedom and any twist this code produced would be a fabrication. And the
- * whole thing degenerates correctly — at `blend = 0` the simulated direction
- * *is* the animated one, the swing is the identity, and the write-back
- * reproduces the authored pose exactly.
- *
- * A node with a parent and children has two links and two swings, and takes the
- * half-way rotation between them. A leaf has only its parent's. That is
- * §11.1's "interpolated between parent-driven and child-driven orientation",
- * and without it every link points at its child and a two-link chain reads as a
- * hinge with a corner in it.
- *
- * ## Both transforms are written, and that is what makes ordering harmless
- *
- * Each simulated node gets its **world** `Transform64` and its **local**
- * `TransformAttachment#transform`, and the two agree. Writing only the world
- * one is cheaper and is a trap: the attachment system recomposes a child from
- * its local whenever the parent announces, so any announcement arriving after
- * cloth had written would replace the simulated pose with the authored one for
- * the rest of the frame — and where in the fixed step cloth ran would be a
- * correctness question. With the local written too, **a recomposition
- * reproduces exactly what cloth just wrote**, and cloth's placement costs at
- * worst a shape one fixed step stale.
- *
- * That is also the shape of the engine's own precedent:
- * `InverseKinematicsSystem` reads world transforms the hierarchy has composed
- * and writes local ones back, and §11.2's rig path writes
- * `Node3D.transform_local` for the same reason — a local transform is what a
- * joint angle *is*.
- *
- * A node's local transform is its pose relative to its **parent's**, so this
- * needs the parent's reconstructed rotation as well as its position — which is
- * what {@link ClothInstance#blended_rotation} is for, and why the pass runs in
- * the walk's breadth-first order.
- *
- * The authored pose is not lost by being overwritten: it was captured at seed
- * and lives in the cloth (`cloth_seed_subtree.js`), which is what `blend = 0`
- * returns to and what the leash centres are measured from.
- *
- * ## What is announced
- *
- * Nothing on a simulated node: every one of its simulated children has already
- * been written by this same pass, so the cascade would recompute a pose that is
- * already correct, once per level, quadratically for a chain. What *is*
- * announced is each pruned boundary — the root of a `ClothExclude` subtree, or
- * a nested `Cloth`'s anchor — which this pass poses by hand against the link it
- * hangs from, so everything below the cut follows the simulation through the
- * ordinary attachment cascade and nothing above it is disturbed.
- *
- * @author Alex Goldring
- * @copyright Company Named Limited (c) 2026
- */
-
-/**
- * Scratch for one node's swing quaternions. Module-level: the write-back
- * allocates nothing.
- * @type {Float64Array}
- * @private
- */
-const SWING = new Float64Array(12);
-
-/** Where the parent-driven swing sits in {@link SWING}. @private */
-const SWING_PARENT = 0;
-/** Where the child-driven swing sits in {@link SWING}. @private */
-const SWING_CHILD = 4;
-/** Where the blended swing, and then the local rotation, sit. @private */
-const SWING_OUT = 8;
-
-/**
- * @type {Float64Array}
- * @private
- */
-const DIRECTION = new Float64Array(12);
-
-/**
- * Write one cloth's solved pose onto its subtree.
- *
- * @param {ClothInstance} instance
- * @param {EntityComponentDataset} ecd
- * @returns {void}
- */
-export function cloth_write_back(instance, ecd) {
-    const node_count = instance.node_count;
-
-    const state = instance.state;
-    const particle_of_node = instance.particle_of_node;
-
-    const blended = instance.blended_position;
-    const animated = instance.animated_position;
-
-    let blend = instance.cloth.blend;
-
-    if (blend < 0) {
-        blend = 0;
-    } else if (blend > 1) {
-        blend = 1;
-    }
-
-    const position = state.position;
-
-    for (let node = 0; node < node_count; node++) {
-        const p = particle_of_node[node] * 3;
-        const n = node * 3;
-
-        blended[n] = animated[n] + blend * (position[p] - animated[n]);
-        blended[n + 1] = animated[n + 1] + blend * (position[p + 1] - animated[n + 1]);
-        blended[n + 2] = animated[n + 2] + blend * (position[p + 2] - animated[n + 2]);
-    }
-
-    const anchor = instance.node_transform[0];
-
-    const ax = anchor[TRANSFORM64_TRANSLATION_OFFSET];
-    const ay = anchor[TRANSFORM64_TRANSLATION_OFFSET + 1];
-    const az = anchor[TRANSFORM64_TRANSLATION_OFFSET + 2];
-
-    const aqx = anchor[TRANSFORM64_ROTATION_OFFSET];
-    const aqy = anchor[TRANSFORM64_ROTATION_OFFSET + 1];
-    const aqz = anchor[TRANSFORM64_ROTATION_OFFSET + 2];
-    const aqw = anchor[TRANSFORM64_ROTATION_OFFSET + 3];
-
-    const parent = instance.node_parent;
-    const rotation = instance.animated_rotation;
-
-    const local_rotation = instance.blended_rotation;
-
-    // The anchor is the origin of the frame it anchors, so its reconstructed
-    // rotation — which its children's local transforms are relative to — is the
-    // identity rather than anything read back off it.
-    local_rotation[0] = 0;
-    local_rotation[1] = 0;
-    local_rotation[2] = 0;
-    local_rotation[3] = 1;
-
-    for (let node = 1; node < node_count; node++) {
-        const transform = instance.node_transform[node];
-
-        const n = node * 3;
-
-        v3_quaternion_apply(
-            transform, TRANSFORM64_TRANSLATION_OFFSET,
-            blended[n], blended[n + 1], blended[n + 2],
-            aqx, aqy, aqz, aqw
-        );
-
-        transform[TRANSFORM64_TRANSLATION_OFFSET] += ax;
-        transform[TRANSFORM64_TRANSLATION_OFFSET + 1] += ay;
-        transform[TRANSFORM64_TRANSLATION_OFFSET + 2] += az;
-
-        const has_parent_swing = link_swing(instance, parent[node], node, SWING_PARENT);
-        const has_child_swing = children_swing(instance, node, SWING_CHILD);
-
-        let swing = SWING_OUT;
-
-        if (has_parent_swing && has_child_swing) {
-            quat3_nlerp(
-                SWING, SWING_OUT,
-                SWING[SWING_PARENT], SWING[SWING_PARENT + 1], SWING[SWING_PARENT + 2], SWING[SWING_PARENT + 3],
-                SWING[SWING_CHILD], SWING[SWING_CHILD + 1], SWING[SWING_CHILD + 2], SWING[SWING_CHILD + 3],
-                0.5
-            );
-        } else if (has_child_swing) {
-            swing = SWING_CHILD;
-        } else {
-            // A leaf, or a link so short its direction is meaningless — in
-            // which case `link_swing` left the identity there.
-            swing = SWING_PARENT;
-        }
-
-        // Animated orientation first, then the swing: the swing is expressed in
-        // the cloth's local frame, and the anchor's rotation carries the pair
-        // out to world.
-        quat3_multiply(
-            local_rotation, node * 4,
-            SWING[swing], SWING[swing + 1], SWING[swing + 2], SWING[swing + 3],
-            rotation[node * 4], rotation[node * 4 + 1], rotation[node * 4 + 2], rotation[node * 4 + 3]
-        );
-
-        quat3_multiply(
-            transform, TRANSFORM64_ROTATION_OFFSET,
-            aqx, aqy, aqz, aqw,
-            local_rotation[node * 4], local_rotation[node * 4 + 1],
-            local_rotation[node * 4 + 2], local_rotation[node * 4 + 3]
-        );
-
-        transform.updateMatrix();
-
-        write_local(instance, node, parent[node]);
-    }
-
-    pose_pruned(instance, ecd);
-}
-
-/**
- * Express one node's solved pose relative to its parent's, and write it into
- * the entity's `TransformAttachment`.
- *
- * Both poses are already in the cloth's own frame, so the parent's inverse is
- * its reconstructed rotation conjugated — there is no matrix inverse here and
- * no scale, which is the same rigid-pose convention the colliders use.
- *
- * @param {ClothInstance} instance
- * @param {number} node
- * @param {number} parent
- * @private
- */
-function write_local(instance, node, parent) {
-    const attachment = instance.node_attachment[node];
-
-    if (attachment === undefined || attachment === null) {
-        return;
-    }
-
-    const blended = instance.blended_position;
-    const rotation = instance.blended_rotation;
-
-    const local = attachment.transform;
-
-    const pqx = rotation[parent * 4];
-    const pqy = rotation[parent * 4 + 1];
-    const pqz = rotation[parent * 4 + 2];
-    const pqw = rotation[parent * 4 + 3];
-
-    v3_quaternion_apply_inverse(
-        local, TRANSFORM64_TRANSLATION_OFFSET,
-        blended[node * 3] - blended[parent * 3],
-        blended[node * 3 + 1] - blended[parent * 3 + 1],
-        blended[node * 3 + 2] - blended[parent * 3 + 2],
-        pqx, pqy, pqz, pqw
-    );
-
-    quat3_multiply(
-        local, TRANSFORM64_ROTATION_OFFSET,
-        -pqx, -pqy, -pqz, pqw,
-        rotation[node * 4], rotation[node * 4 + 1], rotation[node * 4 + 2], rotation[node * 4 + 3]
-    );
-
-    // `multiplyTransforms` reads the matrix, not the components, so a local
-    // transform that is not refreshed here composes the pose it had last time.
-    local.updateMatrix();
-}
-
-/**
- * Swing of the link from `from` to `to`, into `SWING` at `out`.
- *
- * @param {ClothInstance} instance
- * @param {number} from node
- * @param {number} to node
- * @param {number} out
- * @returns {boolean} false when the link is too short to carry a direction, in
- *   which case the identity was written
- * @private
- */
-function link_swing(instance, from, to, out) {
-    const animated = instance.animated_position;
-    const blended = instance.blended_position;
-
-    const a = from * 3;
-    const b = to * 3;
-
-    const has_animated = unit(DIRECTION, 0,
-        animated[b] - animated[a], animated[b + 1] - animated[a + 1], animated[b + 2] - animated[a + 2]);
-
-    const has_blended = unit(DIRECTION, 3,
-        blended[b] - blended[a], blended[b + 1] - blended[a + 1], blended[b + 2] - blended[a + 2]);
-
-    if (!has_animated || !has_blended) {
-        identity(out);
-
-        return false;
-    }
-
-    quat3_shortest_arc(
-        SWING, out,
-        DIRECTION[0], DIRECTION[1], DIRECTION[2],
-        DIRECTION[3], DIRECTION[4], DIRECTION[5]
-    );
-
-    return true;
-}
-
-/**
- * Swing of a node's outgoing links, averaged over its simulated children.
- *
- * The average is over **unit** directions rather than over child positions, so
- * a fork whose two branches are of different lengths is not dominated by the
- * longer one.
- *
- * @param {ClothInstance} instance
- * @param {number} node
- * @param {number} out
- * @returns {boolean}
- * @private
- */
-function children_swing(instance, node, out) {
-    const start = instance.node_child_offset[node];
-    const end = instance.node_child_offset[node + 1];
-
-    if (start === end) {
-        identity(out);
-
-        return false;
-    }
-
-    const animated = instance.animated_position;
-    const blended = instance.blended_position;
-    const child = instance.node_child;
-
-    const a = node * 3;
-
-    let anim_x = 0, anim_y = 0, anim_z = 0;
-    let sim_x = 0, sim_y = 0, sim_z = 0;
-
-    for (let i = start; i < end; i++) {
-        const b = child[i] * 3;
-
-        if (unit(DIRECTION, 6,
-            animated[b] - animated[a], animated[b + 1] - animated[a + 1], animated[b + 2] - animated[a + 2])) {
-            anim_x += DIRECTION[6];
-            anim_y += DIRECTION[7];
-            anim_z += DIRECTION[8];
-        }
-
-        if (unit(DIRECTION, 9,
-            blended[b] - blended[a], blended[b + 1] - blended[a + 1], blended[b + 2] - blended[a + 2])) {
-            sim_x += DIRECTION[9];
-            sim_y += DIRECTION[10];
-            sim_z += DIRECTION[11];
-        }
-    }
-
-    const has_animated = unit(DIRECTION, 0, anim_x, anim_y, anim_z);
-    const has_blended = unit(DIRECTION, 3, sim_x, sim_y, sim_z);
-
-    if (!has_animated || !has_blended) {
-        // Branches that cancel — a node with two children pointing opposite
-        // ways has no outgoing direction to speak of, and the parent link is a
-        // better answer than an arbitrary one.
-        identity(out);
-
-        return false;
-    }
-
-    quat3_shortest_arc(
-        SWING, out,
-        DIRECTION[0], DIRECTION[1], DIRECTION[2],
-        DIRECTION[3], DIRECTION[4], DIRECTION[5]
-    );
-
-    return true;
-}
-
-/**
- * Pose the entities the walk pruned, and announce there.
- *
- * @param {ClothInstance} instance
- * @param {EntityComponentDataset} ecd
- * @private
- */
-function pose_pruned(instance, ecd) {
-    const count = instance.pruned_count;
-
-    for (let i = 0; i < count; i++) {
-        const transform = instance.pruned_transform[i];
-        const attachment = instance.pruned_attachment[i];
-
-        if (transform === undefined || attachment === undefined) {
-            continue;
-        }
-
-        transform.multiplyTransforms(
-            instance.node_transform[instance.pruned_parent[i]],
-            attachment.transform
-        );
-
-        // The one announcement this pass makes. Everything below a pruned
-        // boundary is outside the cloth and reaches its new pose through the
-        // ordinary attachment cascade; everything above it has already been
-        // written by hand.
-        t64_announce_change(ecd, instance.pruned_entity[i]);
-    }
-}
-
-/**
- * Normalise into `DIRECTION`, reporting whether there was a direction to find.
- *
- * @param {Float64Array} out
- * @param {number} offset
- * @param {number} x
- * @param {number} y
- * @param {number} z
- * @returns {boolean}
- * @private
- */
-function unit(out, offset, x, y, z) {
-    const length_squared = x * x + y * y + z * z;
-
-    if (length_squared < 1e-24) {
-        return false;
-    }
-
-    const inverse = 1 / Math.sqrt(length_squared);
-
-    out[offset] = x * inverse;
-    out[offset + 1] = y * inverse;
-    out[offset + 2] = z * inverse;
-
-    return true;
-}
-
-/**
- * @param {number} offset
- * @private
- */
-function identity(offset) {
-    SWING[offset] = 0;
-    SWING[offset + 1] = 0;
-    SWING[offset + 2] = 0;
-    SWING[offset + 3] = 1;
-}
+import { quat3_multiply } from "../../../../core/geom/3d/quaternion/quat3_multiply.js";
+import { v3_quaternion_apply } from "../../../../core/geom/vec3/v3_quaternion_apply.js";
+import {
+    v3_quaternion_apply_inverse
+} from "../../../../core/geom/vec3/v3_quaternion_apply_inverse.js";
+import { t64_announce_change } from "../../../ecs/transform/t64_announce_change.js";
+import {
+    TRANSFORM64_ROTATION_OFFSET,
+    TRANSFORM64_TRANSLATION_OFFSET
+} from "../../../ecs/transform/Transform64.js";
+import { cloth_reconstruct_pose } from "./cloth_reconstruct_pose.js";
+
+/**
+ * # Putting the answer back onto entities — §11.1
+ *
+ * `cloth_reconstruct_pose.js` turns particles into a pose in the cloth's own
+ * frame — positions blended towards the animated ones, orientations
+ * reconstructed as a swing on the animated orientation. This file is what puts
+ * that pose onto **entities**; `cloth_write_back_rig.js` is what puts the same
+ * pose onto a skinned model's joints, and the split is M3's, made when the
+ * second writer arrived rather than by anticipating it.
+ *
+ * ## Both transforms are written, and that is what makes ordering harmless
+ *
+ * Each simulated node gets its **world** `Transform64` and its **local**
+ * `TransformAttachment#transform`, and the two agree. Writing only the world
+ * one is cheaper and is a trap: the attachment system recomposes a child from
+ * its local whenever the parent announces, so any announcement arriving after
+ * cloth had written would replace the simulated pose with the authored one for
+ * the rest of the frame — and where in the fixed step cloth ran would be a
+ * correctness question. With the local written too, **a recomposition
+ * reproduces exactly what cloth just wrote**, and cloth's placement costs at
+ * worst a shape one fixed step stale.
+ *
+ * That is also the shape of the engine's own precedent:
+ * `InverseKinematicsSystem` reads world transforms the hierarchy has composed
+ * and writes local ones back, and §11.2's rig path writes
+ * `Node3D.transform_local` for the same reason — a local transform is what a
+ * joint angle *is*.
+ *
+ * A node's local transform is its pose relative to its **parent's**, so this
+ * needs the parent's reconstructed rotation as well as its position — which is
+ * what {@link ClothInstance#blended_rotation} is for, and why the pass runs in
+ * the walk's breadth-first order.
+ *
+ * The authored pose is not lost by being overwritten: it was captured at seed
+ * and lives in the cloth (`cloth_seed_subtree.js`), which is what `blend = 0`
+ * returns to and what the leash centres are measured from.
+ *
+ * ## What is announced
+ *
+ * Nothing on a simulated node: every one of its simulated children has already
+ * been written by this same pass, so the cascade would recompute a pose that is
+ * already correct, once per level, quadratically for a chain. What *is*
+ * announced is each pruned boundary — the root of a `ClothExclude` subtree, or
+ * a nested `Cloth`'s anchor — which this pass poses by hand against the link it
+ * hangs from, so everything below the cut follows the simulation through the
+ * ordinary attachment cascade and nothing above it is disturbed.
+ *
+ * @author Alex Goldring
+ * @copyright Company Named Limited (c) 2026
+ */
+
+/**
+ * Write one cloth's solved pose onto its subtree.
+ *
+ * @param {ClothInstance} instance
+ * @param {EntityComponentDataset} ecd
+ * @returns {void}
+ */
+export function cloth_write_back(instance, ecd) {
+    cloth_reconstruct_pose(instance);
+
+    const node_count = instance.node_count;
+
+    const blended = instance.blended_position;
+    const reconstructed = instance.blended_rotation;
+
+    const anchor = instance.node_transform[0];
+
+    const ax = anchor[TRANSFORM64_TRANSLATION_OFFSET];
+    const ay = anchor[TRANSFORM64_TRANSLATION_OFFSET + 1];
+    const az = anchor[TRANSFORM64_TRANSLATION_OFFSET + 2];
+
+    const aqx = anchor[TRANSFORM64_ROTATION_OFFSET];
+    const aqy = anchor[TRANSFORM64_ROTATION_OFFSET + 1];
+    const aqz = anchor[TRANSFORM64_ROTATION_OFFSET + 2];
+    const aqw = anchor[TRANSFORM64_ROTATION_OFFSET + 3];
+
+    const parent = instance.node_parent;
+
+    for (let node = 1; node < node_count; node++) {
+        const transform = instance.node_transform[node];
+
+        const n = node * 3;
+
+        v3_quaternion_apply(
+            transform, TRANSFORM64_TRANSLATION_OFFSET,
+            blended[n], blended[n + 1], blended[n + 2],
+            aqx, aqy, aqz, aqw
+        );
+
+        transform[TRANSFORM64_TRANSLATION_OFFSET] += ax;
+        transform[TRANSFORM64_TRANSLATION_OFFSET + 1] += ay;
+        transform[TRANSFORM64_TRANSLATION_OFFSET + 2] += az;
+
+        quat3_multiply(
+            transform, TRANSFORM64_ROTATION_OFFSET,
+            aqx, aqy, aqz, aqw,
+            reconstructed[node * 4], reconstructed[node * 4 + 1],
+            reconstructed[node * 4 + 2], reconstructed[node * 4 + 3]
+        );
+
+        transform.updateMatrix();
+
+        write_local(instance, node, parent[node]);
+    }
+
+    pose_pruned(instance, ecd);
+}
+
+/**
+ * Express one node's solved pose relative to its parent's, and write it into
+ * the entity's `TransformAttachment`.
+ *
+ * Both poses are already in the cloth's own frame, so the parent's inverse is
+ * its reconstructed rotation conjugated — there is no matrix inverse here and
+ * no scale, which is the same rigid-pose convention the colliders use.
+ *
+ * @param {ClothInstance} instance
+ * @param {number} node
+ * @param {number} parent
+ * @private
+ */
+function write_local(instance, node, parent) {
+    const attachment = instance.node_attachment[node];
+
+    if (attachment === undefined || attachment === null) {
+        return;
+    }
+
+    cloth_write_local_pose(attachment.transform, instance, node, parent);
+}
+
+/**
+ * One node's pose relative to its parent's, in whatever `Transform64` the
+ * caller holds. Both writers need it and neither owns it.
+ *
+ * @param {Transform64} local written in place, matrix included
+ * @param {ClothInstance} instance
+ * @param {number} node
+ * @param {number} parent
+ * @returns {void}
+ */
+export function cloth_write_local_pose(local, instance, node, parent) {
+    const blended = instance.blended_position;
+    const rotation = instance.blended_rotation;
+
+    const pqx = rotation[parent * 4];
+    const pqy = rotation[parent * 4 + 1];
+    const pqz = rotation[parent * 4 + 2];
+    const pqw = rotation[parent * 4 + 3];
+
+    v3_quaternion_apply_inverse(
+        local, TRANSFORM64_TRANSLATION_OFFSET,
+        blended[node * 3] - blended[parent * 3],
+        blended[node * 3 + 1] - blended[parent * 3 + 1],
+        blended[node * 3 + 2] - blended[parent * 3 + 2],
+        pqx, pqy, pqz, pqw
+    );
+
+    quat3_multiply(
+        local, TRANSFORM64_ROTATION_OFFSET,
+        -pqx, -pqy, -pqz, pqw,
+        rotation[node * 4], rotation[node * 4 + 1], rotation[node * 4 + 2], rotation[node * 4 + 3]
+    );
+
+    // `multiplyTransforms` reads the matrix, not the components, so a local
+    // transform that is not refreshed here composes the pose it had last time.
+    local.updateMatrix();
+}
+
+/**
+ * Pose the entities the walk pruned, and announce there.
+ *
+ * @param {ClothInstance} instance
+ * @param {EntityComponentDataset} ecd
+ * @private
+ */
+function pose_pruned(instance, ecd) {
+    const count = instance.pruned_count;
+
+    for (let i = 0; i < count; i++) {
+        const transform = instance.pruned_transform[i];
+        const attachment = instance.pruned_attachment[i];
+
+        if (transform === undefined || attachment === undefined) {
+            continue;
+        }
+
+        transform.multiplyTransforms(
+            instance.node_transform[instance.pruned_parent[i]],
+            attachment.transform
+        );
+
+        // The one announcement this pass makes. Everything below a pruned
+        // boundary is outside the cloth and reaches its new pose through the
+        // ordinary attachment cascade; everything above it has already been
+        // written by hand.
+        t64_announce_change(ecd, instance.pruned_entity[i]);
+    }
+}