@woosh/meep-engine/src/engine/physics/inverse_kinematics/fabrik/fabrik3d_solve_primitive.js

Pure JavaScript game engine. Fully featured and production ready.

/**
 * Implementation of FABRIK algorithm, "Forward And Backward Reaching Inverse Kinematics"
 * 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
 * @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;
    }

    // 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++) {

        // --- 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;

        for (let j = last_index - 1; j >= 0; j--) {
            const curr = j * 3;
            const next = (j + 1) * 3;

            // 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];

            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

            const bone_len = lengths[j];

            solve_joint(positions, curr, prev, bone_len);
        }

        // --- Check Tolerance ---
        const tip_x = positions[last_offset];
        const tip_y = positions[last_offset + 1];
        const tip_z = positions[last_offset + 2];

        const delta_x = tip_x - target_x;
        const delta_y = tip_y - target_y;
        const delta_z = tip_z - target_z;

        const dist_to_target_sq =
            delta_x * delta_x
            + delta_y * delta_y
            + delta_z * delta_z;

        if (dist_to_target_sq <= distance_tolerance) {
            break;
        }
    }

}

/**
 * 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 {number} curr_ptr
 * @param {number} anchor_ptr
 * @param {number} 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} lengths
 * @param {number} ox
 * @param {number} oy
 * @param {number} oz
 * @param {number} dx
 * @param {number} dy
 * @param {number} dz
 * @param {number} current_dist
 */
function stretch_chain(
    size,
    positions,
    lengths,
    ox, oy, oz,
    dx, dy, dz,
    current_dist
) {

    // 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;
    }
}