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

Pure JavaScript game engine. Fully featured and production ready.

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

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

        return;
    }


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

        const last_join_address = (size - 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];

        if (v3_distance_sqr(last_joint_x, last_joint_y, last_joint_z, target_x, target_y, target_z) <= distance_tolerance) {
            // close enough
            break;
        }

    }

}

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

}

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

    const length_sqr = v3_length_sqr(dx, dy, dz);

    if (length_sqr === 0) {
        // TODO use random point on a sphere to prevent pathology

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

        return;
    }

    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
 * @param {Float32Array} positions
 * @param {Float32Array|number[]} lengths
 * @param {number} origin_x
 * @param {number} origin_y
 * @param {number} origin_z
 */
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
        );
    }

}

/**
 *
 * @param {number} size
 * @param {Float32Array} positions
 * @param {Float32Array|number[]} lengths
 * @param {number} target_x
 * @param {number} target_y
 * @param {number} target_z
 */
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
        );

        v3_displace_in_direction_array(
            positions, current_address,
            positions, next_address,
            scratch_direction, 0,
            lengths[i]
        )
    }
}