planck-js/lib/joint/DistanceJoint.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 = DistanceJoint;

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');

DistanceJoint.TYPE = 'distance-joint';

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

/**
 * Distance joint definition. This requires defining an anchor point on both
 * bodies and the non-zero length of the distance joint. The definition uses
 * local anchor points so that the initial configuration can violate the
 * constraint slightly. This helps when saving and loading a game. Warning: Do
 * not use a zero or short length.
 * 
 * @prop {float} frequencyHz The mass-spring-damper frequency in Hertz. A value
 *       of 0 disables softness.
 * @prop {float} dampingRatio The damping ratio. 0 = no damping, 1 = critical
 *       damping.
 */
var DistanceJointDef = {
  frequencyHz : 0.0,
  dampingRatio : 0.0
};

/**
 * A distance joint constrains two points on two bodies to remain at a fixed
 * distance from each other. You can view this as a massless, rigid rod.
 * 
 * @param {Vec2} localAnchorA The local anchor point relative to bodyA's origin.
 * @param {Vec2} localAnchorB The local anchor point relative to bodyB's origin.
 */
function DistanceJoint(def, bodyA, anchorA, bodyB, anchorB) {
  if (!(this instanceof DistanceJoint)) {
    return new DistanceJoint(def, bodyA, anchorA, bodyB, anchorB);
  }
  
  def = options(def, DistanceJointDef);
  Joint.call(this, def, bodyA, bodyB);
  this.m_type = DistanceJoint.TYPE;

  // Solver shared
  this.m_localAnchorA = bodyA.GetLocalPoint(anchorA);
  this.m_localAnchorB = bodyB.GetLocalPoint(anchorB);
  this.m_length = Vec2.Distance(anchorB, anchorA);
  this.m_frequencyHz = def.frequencyHz;
  this.m_dampingRatio = def.dampingRatio;
  this.m_impulse = 0.0;
  this.m_gamma = 0.0;
  this.m_bias = 0.0;

  // Solver temp
  this.m_u; // Vec2
  this.m_rA; // Vec2
  this.m_rB; // Vec2
  this.m_localCenterA; // Vec2
  this.m_localCenterB; // Vec2
  this.m_invMassA;
  this.m_invMassB;
  this.m_invIA;
  this.m_invIB;
  this.m_mass;

  // 1-D constrained system
  // m (v2 - v1) = lambda
  // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
  // x2 = x1 + h * v2

  // 1-D mass-damper-spring system
  // m (v2 - v1) + h * d * v2 + h * k *

  // C = norm(p2 - p1) - L
  // u = (p2 - p1) / norm(p2 - p1)
  // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
  // J = [-u -cross(r1, u) u cross(r2, u)]
  // K = J * invM * JT
  // = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
};

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

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

/**
 * Set/get the natural length. Manipulating the length can lead to non-physical
 * behavior when the frequency is zero.
 */
DistanceJoint.prototype.SetLength = function(length) {
  this.m_length = length;
}

DistanceJoint.prototype.GetLength = function() {
  return this.m_length;
}

DistanceJoint.prototype.SetFrequency = function(hz) {
  this.m_frequencyHz = hz;
}

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

DistanceJoint.prototype.SetDampingRatio = function(ratio) {
  this.m_dampingRatio = ratio;
}

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

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

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

DistanceJoint.prototype.GetReactionForce = function(inv_dt) {
  var F = Vec2.Mul(inv_dt * this.m_impulse, this.m_u);
  return F;
}

DistanceJoint.prototype.GetReactionTorque = function(inv_dt) {
  return 0.0;
}

DistanceJoint.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 cA = this.m_bodyA.c_position.c;
  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 cB = this.m_bodyB.c_position.c;
  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);
  var 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));
  this.m_u = Vec2.Sub(Vec2.Add(cB, this.m_rB), Vec2.Add(cA, this.m_rA));

  // Handle singularity.
  var length = this.m_u.Length();
  if (length > Settings.linearSlop) {
    this.m_u.Mul(1.0 / length);
  } else {
    this.m_u.Set(0.0, 0.0);
  }

  var crAu = Vec2.Cross(this.m_rA, this.m_u);
  var crBu = Vec2.Cross(this.m_rB, this.m_u);
  var invMass = this.m_invMassA + this.m_invIA * crAu * crAu + this.m_invMassB
      + this.m_invIB * crBu * crBu;

  // Compute the effective mass matrix.
  this.m_mass = invMass != 0.0 ? 1.0 / invMass : 0.0;

  if (this.m_frequencyHz > 0.0) {
    var C = length - this.m_length;

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

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

    // Spring stiffness
    var k = this.m_mass * omega * omega;

    // magic formulas
    var h = step.dt;
    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;

    invMass += this.m_gamma;
    this.m_mass = invMass != 0.0 ? 1.0 / invMass : 0.0;
  } else {
    this.m_gamma = 0.0;
    this.m_bias = 0.0;
  }

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

    var P = Vec2.Mul(this.m_impulse, this.m_u);
    vA.WSub(this.m_invMassA, P);
    wA -= this.m_invIA * Cross(this.m_rA, P);
    vB.WAdd(this.m_invMassB, P);
    wB += this.m_invIB * Vec2.Cross(this.m_rB, P);
  } else {
    this.m_impulse = 0.0;
  }

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

DistanceJoint.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;

  // Cdot = dot(u, v + cross(w, r))
  var vpA = Vec2.Add(vA, Vec2.Cross(wA, this.m_rA));
  var vpB = Vec2.Add(vB, Vec2.Cross(wB, this.m_rB));
  var Cdot = Vec2.Dot(this.m_u, vpB) - Vec2.Dot(this.m_u, vpA);

  var impulse = -this.m_mass
      * (Cdot + this.m_bias + this.m_gamma * this.m_impulse);
  this.m_impulse += impulse;

  var P = Vec2.Mul(impulse, this.m_u);
  vA.WSub(this.m_invMassA, P);
  wA -= this.m_invIA * Vec2.Cross(this.m_rA, P);
  vB.WAdd(this.m_invMassB, P);
  wB += this.m_invIB * Vec2.Cross(this.m_rB, P);

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

DistanceJoint.prototype.SolvePositionConstraints = function(step) {
  if (this.m_frequencyHz > 0.0) {
    // There is no position correction for soft distance constraints.
    return true;
  }

  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);
  var qB = Rot(aB);

  var rA = Rot.MulSub(qA, this.m_localAnchorA, this.m_localCenterA);
  var rB = Rot.MulSub(qB, this.m_localAnchorB, this.m_localCenterB);
  var u = Vec2.Sub(Vec2.Add(cB, rB), Vec2.Add(cA, rA));

  var length = u.Normalize();
  var C = length - this.m_length;
  C = Math
      .clamp(C, -Settings.maxLinearCorrection, Settings.maxLinearCorrection);

  var impulse = -this.m_mass * C;
  var P = Vec2.Mul(impulse, u);

  cA.WSub(this.m_invMassA, P);
  aA -= this.m_invIA * Vec2.Cross(rA, P);
  cB.WAdd(this.m_invMassB, P);
  aB += this.m_invIB * Vec2.Cross(rB, P);

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

  return Math.abs(C) < Settings.linearSlop;
}