@woosh/meep-engine 2.131.28 → 2.131.29

package/package.json

@@ -5,7 +5,7 @@
   "description": "Pure JavaScript game engine. Fully featured and production ready.",
   "type": "module",
   "author": "Alexander Goldring",
-  "version": "2.131.28",
+  "version": "2.131.29",
   "main": "build/meep.module.js",
   "module": "build/meep.module.js",
   "exports": {

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

@@ -1,6 +1,6 @@
 /**
  * Implementation of FABRIK algorithm, "Forward And Backward Reaching Inverse Kinematics"
- * This implementation deals with single chain at a time, without support for multiple effectors
+ * This implementation deals with a 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

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

@@ -1 +1 @@
-{"version":3,"file":"fabrik3d_solve_primitive.d.ts","sourceRoot":"","sources":["../../../../../../src/engine/physics/inverse_kinematics/fabrik/fabrik3d_solve_primitive.js"],"names":[],"mappings":"AAQA;;;;;;;;;;;;;;;GAeG;AACH,+CAZW,MAAM,aACN,YAAY,GAAC,MAAM,EAAE,WACrB,YAAY,GAAC,MAAM,EAAE,YACrB,MAAM,YACN,MAAM,YACN,MAAM,YACN,MAAM,YACN,MAAM,YACN,MAAM,mBACN,MAAM,uBACN,MAAM,QAsDhB"}
+{"version":3,"file":"fabrik3d_solve_primitive.d.ts","sourceRoot":"","sources":["../../../../../../src/engine/physics/inverse_kinematics/fabrik/fabrik3d_solve_primitive.js"],"names":[],"mappings":"AAAA;;;;;;;;;;;;;;;GAeG;AACH,+CAZW,MAAM,aACN,YAAY,GAAC,MAAM,EAAE,WACrB,YAAY,GAAC,MAAM,EAAE,YACrB,MAAM,YACN,MAAM,YACN,MAAM,YACN,MAAM,YACN,MAAM,YACN,MAAM,mBACN,MAAM,uBACN,MAAM,QA8FhB"}

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

@@ -1,14 +1,6 @@
-import { v3_array_normalize } from "../../../../core/geom/vec3/v3_array_normalize.js";
-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";
-
-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
+ * This implementation deals with a 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
@@ -37,241 +29,183 @@ export function fabrik3d_solve_primitive(
         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
-        );
+    // 1. Check Reachability
+    // Calculate distance to target squared first to avoid sqrt if not needed
+    const dx = target_x - origin_x;
+    const dy = target_y - origin_y;
+    const dz = target_z - origin_z;
+    const dist_sq = dx * dx + dy * dy + dz * dz;
+
+    // Calculate total length if not provided (fallback)
+    let total_chain_length = 0;
+    for (let i = 0; i < size - 1; i++) {
+        total_chain_length += lengths[i];
+    }
 
+    // Unreachable: The target is further than the total length
+    // (Compare squares to save a SQRT on the distance check)
+    if (dist_sq > total_chain_length * total_chain_length) {
+        stretch_chain(size, positions, lengths, origin_x, origin_y, origin_z, dx, dy, dz, Math.sqrt(dist_sq));
         return;
     }
 
+    // 2. Iteration Loop
+    const last_index = size - 1;
+    const last_offset = last_index * 3;
 
     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);
+        // --- Backward Pass (Target -> Origin) ---
+        // Snap the last point to target
+        positions[last_offset] = target_x;
+        positions[last_offset + 1] = target_y;
+        positions[last_offset + 2] = target_z;
 
-        const last_join_address = (size - 1) * 3;
+        for (let j = last_index - 1; j >= 0; j--) {
+            const curr = j * 3;
+            const next = (j + 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];
+            // Vector from Next (fixed) to Current (to be moved)
+            // We want to move Current towards Next until distance is lengths[j]
+            const bone_len = lengths[j];
 
-        if (v3_distance_sqr(last_joint_x, last_joint_y, last_joint_z, target_x, target_y, target_z) <= distance_tolerance) {
-            // close enough
-            break;
+            solve_joint(positions, curr, next, bone_len);
         }
 
-    }
+        // --- Forward Pass (Origin -> Target) ---
+        // Snap first point to origin
+        positions[0] = origin_x;
+        positions[1] = origin_y;
+        positions[2] = origin_z;
 
-}
+        for (let j = 0; j < last_index; j++) {
+            const curr = (j + 1) * 3; // The point we are moving
+            const prev = j * 3;       // The fixed point
 
-/**
- * @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_array_normalize(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;
-    }
-
-}
+            const bone_len = lengths[j];
 
-/**
- *
- * @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;
+            solve_joint(positions, curr, prev, bone_len);
+        }
 
-    const length_sqr = v3_length_sqr(dx, dy, dz);
+        // --- Check Tolerance ---
+        const tip_x = positions[last_offset];
+        const tip_y = positions[last_offset + 1];
+        const tip_z = positions[last_offset + 2];
 
-    if (length_sqr === 0) {
-        // TODO use random point on a sphere to prevent pathology
+        const delta_x = tip_x - target_x;
+        const delta_y = tip_y - target_y;
+        const delta_z = tip_z - target_z;
 
-        // 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;
+        const dist_to_target_sq =
+            delta_x * delta_x
+            + delta_y * delta_y
+            + delta_z * delta_z;
 
-        return;
+        if (dist_to_target_sq <= distance_tolerance) {
+            break;
+        }
     }
 
-    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
+ * Inline-friendly helper to project point 'curr' onto the line starting at 'anchor'
+ * such that the distance between them becomes 'length'.
  * @param {Float32Array} positions
- * @param {Float32Array|number[]} lengths
- * @param {number} origin_x
- * @param {number} origin_y
- * @param {number} origin_z
+ * @param {number} curr_ptr
+ * @param {number} anchor_ptr
+ * @param {number} length
  */
-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
-        );
+function solve_joint(
+    positions,
+    curr_ptr,
+    anchor_ptr,
+    length
+) {
+    const ax = positions[anchor_ptr];
+    const ay = positions[anchor_ptr + 1];
+    const az = positions[anchor_ptr + 2];
+
+    const cx = positions[curr_ptr];
+    const cy = positions[curr_ptr + 1];
+    const cz = positions[curr_ptr + 2];
+
+    // Vector from Anchor -> Current
+    const dx = cx - ax;
+    const dy = cy - ay;
+    const dz = cz - az;
+
+    const len_sq = dx * dx + dy * dy + dz * dz;
+
+    if (len_sq < 1e-15) {
+        // Prevent division by zero / singularity
+        // If points overlap perfectly, pick arbitrary direction (Z-axis)
+        positions[curr_ptr] = ax;
+        positions[curr_ptr + 1] = ay;
+        positions[curr_ptr + 2] = az + length;
+    }else {
+
+        // Math: NewPos = Anchor + (Direction * Length)
+        // Direction = Vector / CurrentLength
+        // NewPos = Anchor + Vector * (Length / CurrentLength)
+        // We use inverse sqrt for speed if available, or just standard math
+
+        const scale = length / Math.sqrt(len_sq);
+
+        positions[curr_ptr] = ax + dx * scale;
+        positions[curr_ptr + 1] = ay + dy * scale;
+        positions[curr_ptr + 2] = az + dz * scale;
     }
-
 }
 
 /**
- *
+ * Handle unreachable target by stretching chain in a straight line
  * @param {number} size
  * @param {Float32Array} positions
- * @param {Float32Array|number[]} lengths
- * @param {number} target_x
- * @param {number} target_y
- * @param {number} target_z
+ * @param {Float32Array} lengths
+ * @param {number} ox
+ * @param {number} oy
+ * @param {number} oz
+ * @param {number} dx
+ * @param {number} dy
+ * @param {number} dz
+ * @param {number} current_dist
  */
