planck-js/lib/joint/WeldJoint.js

2D physics engine for JavaScript/HTML5 game development

/*
 * Copyright (c) 2016      Ali Shakiba http://shakiba.me/planck.js
 * Copyright (c) 2006-2011 Erin Catto  http://www.box2d.org
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

module.exports = WeldJoint;

var options = require('../util/options');
var create = require('../util/create');
var Settings = require('../Settings');

var Math = require('../common/Math');
var Vec2 = require('../common/Vec2');
var Vec3 = require('../common/Vec3');
var Mat22 = require('../common/Mat22');
var Mat33 = require('../common/Mat33');
var Rot = require('../common/Rot');
var Sweep = require('../common/Sweep');
var Transform = require('../common/Transform');
var Velocity = require('../common/Velocity');
var Position = require('../common/Position');

var Joint = require('../Joint');

WeldJoint.TYPE = 'weld-joint';

WeldJoint._super = Joint;
WeldJoint.prototype = create(WeldJoint._super.prototype);

/**
 * Weld joint definition. You need to specify local anchor points where they are
 * attached and the relative body angle. The position of the anchor points is
 * important for computing the reaction torque.
 * 
 * @prop {float} frequencyHz The mass-spring-damper frequency in Hertz. Rotation
 *       only. Disable softness with a value of 0.
 * @prop {float} dampingRatio The damping ratio. 0 = no damping, 1 = critical
 *       damping.
 */
var WeldJointDef = {
  frequencyHz : 0.0,
  dampingRatio : 0.0
}

/**
 * A weld joint essentially glues two bodies together. A weld joint may distort
 * somewhat because the island constraint solver is approximate.
 * 
 * @prop {Vec2} localAnchorA The local anchor point relative to bodyA's origin.
 * @prop {Vec2} localAnchorB The local anchor point relative to bodyB's origin.
 * @prop {float} referenceAngle The bodyB angle minus bodyA angle in the
 *       reference state (radians).
 */
function WeldJoint(def, bodyA, bodyB, anchor) {
  if (!(this instanceof WeldJoint)) {
    return new WeldJoint(def, bodyA, bodyB, anchor);
  }

  def = options(def, WeldJointDef);
  Joint.call(this, def, bodyA, bodyB);
  this.m_type = WeldJoint.TYPE;

  this.m_localAnchorA = bodyA.GetLocalPoint(anchor);
  this.m_localAnchorB = bodyB.GetLocalPoint(anchor);
  this.m_referenceAngle = bodyB.GetAngle() - bodyA.GetAngle();

  this.m_frequencyHz = def.frequencyHz;
  this.m_dampingRatio = def.dampingRatio;

  this.m_impulse = Vec3();

  this.m_bias = 0.0;
  this.m_gamma = 0.0;

  // Solver temp
  this.m_rA; // Vec2
  this.m_rB; // Vec2
  this.m_localCenterA; // Vec2
  this.m_localCenterB; // Vec2
  this.m_invMassA; // float
  this.m_invMassB; // float
  this.m_invIA; // float
  this.m_invIB; // float
  this.m_mass = new Mat33();

  // Point-to-point constraint
  // C = p2 - p1
  // Cdot = v2 - v1
  // / = v2 + cross(w2, r2) - v1 - cross(w1, r1)
  // J = [-I -r1_skew I r2_skew ]
  // Identity used:
  // w k % (rx i + ry j) = w * (-ry i + rx j)

  // Angle constraint
  // C = angle2 - angle1 - referenceAngle
  // Cdot = w2 - w1
  // J = [0 0 -1 0 0 1]
  // K = invI1 + invI2
}

/**
 * The local anchor point relative to bodyA's origin.
 */
WeldJoint.prototype.GetLocalAnchorA = function() {
  return this.m_localAnchorA;
};

/**
 * The local anchor point relative to bodyB's origin.
 */
WeldJoint.prototype.GetLocalAnchorB = function() {
  return this.m_localAnchorB;
};

/**
 * Get the reference angle.
 */
WeldJoint.prototype.GetReferenceAngle = function() {
  return this.m_referenceAngle;
};

/**
 * Set/get frequency in Hz.
 */
WeldJoint.prototype.SetFrequency = function(hz) {
  this.m_frequencyHz = hz;
};

WeldJoint.prototype.GetFrequency = function() {
  return this.m_frequencyHz;
};

/**
 * Set/get damping ratio.
 */
WeldJoint.prototype.SetDampingRatio = function(ratio) {
  this.m_dampingRatio = ratio;
};

WeldJoint.prototype.GetDampingRatio = function() {
  return this.m_dampingRatio;
};

WeldJoint.prototype.GetAnchorA = function() {
  return this.m_bodyA.GetWorldPoint(this.m_localAnchorA);
};

WeldJoint.prototype.GetAnchorB = function() {
  return this.m_bodyB.GetWorldPoint(this.m_localAnchorB);
};

