@woosh/meep-engine/src/engine/ecs/ik/TwoBoneInverseKinematicsSolver.js

Pure JavaScript game engine. Fully featured and production ready.

import { assert } from "../../../core/assert.js";
import { SurfacePoint3 } from "../../../core/geom/3d/SurfacePoint3.js";
import Quaternion from "../../../core/geom/Quaternion.js";
import { v3_displace_in_direction } from "../../../core/geom/vec3/v3_displace_in_direction.js";
import { v3_dot } from "../../../core/geom/vec3/v3_dot.js";
import { v3_length } from "../../../core/geom/vec3/v3_length.js";
import Vector3 from "../../../core/geom/Vector3.js";
import { clamp01 } from "../../../core/math/clamp01.js";
import { inverseLerp } from "../../../core/math/inverseLerp.js";
import { findSkeletonBoneByType } from "../../graphics/ecs/mesh/SkeletonUtils.js";
import { two_joint_ik } from "../../physics/inverse_kinematics/two_joint_ik.js";
import { IKSolver } from "./IKSolver.js";

const boneWorldPositionC = new Vector3();
const boneWorldPositionB = new Vector3();
const boneWorldPositionA = new Vector3();

const contact = new SurfacePoint3();

const targetPosition = new Vector3();

const v3 = new Vector3();

const globalRotationA = new Quaternion();
const globalRotationB = new Quaternion();
const globalRotationC = new Quaternion();

const targetLocalRotationA = new Quaternion();
const targetLocalRotationB = new Quaternion();

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

/**
 *
 * @param {Object3D} object
 * @param {Vector3} position
 * @param {Quaternion} rotation
 * @param {Vector3} scale
 */
function computeGlobalTransform(object, position, rotation, scale) {
    const matrixWorld = object.matrixWorld;

    matrixWorld.decompose(position, rotation, scale);
}

export class TwoBoneInverseKinematicsSolver extends IKSolver {


    solve(problem) {

        const { skeleton, terrain, constraint } = problem;

        const effectorBoneType = constraint.effector;

        const boneC = findSkeletonBoneByType(skeleton, effectorBoneType);

        assert.notNull(boneC, 'boneC');

        const boneB = boneC.parent;

        assert.notNull(boneB, 'boneB');

        const boneA = boneB.parent;
        assert.notNull(boneA, 'boneA');

        //Update matrix root bone, this will update all bones in the chain. Matrix update is necessary to ensure we read actual transform values
        boneA.updateMatrixWorld(true);

        //compute current position of the bones
        computeGlobalTransform(boneA, boneWorldPositionA, globalRotationA, v3);
        computeGlobalTransform(boneB, boneWorldPositionB, globalRotationB, v3);
        computeGlobalTransform(boneC, boneWorldPositionC, globalRotationC, v3);

        //check if the there is a penetration with the terrain between the two bones
        const directionX = boneWorldPositionC.x - boneWorldPositionA.x;
        const directionY = boneWorldPositionC.y - boneWorldPositionA.y;
        const directionZ = boneWorldPositionC.z - boneWorldPositionA.z;

        let contactExists = terrain.raycastFirstSync(
            contact,
            boneWorldPositionA.x,
            boneWorldPositionA.y,
            boneWorldPositionA.z,
            directionX,
            directionY,
            directionZ
        );

        if (!contactExists) {
            //no contact
            return;
        }

        const contactPosition = contact.position;
        const contactNormal = contact.normal;

        //perform secondary cast from the bone, back along the surface normal
        contactExists = terrain.raycastFirstSync(
            contact,
            boneWorldPositionC.x + contactNormal.x,
            boneWorldPositionC.y + contactNormal.y,
            boneWorldPositionC.z + contactNormal.z,
            -contactNormal.x,
            -contactNormal.y,
            -contactNormal.z
        );

        if (!contactExists) {
            //no contact
            return;
        }

        //compute total length of the limb
        const limbLength = boneWorldPositionA.distanceTo(boneWorldPositionB) + boneWorldPositionB.distanceTo(boneWorldPositionC);

        const targetOffsetDistance = constraint.offset * limbLength;

        v3_displace_in_direction(
            targetPosition,
            targetOffsetDistance,
            contactPosition.x, contactPosition.y, contactPosition.z,
            contact.normal.x, contact.normal.y, contact.normal.z
        );

        const delta_contact_c_x = targetPosition.x - boneWorldPositionC.x;
        const delta_contact_c_y = targetPosition.y - boneWorldPositionC.y;
        const delta_contact_c_z = targetPosition.z - boneWorldPositionC.z;

        //check if current effector position is above the contact point
        const dot_contact_side = v3_dot(directionX, directionY, directionZ, delta_contact_c_x, delta_contact_c_y, delta_contact_c_z);

        const delta_contact_c_length = v3_length(delta_contact_c_x, delta_contact_c_y, delta_contact_c_z);

        let influence;

        if (dot_contact_side < 0) {
            //penetration detected
            influence = 1;

        } else {
            //no penetration, hovering case, use hover distance for influence
            const normalized_effector_distance = inverseLerp(0, limbLength, delta_contact_c_length);

            influence = 1 - clamp01(constraint.distance.normalizeValue(normalized_effector_distance));
        }

        if (influence <= 0) {
            //no influence
            return;
        }

        targetLocalRotationA.copy(boneA.quaternion);
        targetLocalRotationB.copy(boneB.quaternion);

        //compute
        two_joint_ik(
            boneWorldPositionA,
            boneWorldPositionB,
            boneWorldPositionC,
            targetPosition,
            0.01,
            globalRotationA,
            globalRotationB,
            targetLocalRotationA,
            targetLocalRotationB
        );

        //lerp bone positions based on influence
        r0.lerpQuaternions(boneA.quaternion, targetLocalRotationA, influence);
        r1.lerpQuaternions(boneB.quaternion, targetLocalRotationB, influence);

        boneA.setRotationFromQuaternion(r0);
        boneB.setRotationFromQuaternion(r1);

        boneA.updateMatrix();
        boneB.updateMatrix();
        boneC.updateMatrix();

        boneA.updateMatrixWorld();
        boneB.updateMatrixWorld();
        boneC.updateMatrixWorld();

        boneC.updateWorldMatrix(true, true);
    }
}