Babylon.js/src/Bones/boneIKController.ts

import { Bone } from "./bone";
import { Vector3, Quaternion, Matrix } from "../Maths/math.vector";
import { AbstractMesh } from "../Meshes/abstractMesh";
import { Nullable } from "../types";
import { Space } from '../Maths/math.axis';

/**
 * Class used to apply inverse kinematics to bones
 * @see https://doc.babylonjs.com/how_to/how_to_use_bones_and_skeletons#boneikcontroller
 */
export class BoneIKController {

    private static _tmpVecs: Vector3[] = [Vector3.Zero(), Vector3.Zero(), Vector3.Zero(), Vector3.Zero(), Vector3.Zero(), Vector3.Zero()];
    private static _tmpQuat = Quaternion.Identity();
    private static _tmpMats: Matrix[] = [Matrix.Identity(), Matrix.Identity()];

    /**
     * Gets or sets the target mesh
     */
    public targetMesh: AbstractMesh;

    /** Gets or sets the mesh used as pole */
    public poleTargetMesh: AbstractMesh;

    /**
     * Gets or sets the bone used as pole
     */
    public poleTargetBone: Nullable<Bone>;

    /**
     * Gets or sets the target position
     */
    public targetPosition = Vector3.Zero();

    /**
     * Gets or sets the pole target position
     */
    public poleTargetPosition = Vector3.Zero();

    /**
     * Gets or sets the pole target local offset
     */
    public poleTargetLocalOffset = Vector3.Zero();

    /**
     * Gets or sets the pole angle
     */
    public poleAngle = 0;

    /**
     * Gets or sets the mesh associated with the controller
     */
    public mesh: AbstractMesh;

    /**
     * The amount to slerp (spherical linear interpolation) to the target.  Set this to a value between 0 and 1 (a value of 1 disables slerp)
     */
    public slerpAmount = 1;

    private _bone1Quat = Quaternion.Identity();
    private _bone1Mat = Matrix.Identity();
    private _bone2Ang = Math.PI;

    private _bone1: Nullable<Bone>;
    private _bone2: Bone;
    private _bone1Length: number;
    private _bone2Length: number;
    private _maxAngle = Math.PI;
    private _maxReach: number;

    private _rightHandedSystem = false;

    private _bendAxis = Vector3.Right();
    private _slerping = false;

    private _adjustRoll = 0;

    /**
     * Gets or sets maximum allowed angle
     */
    public get maxAngle(): number {
        return this._maxAngle;
    }

    public set maxAngle(value: number) {
        this._setMaxAngle(value);
    }

    /**
     * Creates a new BoneIKController
     * @param mesh defines the mesh to control
     * @param bone defines the bone to control
     * @param options defines options to set up the controller
     */
    constructor(mesh: AbstractMesh,
        bone: Bone,
        options?: {
            targetMesh?: AbstractMesh,
            poleTargetMesh?: AbstractMesh,
            poleTargetBone?: Bone,
            poleTargetLocalOffset?: Vector3,
            poleAngle?: number,
            bendAxis?: Vector3,
            maxAngle?: number,
            slerpAmount?: number
        }) {

        this._bone2 = bone;
        this._bone1 = bone.getParent();

        if (!this._bone1) {
            return;
        }

        this.mesh = mesh;

        var bonePos = bone.getPosition();

        if (bone.getAbsoluteTransform().determinant() > 0) {
            this._rightHandedSystem = true;
            this._bendAxis.x = 0;
            this._bendAxis.y = 0;
            this._bendAxis.z = -1;

            if (bonePos.x > bonePos.y && bonePos.x > bonePos.z) {
                this._adjustRoll = Math.PI * .5;
                this._bendAxis.z = 1;
            }
        }

        if (this._bone1.length) {

            var boneScale1 = this._bone1.getScale();
            var boneScale2 = this._bone2.getScale();

            this._bone1Length = this._bone1.length * boneScale1.y * this.mesh.scaling.y;
            this._bone2Length = this._bone2.length * boneScale2.y * this.mesh.scaling.y;

        } else if (this._bone1.children[0]) {

            mesh.computeWorldMatrix(true);

            var pos1 = this._bone2.children[0].getAbsolutePosition(mesh);
            var pos2 = this._bone2.getAbsolutePosition(mesh);
            var pos3 = this._bone1.getAbsolutePosition(mesh);

            this._bone1Length = Vector3.Distance(pos1, pos2);
            this._bone2Length = Vector3.Distance(pos2, pos3);
        }

        this._bone1.getRotationMatrixToRef(Space.WORLD, mesh, this._bone1Mat);
        this.maxAngle = Math.PI;

        if (options) {

            if (options.targetMesh) {
                this.targetMesh = options.targetMesh;
                this.targetMesh.computeWorldMatrix(true);
            }

            if (options.poleTargetMesh) {

                this.poleTargetMesh = options.poleTargetMesh;
                this.poleTargetMesh.computeWorldMatrix(true);

            } else if (options.poleTargetBone) {
                this.poleTargetBone = options.poleTargetBone;
            } else if (this._bone1.getParent()) {
                this.poleTargetBone = this._bone1.getParent();
            }

            if (options.poleTargetLocalOffset) {
                this.poleTargetLocalOffset.copyFrom(options.poleTargetLocalOffset);
            }

            if (options.poleAngle) {
                this.poleAngle = options.poleAngle;
            }

            if (options.bendAxis) {
                this._bendAxis.copyFrom(options.bendAxis);
            }

            if (options.maxAngle) {
                this.maxAngle = options.maxAngle;
            }

            if (options.slerpAmount) {
                this.slerpAmount = options.slerpAmount;
            }

        }

    }

