@woosh/meep-engine/src/engine/physics/inverse_kinematics/two_joint_ik.js

Pure JavaScript game engine. Fully featured and production ready.

import Quaternion from "../../../core/geom/Quaternion.js";
import { v3_compute_interior_angle } from "../../../core/geom/vec3/v3_compute_interior_angle.js";
import Vector3 from "../../../core/geom/Vector3.js";
import { clamp } from "../../../core/math/clamp.js";

const d = new Vector3();

const delta_ca = new Vector3();
const delta_ta = new Vector3();

const axis0 = new Vector3();
const axis1 = new Vector3();

const inv_a_gr = new Quaternion();
const inv_b_gr = new Quaternion();

const r0 = new Quaternion();
const r1 = new Quaternion();
const r2 = new Quaternion();

const axis0_la = new Vector3();
const axis0_lb = new Vector3();
const axis1_la = new Vector3();

// TODO: investigate https://github.com/guillaumeblanc/ozz-animation/blob/master/src/animation/runtime/ik_two_bone_job.cc

/**
 * Based on http://theorangeduck.com/page/simple-two-joint
 * @param {Vector3} a Root bone position
 * @param {Vector3} b Second bone position
 * @param {Vector3} c Effector position
 * @param {Vector3} t Target position
 * @param {number} eps EPSILON value, small value for rounding error compensation
 * @param {Quaternion} a_gr Global rotation of root bone
 * @param {Quaternion} b_gr Global rotation of second bone
 * @param {Quaternion} a_lr local rotation for root bone, this will be updated as a result
 * @param {Quaternion} b_lr local rotation for second bone, this will be updated as a result
 */
export function two_joint_ik(
    a, b, c, t, eps,
    a_gr, b_gr,
    a_lr, b_lr
) {

    //Compute lengths of bones
    const lab = b.distanceTo(a);
    const lcb = b.distanceTo(c);

    const maximum_extension = lab + lcb - eps;

    //clamp length to the target to maximum extension of the joint
    const lat = clamp(t.distanceTo(a), eps, maximum_extension);

    //compute current interior angles
    const ac_ab_0 = v3_compute_interior_angle(c, a, b);
    const ba_bc_0 = v3_compute_interior_angle(a, b, c);
    const ac_at_0 = v3_compute_interior_angle(c, a, t);

    // Using the cosine rule to compute desired interior angles
    const length_at_sqr = lat * lat;
    const length_cb_sqr = lcb * lcb;
    const length_ab_sqr = lab * lab;

    const ac_ab_1 = Math.acos(clamp((length_cb_sqr - length_ab_sqr - length_at_sqr) / (-2 * lab * lat), -1, 1));
    const ba_bc_1 = Math.acos(clamp((length_at_sqr - length_ab_sqr - length_cb_sqr) / (-2 * lab * lcb), -1, 1));

    d.copy(Vector3.back);
    d.applyQuaternion(b_gr);

    delta_ca.subVectors(c, a);
    delta_ta.subVectors(t, a);

    axis0.crossVectors(delta_ca, d);
    axis0.normalize();

    axis1.crossVectors(delta_ca, delta_ta);
    axis1.normalize();

    inv_a_gr.copyInverse(a_gr);
    inv_b_gr.copyInverse(b_gr);

    axis0_la.copy(axis0);
    axis0_la.applyQuaternion(inv_a_gr);

    const angle0 = ac_ab_1 - ac_ab_0;

    r0.fromAxisAngle(axis0_la, angle0);

    const angle1 = ba_bc_1 - ba_bc_0;

    axis0_lb.copy(axis0);
    axis0_lb.applyQuaternion(inv_b_gr);

    r1.fromAxisAngle(axis0_lb, angle1);

    axis1_la.copy(axis1);
    axis1_la.applyQuaternion(inv_a_gr);

    r2.fromAxisAngle(axis1_la, ac_at_0);

    r0.multiply(r2);

    a_lr.multiply(r0);
    b_lr.multiply(r1);
}