planck-js/lib/Rope.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.
 */

function RopeDef() {
  this.vertices = [];
  this.count = 0;
  this.masses = [];
  this.gravity = Vec2().SetZero();
  this.damping = 0.1;
  // Stretching stiffness
  this.k2 = 0.9;
  // Bending stiffness. Values above 0.5 can make the simulation blow up.
  this.k3 = 0.1;
};

function Rope() {

  // int32
  // this.m_count;
  // Vec2 * this.m_ps;
  // Vec2 * this.m_p0s;
  // Vec2 * this.m_vs;
  //
  // float * this.m_ims;
  //
  // float * this.m_Ls;
  // float * this.m_as;

  this.m_gravity;// Vec2
  this.m_damping;

  this.m_count = 0;
  this.m_ps = null;
  this.m_p0s = null;
  this.m_vs = null;
  this.m_ims = null;
  this.m_Ls = null;
  this.m_as = null;
  this.m_gravity.SetZero();
  this.m_k2 = 1.0;
  this.m_k3 = 0.1;
}

Rope.prototype.Initialize = function(def) {
  Assert(def.count >= 3);
  this.m_count = def.count;
  this.m_ps = [];// Vec2[m_count]
  this.m_p0s = [];// Vec2[m_count]
  this.m_vs = [];// Vec2[m_count]
  this.m_ims = [];// float[m_count]

  for (var i = 0; i < this.m_count; ++i) {
    this.m_ps[i] = def.vertices[i];
    this.m_p0s[i] = def.vertices[i];
    this.m_vs[i].SetZero();

    var m = def.masses[i];// float
    if (m > 0.0) {
      this.m_ims[i] = 1.0 / m;
    } else {
      this.m_ims[i] = 0.0;
    }
  }

  var count2 = this.m_count - 1;
  var count3 = this.m_count - 2;
  this.m_Ls = [];// float[count2]
  this.m_as = [];// float[count3]

  for (var i = 0; i < count2; ++i) {
    var p1 = this.m_ps[i];
    var p2 = this.m_ps[i + 1];
    this.m_Ls[i] = Vec2.Distance(p1, p2);
  }

  for (var i = 0; i < count3; ++i) {
    var p1 = this.m_ps[i];
    var p2 = this.m_ps[i + 1];
    var p3 = this.m_ps[i + 2];

    var d1 = Sub(p2, p1);
    var d2 = Sub(p3, p2);

    var a = Cross(d1, d2);
    var b = Dot(d1, d2);

    this.m_as[i] = Atan2(a, b);
  }

  this.m_gravity = def.gravity;
  this.m_damping = def.damping;
  this.m_k2 = def.k2;
  this.m_k3 = def.k3;
}

Rope.prototype.GetVertexCount = function() {
  return this.m_count;
}

Rope.prototype.GetVertices = function() {
  return this.m_ps;
}

// h: timeStep
Rope.prototype.Step = function(h, iterations) {
  if (h == 0.0) {
    return;
  }

  var d = expf(-h * this.m_damping);

  for (var i = 0; i < this.m_count; ++i) {
    this.m_p0s[i] = this.m_ps[i];
    if (this.m_ims[i] > 0.0) {
      this.m_vs[i] += h * this.m_gravity;
    }
    this.m_vs[i] *= d;
    this.m_ps[i] += h * this.m_vs[i];

  }

  for (var i = 0; i < iterations; ++i) {
    SolveC2();
    SolveC3();
    SolveC2();
  }

  var inv_h = 1.0 / h;
  for (var i = 0; i < this.m_count; ++i) {
    this.m_vs[i] = inv_h * (this.m_ps[i] - this.m_p0s[i]);
  }
}

Rope.prototype.SolveC2 = function() {
  var count2 = this.m_count - 1;

  for (var i = 0; i < count2; ++i) {
    var p1 = this.m_ps[i]; // Vec2
    var p2 = this.m_ps[i + 1]; // Vec2

    var d = p2 - p1; // Vec2
    var L = d.Normalize();

    var im1 = this.m_ims[i];
    var im2 = this.m_ims[i + 1];

    if (im1 + im2 == 0.0) {
      continue;
    }

    var s1 = im1 / (im1 + im2);
    var s2 = im2 / (im1 + im2);

    p1 -= this.m_k2 * s1 * (this.m_Ls[i] - L) * d;
    p2 += this.m_k2 * s2 * (this.m_Ls[i] - L) * d;

    this.m_ps[i] = p1;
    this.m_ps[i + 1] = p2;
  }
}

Rope.prototype.SetAngle = function(angle) {
  var count3 = this.m_count - 2;
  for (var i = 0; i < count3; ++i) {
    this.m_as[i] = angle;
  }
}

Rope.prototype.SolveC3 = function() {
  var count3 = this.m_count - 2;

  for (var i = 0; i < count3; ++i) {
    var p1 = this.m_ps[i]; // Vec2
    var p2 = this.m_ps[i + 1]; // Vec2
    var p3 = this.m_ps[i + 2]; // Vec2

    var m1 = this.m_ims[i];
    var m2 = this.m_ims[i + 1];
    var m3 = this.m_ims[i + 2];

    var d1 = p2 - p1;
    var d2 = p3 - p2;

    var L1sqr = d1.LengthSquared();
    var L2sqr = d2.LengthSquared();

    if (L1sqr * L2sqr == 0.0) {
      continue;
    }

    var a = Cross(d1, d2);
    var b = Dot(d1, d2);

    var angle = Atan2(a, b);

    var Jd1 = (-1.0 / L1sqr) * d1.Skew(); // Vec2
    var Jd2 = (1.0 / L2sqr) * d2.Skew(); // Vec2

    var J1 = -Jd1;
    var J2 = Jd1 - Jd2;
    var J3 = Jd2;

    var mass = m1 * Dot(J1, J1) + m2 * Dot(J2, J2) + m3 * Dot(J3, J3);
    if (mass == 0.0) {
      continue;
    }

    mass = 1.0 / mass;

    var C = angle - this.m_as[i];

    while (C > Math.PI) {
      angle -= 2 * Math.PI;
      C = angle - this.m_as[i];
    }

    while (C < -Math.PI) {
      angle += 2.0 * Math.PI;
      C = angle - this.m_as[i];
    }

    var impulse = -this.m_k3 * mass * C;

    p1 += (m1 * impulse) * J1;
    p2 += (m2 * impulse) * J2;
    p3 += (m3 * impulse) * J3;

    this.m_ps[i] = p1;
    this.m_ps[i + 1] = p2;
    this.m_ps[i + 2] = p3;
  }
}