    private _setMaxAngle(ang: number): void {

        if (ang < 0) {
            ang = 0;
        }

        if (ang > Math.PI || ang == undefined) {
            ang = Math.PI;
        }

        this._maxAngle = ang;

        var a = this._bone1Length;
        var b = this._bone2Length;

        this._maxReach = Math.sqrt(a * a + b * b - 2 * a * b * Math.cos(ang));

    }

    /**
     * Force the controller to update the bones
     */
    public update(): void {
        var bone1 = this._bone1;

        if (!bone1) {
            return;
        }

        var target = this.targetPosition;
        var poleTarget = this.poleTargetPosition;

        var mat1 = BoneIKController._tmpMats[0];
        var mat2 = BoneIKController._tmpMats[1];

        if (this.targetMesh) {
            target.copyFrom(this.targetMesh.getAbsolutePosition());
        }

        if (this.poleTargetBone) {
            this.poleTargetBone.getAbsolutePositionFromLocalToRef(this.poleTargetLocalOffset, this.mesh, poleTarget);
        } else if (this.poleTargetMesh) {
            Vector3.TransformCoordinatesToRef(this.poleTargetLocalOffset, this.poleTargetMesh.getWorldMatrix(), poleTarget);
        }

        var bonePos = BoneIKController._tmpVecs[0];
        var zaxis = BoneIKController._tmpVecs[1];
        var xaxis = BoneIKController._tmpVecs[2];
        var yaxis = BoneIKController._tmpVecs[3];
        var upAxis = BoneIKController._tmpVecs[4];

        var _tmpQuat = BoneIKController._tmpQuat;

        bone1.getAbsolutePositionToRef(this.mesh, bonePos);

        poleTarget.subtractToRef(bonePos, upAxis);

        if (upAxis.x == 0 && upAxis.y == 0 && upAxis.z == 0) {
            upAxis.y = 1;
        } else {
            upAxis.normalize();
        }

        target.subtractToRef(bonePos, yaxis);
        yaxis.normalize();

        Vector3.CrossToRef(yaxis, upAxis, zaxis);
        zaxis.normalize();

        Vector3.CrossToRef(yaxis, zaxis, xaxis);
        xaxis.normalize();

        Matrix.FromXYZAxesToRef(xaxis, yaxis, zaxis, mat1);

        var a = this._bone1Length;
        var b = this._bone2Length;

        var c = Vector3.Distance(bonePos, target);

        if (this._maxReach > 0) {
            c = Math.min(this._maxReach, c);
        }

        var acosa = (b * b + c * c - a * a) / (2 * b * c);
        var acosb = (c * c + a * a - b * b) / (2 * c * a);

        if (acosa > 1) {
            acosa = 1;
        }

        if (acosb > 1) {
            acosb = 1;
        }

        if (acosa < -1) {
            acosa = -1;
        }

        if (acosb < -1) {
            acosb = -1;
        }

        var angA = Math.acos(acosa);
        var angB = Math.acos(acosb);

        var angC = -angA - angB;

        if (this._rightHandedSystem) {

            Matrix.RotationYawPitchRollToRef(0, 0, this._adjustRoll, mat2);
            mat2.multiplyToRef(mat1, mat1);

            Matrix.RotationAxisToRef(this._bendAxis, angB, mat2);
            mat2.multiplyToRef(mat1, mat1);

        } else {

            var _tmpVec = BoneIKController._tmpVecs[5];

            _tmpVec.copyFrom(this._bendAxis);
            _tmpVec.x *= -1;

            Matrix.RotationAxisToRef(_tmpVec, -angB, mat2);
            mat2.multiplyToRef(mat1, mat1);

        }

        if (this.poleAngle) {
            Matrix.RotationAxisToRef(yaxis, this.poleAngle, mat2);
            mat1.multiplyToRef(mat2, mat1);
        }

        if (this._bone1) {
            if (this.slerpAmount < 1) {
                if (!this._slerping) {
                    Quaternion.FromRotationMatrixToRef(this._bone1Mat, this._bone1Quat);
                }
                Quaternion.FromRotationMatrixToRef(mat1, _tmpQuat);
                Quaternion.SlerpToRef(this._bone1Quat, _tmpQuat, this.slerpAmount, this._bone1Quat);
                angC = this._bone2Ang * (1.0 - this.slerpAmount) + angC * this.slerpAmount;
                this._bone1.setRotationQuaternion(this._bone1Quat, Space.WORLD, this.mesh);
                this._slerping = true;
            } else {
                this._bone1.setRotationMatrix(mat1, Space.WORLD, this.mesh);
                this._bone1Mat.copyFrom(mat1);
                this._slerping = false;
            }
        }

        this._bone2.setAxisAngle(this._bendAxis, angC, Space.LOCAL);
        this._bone2Ang = angC;
    }
}