#

API Docs for: 0.6.0
Show: Inherited Protected Private Deprecated

# File: src/equations/Equation.js

    module.exports = Equation;

    var JacobianElement = require('../math/JacobianElement'),
        Vec3 = require('../math/Vec3');

    /**
     * Equation base class
     * @class Equation
     * @constructor
     * @author schteppe
     * @param {Body} bi
     * @param {Body} bj
     * @param {Number} minForce Minimum (read: negative max) force to be applied by the constraint.
     * @param {Number} maxForce Maximum (read: positive max) force to be applied by the constraint.
     */
    function Equation(bi,bj,minForce,maxForce){
        this.id = -1;

        /**
         * @property {number} minForce
         */
        this.minForce = typeof(minForce)==="undefined" ? -1e6 : minForce;

        /**
         * @property {number} maxForce
         */
        this.maxForce = typeof(maxForce)==="undefined" ? 1e6 : maxForce;

        /**
         * @property bi
         * @type {Body}
         */
        this.bi = bi;

        /**
         * @property bj
         * @type {Body}
         */
        this.bj = bj;

        /**
         * SPOOK parameter
         * @property {number} a
         */
        this.a = 0.0;

        /**
         * SPOOK parameter
         * @property {number} b
         */
        this.b = 0.0;

        /**
         * SPOOK parameter
         * @property {number} eps
         */
        this.eps = 0.0;

        /**
         * @property {JacobianElement} jacobianElementA
         */
        this.jacobianElementA = new JacobianElement();

        /**
         * @property {JacobianElement} jacobianElementA
         */
        this.jacobianElementB = new JacobianElement();

        // Set typical spook params
        this.setSpookParams(1e7,4,1/60);

        this.enabled = true;
    }
    Equation.prototype.constructor = Equation;

    /**
     * Recalculates a,b,eps.
     * @method setSpookParams
     */
    Equation.prototype.setSpookParams = function(stiffness,relaxation,timeStep){
        var d = relaxation,
            k = stiffness,
            h = timeStep;
        this.a = 4.0 / (h * (1 + 4 * d));
        this.b = (4.0 * d) / (1 + 4 * d);
        this.eps = 4.0 / (h * h * k * (1 + 4 * d));
    };

    /**
     * Computes the RHS of the SPOOK equation
     * @method computeB
     * @return {Number}
     */
    Equation.prototype.computeB = function(a,b,h){
        var GW = this.computeGW(),
            Gq = this.computeGq(),
            GiMf = this.computeGiMf();
        return - Gq * a - GW * b - GiMf*h;
    };

    /**
     * Computes G*q, where q are the generalized body coordinates
     * @method computeGq
     * @return {Number}
     */
    Equation.prototype.computeGq = function(){
        var GA = this.jacobianElementA,
            GB = this.jacobianElementB,
            bi = this.bi,
            bj = this.bj,
            xi = bi.position,
            xj = bj.position;
        return GA.spatial.dot(xi) + GB.spatial.dot(xj);
    };

    var zero = new Vec3();

    /**
     * Computes G*W, where W are the body velocities
     * @method computeGW
     * @return {Number}
     */
    Equation.prototype.computeGW = function(){
        var GA = this.jacobianElementA,
            GB = this.jacobianElementB,
            bi = this.bi,
            bj = this.bj,
            vi = bi.velocity,
            vj = bj.velocity,
            wi = bi.angularVelocity || zero,
            wj = bj.angularVelocity || zero;
        return GA.multiplyVectors(vi,wi) + GB.multiplyVectors(vj,wj);
    };

    /**
     * Computes G*Wlambda, where W are the body velocities
     * @method computeGWlambda
     * @return {Number}
     */
    Equation.prototype.computeGWlambda = function(){
        var GA = this.jacobianElementA,
            GB = this.jacobianElementB,
            bi = this.bi,
            bj = this.bj,
            vi = bi.vlambda,
            vj = bj.vlambda,
            wi = bi.wlambda || zero,
            wj = bj.wlambda || zero;
        return GA.multiplyVectors(vi,wi) + GB.multiplyVectors(vj,wj);
    };

    /**
     * Computes G*inv(M)*f, where M is the mass matrix with diagonal blocks for each body, and f are the forces on the bodies.
     * @method computeGiMf
     * @return {Number}
     */
    var iMfi = new Vec3(),
        iMfj = new Vec3(),
        invIi_vmult_taui = new Vec3(),
        invIj_vmult_tauj = new Vec3();
    Equation.prototype.computeGiMf = function(){
        var GA = this.jacobianElementA,
            GB = this.jacobianElementB,
            bi = this.bi,
            bj = this.bj,
            fi = bi.force,
            ti = bi.tau,
            fj = bj.force,
            tj = bj.tau,
            invMassi = bi.invMassSolve,
            invMassj = bj.invMassSolve;

        if(bi.invInertiaWorldSolve){ bi.invInertiaWorldSolve.vmult(ti,invIi_vmult_taui); }
        else { invIi_vmult_taui.set(0,0,0); }
        if(bj.invInertiaWorldSolve){ bj.invInertiaWorldSolve.vmult(tj,invIj_vmult_tauj); }
        else { invIj_vmult_tauj.set(0,0,0); }

        fi.mult(invMassi,iMfi);
        fj.mult(invMassj,iMfj);

        return GA.multiplyVectors(iMfi,invIi_vmult_taui) + GB.multiplyVectors(iMfj,invIj_vmult_tauj);
    };

    /**
     * Computes G*inv(M)*G'
     * @method computeGiMGt
     * @return {Number}
     */
    var tmp = new Vec3();
    Equation.prototype.computeGiMGt = function(){
        var GA = this.jacobianElementA,
            GB = this.jacobianElementB,
            bi = this.bi,
            bj = this.bj,
            invMassi = bi.invMassSolve,
            invMassj = bj.invMassSolve,
            invIi = bi.invInertiaWorldSolve,
            invIj = bj.invInertiaWorldSolve,
            result = invMassi + invMassj;

        if(invIi){
            invIi.vmult(GA.rotational,tmp);
            result += tmp.dot(GA.rotational);
        }

        if(invIj){
            invIj.vmult(GB.rotational,tmp);
            result += tmp.dot(GB.rotational);
        }

        return  result;
    };

    var addToWlambda_temp = new Vec3(),
        addToWlambda_Gi = new Vec3(),
        addToWlambda_Gj = new Vec3(),
        addToWlambda_ri = new Vec3(),
        addToWlambda_rj = new Vec3(),
        addToWlambda_Mdiag = new Vec3();

    /**
     * Add constraint velocity to the bodies.
     * @method addToWlambda
     * @param {Number} deltalambda
     */
    Equation.prototype.addToWlambda = function(deltalambda){
        var GA = this.jacobianElementA,
            GB = this.jacobianElementB,
            bi = this.bi,
            bj = this.bj,
            temp = addToWlambda_temp;

        // Add to linear velocity
        // v_lambda += inv(M) * delta_lamba * G
        GA.spatial.mult(bi.invMassSolve * deltalambda,temp);
        bi.vlambda.vadd(temp, bi.vlambda);

        GB.spatial.mult(bj.invMassSolve * deltalambda,temp);
        bj.vlambda.vadd(temp, bj.vlambda);

        // Add to angular velocity
        if(bi.invInertiaWorldSolve){
            bi.invInertiaWorldSolve.vmult(GA.rotational,temp);
            temp.mult(deltalambda,temp);
            bi.wlambda.vadd(temp,bi.wlambda);
        }

        if(bj.invInertiaWorldSolve){
            bj.invInertiaWorldSolve.vmult(GB.rotational,temp);
            temp.mult(deltalambda,temp);
            bj.wlambda.vadd(temp,bj.wlambda);
        }
    };

    /**
     * Compute the denominator part of the SPOOK equation: C = G*inv(M)*G' + eps
     * @method computeInvC
     * @param  {Number} eps
     * @return {Number}
     */
    Equation.prototype.computeC = function(){
        return this.computeGiMGt() + this.eps;
    };