WeldJoint.prototype.GetReactionForce = function(inv_dt) {
  var P = Vec2(this.m_impulse.x, this.m_impulse.y);
  return inv_dt * P;
};

WeldJoint.prototype.GetReactionTorque = function(inv_dt) {
  return inv_dt * this.m_impulse.z;
};

WeldJoint.prototype.InitVelocityConstraints = function(step) {
  this.m_localCenterA = this.m_bodyA.m_sweep.localCenter;
  this.m_localCenterB = this.m_bodyB.m_sweep.localCenter;
  this.m_invMassA = this.m_bodyA.m_invMass;
  this.m_invMassB = this.m_bodyB.m_invMass;
  this.m_invIA = this.m_bodyA.m_invI;
  this.m_invIB = this.m_bodyB.m_invI;

  var aA = this.m_bodyA.c_position.a;
  var vA = this.m_bodyA.c_velocity.v;
  var wA = this.m_bodyA.c_velocity.w;

  var aB = this.m_bodyB.c_position.a;
  var vB = this.m_bodyB.c_velocity.v;
  var wB = this.m_bodyB.c_velocity.w;

  var qA = Rot(aA), qB = Rot(aB);

  this.m_rA = Rot.Mul(qA, Vec2.Sub(this.m_localAnchorA, this.m_localCenterA));
  this.m_rB = Rot.Mul(qB, Vec2.Sub(this.m_localAnchorB, this.m_localCenterB));

  // J = [-I -r1_skew I r2_skew]
  // [ 0 -1 0 1]
  // r_skew = [-ry; rx]

  // Matlab
  // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
  // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
  // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]

  var mA = this.m_invMassA;
  var mB = this.m_invMassB; // float
  var iA = this.m_invIA;
  var iB = this.m_invIB; // float

  var K = new Mat33();
  K.ex.x = mA + mB + this.m_rA.y * this.m_rA.y * iA + this.m_rB.y * this.m_rB.y
      * iB;
  K.ey.x = -this.m_rA.y * this.m_rA.x * iA - this.m_rB.y * this.m_rB.x * iB;
  K.ez.x = -this.m_rA.y * iA - this.m_rB.y * iB;
  K.ex.y = K.ey.x;
  K.ey.y = mA + mB + this.m_rA.x * this.m_rA.x * iA + this.m_rB.x * this.m_rB.x
      * iB;
  K.ez.y = this.m_rA.x * iA + this.m_rB.x * iB;
  K.ex.z = K.ez.x;
  K.ey.z = K.ez.y;
  K.ez.z = iA + iB;

  if (this.m_frequencyHz > 0.0) {
    K.GetInverse22(this.m_mass);

    var invM = iA + iB; // float
    var m = invM > 0.0 ? 1.0 / invM : 0.0; // float

    var C = aB - aA - this.m_referenceAngle; // float

    // Frequency
    var omega = 2.0 * Math.PI * this.m_frequencyHz; // float

    // Damping coefficient
    var d = 2.0 * m * this.m_dampingRatio * omega; // float

    // Spring stiffness
    var k = m * omega * omega; // float

    // magic formulas
    var h = step.dt; // float
    this.m_gamma = h * (d + h * k);
    this.m_gamma = this.m_gamma != 0.0 ? 1.0 / this.m_gamma : 0.0;
    this.m_bias = C * h * k * this.m_gamma;

    invM += this.m_gamma;
    this.m_mass.ez.z = invM != 0.0 ? 1.0 / invM : 0.0;
  } else if (K.ez.z == 0.0) {
    K.GetInverse22(this.m_mass);
    this.m_gamma = 0.0;
    this.m_bias = 0.0;
  } else {
    K.GetSymInverse33(this.m_mass);
    this.m_gamma = 0.0;
    this.m_bias = 0.0;
  }

  if (step.warmStarting) {
    // Scale impulses to support a variable time step.
    this.m_impulse *= step.dtRatio;

    var P = Vec2(this.m_impulse.x, this.m_impulse.y);

    vA.WSub(mA, P);
    wA -= iA * (Vec2.Cross(this.m_rA, P) + this.m_impulse.z);

    vB.WAdd(mB, P);
    wB += iB * (Vec2.Cross(this.m_rB, P) + this.m_impulse.z);

  } else {
    this.m_impulse.SetZero();
  }

  this.m_bodyA.c_velocity.v = vA;
  this.m_bodyA.c_velocity.w = wA;
  this.m_bodyB.c_velocity.v = vB;
  this.m_bodyB.c_velocity.w = wB;
}

