API Docs for: 0.3.0
Show:

File: src/constraints/ContactEquation.js

var Equation = require("./Equation"),
    vec2 = require('../math/vec2'),
    mat2 = require('../math/mat2');

module.exports = ContactEquation;

/**
 * Non-penetration constraint equation.
 *
 * @class ContactEquation
 * @constructor
 * @extends Equation
 * @param {Body} bi
 * @param {Body} bj
 */
function ContactEquation(bi,bj){
    Equation.call(this,bi,bj,0,1e6);
    this.ri = vec2.create();
    this.penetrationVec = vec2.create();
    this.rj = vec2.create();
    this.ni = vec2.create();
    this.rixn = 0;
    this.rjxn = 0;
};
ContactEquation.prototype = new Equation();
ContactEquation.prototype.constructor = ContactEquation;
ContactEquation.prototype.computeB = function(a,b,h){
    var bi = this.bi,
        bj = this.bj,
        ri = this.ri,
        rj = this.rj,
        xi = bi.position,
        xj = bj.position;

    var vi = bi.velocity,
        wi = bi.angularVelocity,
        fi = bi.force,
        taui = bi.angularForce;

    var vj = bj.velocity,
        wj = bj.angularVelocity,
        fj = bj.force,
        tauj = bj.angularForce;

    var penetrationVec = this.penetrationVec,
        invMassi = bi.invMass,
        invMassj = bj.invMass,
        invIi = bi.invInertia,
        invIj = bj.invInertia,
        n = this.ni;

    // Caluclate cross products
    this.rixn = vec2.crossLength(ri,n);
    this.rjxn = vec2.crossLength(rj,n);

    // Calculate q = xj+rj -(xi+ri) i.e. the penetration vector
    vec2.add(penetrationVec,xj,rj);
    vec2.sub(penetrationVec,penetrationVec,xi);
    vec2.sub(penetrationVec,penetrationVec,ri);

    var Gq = vec2.dot(n,penetrationVec);

    // Compute iteration
    var GW = vec2.dot(vj,n) - vec2.dot(vi,n) + wj * this.rjxn - wi * this.rixn;
    var GiMf = vec2.dot(fj,n)*invMassj - vec2.dot(fi,n)*invMassi + invIj*tauj*this.rjxn - invIi*taui*this.rixn;

    var B = - Gq * a - GW * b - h*GiMf;

    return B;
};

// Compute C = GMG+eps in the SPOOK equation
var computeC_tmp1 = vec2.create(),
    tmpMat1 = mat2.create(),
    tmpMat2 = mat2.create();
ContactEquation.prototype.computeC = function(eps){
    var bi = this.bi,
        bj = this.bj,
        n = this.ni,
        rixn = this.rixn,
        rjxn = this.rjxn,
        tmp = computeC_tmp1,
        imMat1 = tmpMat1,
        imMat2 = tmpMat2;

    mat2.identity(imMat1);
    mat2.identity(imMat2);
    imMat1[0] = imMat1[3] = bi.invMass;
    imMat2[0] = imMat2[3] = bj.invMass;

    var C = vec2.dot(n,vec2.transformMat2(tmp,n,imMat1)) + vec2.dot(n,vec2.transformMat2(tmp,n,imMat2)) + eps;
    //var C = bi.invMass + bj.invMass + eps;

    C += bi.invInertia * this.rixn * this.rixn;
    C += bj.invInertia * this.rjxn * this.rjxn;

    return C;
};

ContactEquation.prototype.computeGWlambda = function(){
    var bi = this.bi,
        bj = this.bj,
        n = this.ni,
        dot = vec2.dot;

    return dot(n, bj.vlambda) + bj.wlambda * this.rjxn - dot(n, bi.vlambda) - bi.wlambda * this.rixn;
};

var addToWlambda_temp = vec2.create();
ContactEquation.prototype.addToWlambda = function(deltalambda){
    var bi = this.bi,
        bj = this.bj,
        n = this.ni,
        temp = addToWlambda_temp,
        imMat1 = tmpMat1,
        imMat2 = tmpMat2;

    mat2.identity(imMat1);
    mat2.identity(imMat2);
    imMat1[0] = imMat1[3] = bi.invMass;
    imMat2[0] = imMat2[3] = bj.invMass;

    // Add to linear velocity
    //vec2.scale(temp,n,-bi.invMass*deltalambda);
    vec2.scale(temp,vec2.transformMat2(temp,n,imMat1),-deltalambda);
    vec2.add( bi.vlambda,bi.vlambda, temp );

    //vec2.scale(temp,n,bj.invMass*deltalambda);
    vec2.scale(temp,vec2.transformMat2(temp,n,imMat2),deltalambda);
    vec2.add( bj.vlambda,bj.vlambda, temp);

    // Add to angular velocity
    bi.wlambda -= bi.invInertia * this.rixn * deltalambda;
    bj.wlambda += bj.invInertia * this.rjxn * deltalambda;
};