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

Pure JavaScript game engine. Fully featured and production ready.

import Quaternion from "../../../../core/geom/Quaternion.js";
import Vector3 from "../../../../core/geom/Vector3.js";
import { fabrik3d_solve_primitive } from "./fabrik3d_solve_primitive.js";

const scratch_positions_count = 64;
const scratch_positions = new Float32Array(scratch_positions_count * 3);

const scratch_v3_0 = new Vector3();
const scratch_v3_1 = new Vector3();
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 {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
 * @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 fabrik_solve(
    joints,
    lengths,
    origin,
    target,
    max_iterations = 4,
    distance_tolerance = 1e-7
) {

    const joint_count = joints.length;

    let positions;
    if (joint_count <= scratch_positions_count) {
        positions = scratch_positions;
    } else {
        positions = new Float32Array(joint_count * 3);
    }

    for (let i = 0; i < joint_count; i++) {
        const offset = i * 3;
        const joint = joints[i];

        joint.position.toArray(positions, offset);
    }

    fabrik3d_solve_primitive(
        joint_count,
        positions,
        lengths,
        origin.x, origin.y, origin.z,
        target.x, target.y, target.z,
        max_iterations, distance_tolerance
    );

    // apply rotation
    for (let i = 1; i < joint_count; i++) {
        const current_joint = joints[i];
        const previous_joint = joints[i - 1];

        const i3 = i * 3;

        const previous_x = positions[i3 - 3];
        const previous_y = positions[i3 - 2];
        const previous_z = positions[i3 - 1];

        const current_x = positions[i3];
        const current_y = positions[i3 + 1];
        const current_z = positions[i3 + 2];

        scratch_v3_0.copy(current_joint.position);
        scratch_v3_0.sub(previous_joint.position);

        if (scratch_v3_0.lengthSqr() === 0) {
            continue;
        }

        scratch_v3_1.set(
            current_x - previous_x,
            current_y - previous_y,
            current_z - previous_z,
        );

        if (scratch_v3_1.lengthSqr() === 0) {
            continue;
        }

        scratch_v3_0.normalize();
        scratch_v3_1.normalize();

        scratch_quat.fromUnitVectors(scratch_v3_0, scratch_v3_1);

        previous_joint.rotation.multiplyQuaternions(scratch_quat, previous_joint.rotation);
    }

    // apply positions
    for (let i = 0; i < joint_count; i++) {
        joints[i].position.fromArray(positions, i * 3);
    }
}