WeldJoint.prototype.SolveVelocityConstraints = function(step) {
  var vA = this.m_bodyA.c_velocity.v;
  var wA = this.m_bodyA.c_velocity.w;
  var vB = this.m_bodyB.c_velocity.v;
  var wB = this.m_bodyB.c_velocity.w;

  var mA = this.m_invMassA;
  var mB = this.m_invMassB; // float
  var iA = this.m_invIA;
  var iB = this.m_invIB; // float

  if (this.m_frequencyHz > 0.0) {
    var Cdot2 = wB - wA; // float

    var impulse2 = -this.m_mass.ez.z
        * (Cdot2 + this.m_bias + this.m_gamma * this.m_impulse.z); // float
    this.m_impulse.z += impulse2;

    wA -= iA * impulse2;
    wB += iB * impulse2;

    var Cdot1 = Vec2();
    Cdot1.WAdd(1, vB, 1, Vec2.Cross(wB, this.m_rB));
    Cdot1.WSub(1, vA, 1, Vec2.Cross(wA, this.m_rA)); // Vec2

    var impulse1 = Vec2.Neg(Mat33.Mul(this.m_mass, Cdot1)); // Vec2
    this.m_impulse.x += impulse1.x;
    this.m_impulse.y += impulse1.y;

    var P = Vec2(impulse1); // Vec2

    vA.WSub(mA, P);
    wA -= iA * Vec2.Cross(this.m_rA, P);

    vB.WAdd(mB, P);
    wB += iB * Vec2.Cross(this.m_rB, P);
  } else {
    var Cdot1 = Vec2();
    Cdot1.WAdd(1, vB, 1, Vec2.Cross(wB, this.m_rB));
    Cdot1.WSub(1, vA, 1, Vec2.Cross(wA, this.m_rA)); // Vec2
    var Cdot2 = wB - wA; // float
    var Cdot = Vec3(Cdot1.x, Cdot1.y, Cdot2); // Vec3

    var impulse = Vec3.Neg(Mat33.Mul(this.m_mass, Cdot)); // Vec3
    this.m_impulse.Add(impulse);

    var P = Vec2(impulse.x, impulse.y);

    vA.WSub(mA, P);
    wA -= iA * (Vec2.Cross(this.m_rA, P) + impulse.z);

    vB.WAdd(mB, P);
    wB += iB * (Vec2.Cross(this.m_rB, P) + impulse.z);
  }

  this.m_bodyA.c_velocity.v = vA;
  this.m_bodyA.c_velocity.w = wA;
  this.m_bodyB.c_velocity.v = vB;
  this.m_bodyB.c_velocity.w = wB;
}

WeldJoint.prototype.SolvePositionConstraints = function(step) {
  var cA = this.m_bodyA.c_position.c;
  var aA = this.m_bodyA.c_position.a;
  var cB = this.m_bodyB.c_position.c;
  var aB = this.m_bodyB.c_position.a;

  var qA = Rot(aA), qB = Rot(aB);

  var mA = this.m_invMassA, mB = this.m_invMassB; // float
  var iA = this.m_invIA, iB = this.m_invIB; // float

  var rA = Rot.Mul(qA, Vec2.Sub(this.m_localAnchorA, this.m_localCenterA));
  var rB = Rot.Mul(qB, Vec2.Sub(this.m_localAnchorB, this.m_localCenterB));

  var positionError, angularError; // float

  var K = new Mat33();
  K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
  K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
  K.ez.x = -rA.y * iA - rB.y * iB;
  K.ex.y = K.ey.x;
  K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
  K.ez.y = rA.x * iA + rB.x * iB;
  K.ex.z = K.ez.x;
  K.ey.z = K.ez.y;
  K.ez.z = iA + iB;

  if (this.m_frequencyHz > 0.0) {
    var C1 = Vec2();
    C1.WAdd(1, cB, 1, rB);
    C1.WSub(1, cA, 1, rA); // Vec2

    positionError = C1.Length();
    angularError = 0.0;

    var P = Vec2.Neg(K.Solve22(C1)); // Vec2

    cA.WSub(mA, P);
    aA -= iA * Vec2.Cross(rA, P);

    cB.WAdd(mB, P);
    aB += iB * Vec2.Cross(rB, P);
  } else {
    var C1 = Vec2();
    C1.WAdd(1, cB, 1, rB);
    C1.WSub(1, cA, 1, rA);

    var C2 = aB - aA - this.m_referenceAngle; // float

    positionError = C1.Length();
    angularError = Math.abs(C2);

    var C = Vec3(C1.x, C1.y, C2);

    var impulse = Vec3();
    if (K.ez.z > 0.0) {
      impulse = Vec3.Neg(K.Solve33(C));
    } else {
      var impulse2 = Vec2.Neg(K.Solve22(C1));
      impulse.Set(impulse2.x, impulse2.y, 0.0);
    }

    var P = Vec2(impulse.x, impulse.y);

    cA.WSub(mA, P);
    aA -= iA * (Vec2.Cross(rA, P) + impulse.z);

    cB.WAdd(mB, P);
    aB += iB * (Vec2.Cross(rB, P) + impulse.z);
  }

  this.m_bodyA.c_position.c = cA;
  this.m_bodyA.c_position.a = aA;
  this.m_bodyB.c_position.c = cB;
  this.m_bodyB.c_position.a = aB;

  return positionError <= Settings.linearSlop
      && angularError <= Settings.angularSlop;
}