-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
-        );
+function stretch_chain(
+    size,
+    positions,
+    lengths,
+    ox, oy, oz,
+    dx, dy, dz,
+    current_dist
+) {
 
-        v3_displace_in_direction_array(
-            positions, current_address,
-            positions, next_address,
-            scratch_direction, 0,
-            lengths[i]
-        )
+    // Normalize direction
+    const inv_dist = 1.0 / current_dist;
+    const nx = dx * inv_dist;
+    const ny = dy * inv_dist;
+    const nz = dz * inv_dist;
+
+    // Reset origin
+    positions[0] = ox;
+    positions[1] = oy;
+    positions[2] = oz;
+
+    let prev_x = ox;
+    let prev_y = oy;
+    let prev_z = oz;
+
+    for (let i = 0; i < size - 1; i++) {
+        const len = lengths[i];
+        const next_offset = (i + 1) * 3;
+
+        // Move point along the line
+        prev_x += nx * len;
+        prev_y += ny * len;
+        prev_z += nz * len;
+
+        positions[next_offset] = prev_x;
+        positions[next_offset + 1] = prev_y;
+        positions[next_offset + 2] = prev_z;
     }
 }

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

@@ -2,7 +2,7 @@
  * Note that bones are defined as a combination of Joint ({@link Transform}) and bone length. Joint defines the origin of the bone.
  * Last joint's position is expected to be bone_length distance from the target if target is reachable and be oriented towards that target ({@link Transform.lookAt})
  * @param {Transform[]} joints Will be updated as a result of the solve
- * @param {number[]} lengths distance to next bone
+ * @param {number[]} lengths distance to next bone, bone lengths.
  * @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

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

@@ -13,7 +13,7 @@ const scratch_quat = new Quaternion();
  * Note that bones are defined as a combination of Joint ({@link Transform}) and bone length. Joint defines the origin of the bone.
  * Last joint's position is expected to be bone_length distance from the target if target is reachable and be oriented towards that target ({@link Transform.lookAt})
  * @param {Transform[]} joints Will be updated as a result of the solve
- * @param {number[]} lengths distance to next bone
+ * @param {number[]} lengths distance to next bone, bone lengths.
  * @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