@box2d/debug-draw
Version:
Debug drawing helper for @box2d
628 lines (627 loc) • 26.1 kB
JavaScript
"use strict";
// MIT License
Object.defineProperty(exports, "__esModule", { value: true });
exports.b2Rope = exports.b2RopeTuning = exports.b2BendingModel = exports.b2StretchingModel = void 0;
// Copyright (c) 2019 Erin Catto
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
const b2_common_1 = require("../common/b2_common");
const b2_draw_1 = require("../common/b2_draw");
const b2_math_1 = require("../common/b2_math");
const temp = {
J1: new b2_math_1.b2Vec2(),
J2: new b2_math_1.b2Vec2(),
J3: new b2_math_1.b2Vec2(),
r: new b2_math_1.b2Vec2(),
e1: new b2_math_1.b2Vec2(),
e2: new b2_math_1.b2Vec2(),
Jd1: new b2_math_1.b2Vec2(),
Jd2: new b2_math_1.b2Vec2(),
d: new b2_math_1.b2Vec2(),
u: new b2_math_1.b2Vec2(),
dp1: new b2_math_1.b2Vec2(),
dp2: new b2_math_1.b2Vec2(),
dp3: new b2_math_1.b2Vec2(),
d1: new b2_math_1.b2Vec2(),
d2: new b2_math_1.b2Vec2(),
dHat: new b2_math_1.b2Vec2(),
};
var b2StretchingModel;
(function (b2StretchingModel) {
b2StretchingModel[b2StretchingModel["b2_pbdStretchingModel"] = 0] = "b2_pbdStretchingModel";
b2StretchingModel[b2StretchingModel["b2_xpbdStretchingModel"] = 1] = "b2_xpbdStretchingModel";
})(b2StretchingModel || (exports.b2StretchingModel = b2StretchingModel = {}));
var b2BendingModel;
(function (b2BendingModel) {
b2BendingModel[b2BendingModel["b2_springAngleBendingModel"] = 0] = "b2_springAngleBendingModel";
b2BendingModel[b2BendingModel["b2_pbdAngleBendingModel"] = 1] = "b2_pbdAngleBendingModel";
b2BendingModel[b2BendingModel["b2_xpbdAngleBendingModel"] = 2] = "b2_xpbdAngleBendingModel";
b2BendingModel[b2BendingModel["b2_pbdDistanceBendingModel"] = 3] = "b2_pbdDistanceBendingModel";
b2BendingModel[b2BendingModel["b2_pbdHeightBendingModel"] = 4] = "b2_pbdHeightBendingModel";
b2BendingModel[b2BendingModel["b2_pbdTriangleBendingModel"] = 5] = "b2_pbdTriangleBendingModel";
})(b2BendingModel || (exports.b2BendingModel = b2BendingModel = {}));
class b2RopeTuning {
constructor() {
this.stretchingModel = b2StretchingModel.b2_pbdStretchingModel;
this.bendingModel = b2BendingModel.b2_pbdAngleBendingModel;
this.damping = 0;
this.stretchStiffness = 1;
this.stretchHertz = 1;
this.stretchDamping = 0;
this.bendStiffness = 0.5;
this.bendHertz = 1;
this.bendDamping = 0;
this.isometric = false;
this.fixedEffectiveMass = false;
this.warmStart = false;
}
Copy(other) {
this.stretchingModel = other.stretchingModel;
this.bendingModel = other.bendingModel;
this.damping = other.damping;
this.stretchStiffness = other.stretchStiffness;
this.stretchHertz = other.stretchHertz;
this.stretchDamping = other.stretchDamping;
this.bendStiffness = other.bendStiffness;
this.bendHertz = other.bendHertz;
this.bendDamping = other.bendDamping;
this.isometric = other.isometric;
this.fixedEffectiveMass = other.fixedEffectiveMass;
this.warmStart = other.warmStart;
return this;
}
}
exports.b2RopeTuning = b2RopeTuning;
class b2RopeStretch {
constructor() {
this.i1 = 0;
this.i2 = 0;
this.invMass1 = 0;
this.invMass2 = 0;
this.L = 0;
this.lambda = 0;
this.spring = 0;
this.damper = 0;
}
}
class b2RopeBend {
constructor() {
this.i1 = 0;
this.i2 = 0;
this.i3 = 0;
this.invMass1 = 0;
this.invMass2 = 0;
this.invMass3 = 0;
this.invEffectiveMass = 0;
this.lambda = 0;
this.L1 = 0;
this.L2 = 0;
this.alpha1 = 0;
this.alpha2 = 0;
this.spring = 0;
this.damper = 0;
}
}
class b2Rope {
constructor(def) {
this.m_position = new b2_math_1.b2Vec2();
this.m_count = 0;
this.m_stretchCount = 0;
this.m_bendCount = 0;
this.m_gravity = new b2_math_1.b2Vec2();
this.m_tuning = new b2RopeTuning();
(0, b2_common_1.b2Assert)(def.vertices.length >= 3);
this.m_position.Copy(def.position);
this.m_count = def.vertices.length;
this.m_bindPositions = (0, b2_common_1.b2MakeArray)(this.m_count, b2_math_1.b2Vec2);
this.m_ps = (0, b2_common_1.b2MakeArray)(this.m_count, b2_math_1.b2Vec2);
this.m_p0s = (0, b2_common_1.b2MakeArray)(this.m_count, b2_math_1.b2Vec2);
this.m_vs = (0, b2_common_1.b2MakeArray)(this.m_count, b2_math_1.b2Vec2);
this.m_invMasses = (0, b2_common_1.b2MakeNumberArray)(this.m_count);
for (let i = 0; i < this.m_count; ++i) {
this.m_bindPositions[i].Copy(def.vertices[i]);
b2_math_1.b2Vec2.Add(def.vertices[i], this.m_position, this.m_ps[i]);
b2_math_1.b2Vec2.Add(def.vertices[i], this.m_position, this.m_p0s[i]);
this.m_vs[i].SetZero();
const m = def.masses[i];
if (m > 0) {
this.m_invMasses[i] = 1 / m;
}
else {
this.m_invMasses[i] = 0;
}
}
this.m_stretchCount = this.m_count - 1;
this.m_bendCount = this.m_count - 2;
this.m_stretchConstraints = new Array(this.m_stretchCount);
for (let i = 0; i < this.m_stretchCount; i++)
this.m_stretchConstraints[i] = new b2RopeStretch();
this.m_bendConstraints = new Array(this.m_bendCount);
for (let i = 0; i < this.m_bendCount; i++)
this.m_bendConstraints[i] = new b2RopeBend();
for (let i = 0; i < this.m_stretchCount; ++i) {
const c = this.m_stretchConstraints[i];
const p1 = this.m_ps[i];
const p2 = this.m_ps[i + 1];
c.i1 = i;
c.i2 = i + 1;
c.L = b2_math_1.b2Vec2.Distance(p1, p2);
c.invMass1 = this.m_invMasses[i];
c.invMass2 = this.m_invMasses[i + 1];
c.lambda = 0;
c.damper = 0;
c.spring = 0;
}
const { J1, J2, r, e1, e2, Jd1, Jd2 } = temp;
for (let i = 0; i < this.m_bendCount; ++i) {
const c = this.m_bendConstraints[i];
const p1 = this.m_ps[i];
const p2 = this.m_ps[i + 1];
const p3 = this.m_ps[i + 2];
c.i1 = i;
c.i2 = i + 1;
c.i3 = i + 2;
c.invMass1 = this.m_invMasses[i];
c.invMass2 = this.m_invMasses[i + 1];
c.invMass3 = this.m_invMasses[i + 2];
c.invEffectiveMass = 0;
c.L1 = b2_math_1.b2Vec2.Distance(p1, p2);
c.L2 = b2_math_1.b2Vec2.Distance(p2, p3);
c.lambda = 0;
// Pre-compute effective mass (TODO use flattened config)
b2_math_1.b2Vec2.Subtract(p2, p1, e1);
b2_math_1.b2Vec2.Subtract(p3, p2, e2);
const L1sqr = e1.LengthSquared();
const L2sqr = e2.LengthSquared();
if (L1sqr * L2sqr === 0) {
continue;
}
b2_math_1.b2Vec2.Skew(e1, Jd1).Scale(-1 / L1sqr);
b2_math_1.b2Vec2.Skew(e2, Jd2).Scale(1 / L2sqr);
b2_math_1.b2Vec2.Negate(Jd1, J1);
b2_math_1.b2Vec2.Subtract(Jd1, Jd2, J2);
const J3 = Jd2;
c.invEffectiveMass =
c.invMass1 * b2_math_1.b2Vec2.Dot(J1, J1) + c.invMass2 * b2_math_1.b2Vec2.Dot(J2, J2) + c.invMass3 * b2_math_1.b2Vec2.Dot(J3, J3);
b2_math_1.b2Vec2.Subtract(p3, p1, r);
const rr = r.LengthSquared();
if (rr === 0) {
continue;
}
// a1 = h2 / (h1 + h2)
// a2 = h1 / (h1 + h2)
c.alpha1 = b2_math_1.b2Vec2.Dot(e2, r) / rr;
c.alpha2 = b2_math_1.b2Vec2.Dot(e1, r) / rr;
}
this.m_gravity.Copy(def.gravity);
this.SetTuning(def.tuning);
}
SetTuning(tuning) {
this.m_tuning.Copy(tuning);
// Pre-compute spring and damper values based on tuning
const bendOmega = 2 * Math.PI * this.m_tuning.bendHertz;
for (let i = 0; i < this.m_bendCount; ++i) {
const c = this.m_bendConstraints[i];
const L1sqr = c.L1 * c.L1;
const L2sqr = c.L2 * c.L2;
if (L1sqr * L2sqr === 0) {
c.spring = 0;
c.damper = 0;
continue;
}
// Flatten the triangle formed by the two edges
const J2 = 1 / c.L1 + 1 / c.L2;
const sum = c.invMass1 / L1sqr + c.invMass2 * J2 * J2 + c.invMass3 / L2sqr;
if (sum === 0) {
c.spring = 0;
c.damper = 0;
continue;
}
const mass = 1 / sum;
c.spring = mass * bendOmega * bendOmega;
c.damper = 2 * mass * this.m_tuning.bendDamping * bendOmega;
}
const stretchOmega = 2 * Math.PI * this.m_tuning.stretchHertz;
for (let i = 0; i < this.m_stretchCount; ++i) {
const c = this.m_stretchConstraints[i];
const sum = c.invMass1 + c.invMass2;
if (sum === 0) {
continue;
}
const mass = 1 / sum;
c.spring = mass * stretchOmega * stretchOmega;
c.damper = 2 * mass * this.m_tuning.stretchDamping * stretchOmega;
}
}
Step(dt, iterations, position) {
if (dt === 0) {
return;
}
const inv_dt = 1 / dt;
const d = Math.exp(-dt * this.m_tuning.damping);
// Apply gravity and damping
for (let i = 0; i < this.m_count; ++i) {
if (this.m_invMasses[i] > 0) {
this.m_vs[i].Scale(d);
this.m_vs[i].AddScaled(dt, this.m_gravity);
}
else {
this.m_vs[i].x = inv_dt * (this.m_bindPositions[i].x + position.x - this.m_p0s[i].x);
this.m_vs[i].y = inv_dt * (this.m_bindPositions[i].y + position.y - this.m_p0s[i].y);
}
}
// Apply bending spring
if (this.m_tuning.bendingModel === b2BendingModel.b2_springAngleBendingModel) {
this.ApplyBendForces(dt);
}
for (let i = 0; i < this.m_bendCount; ++i) {
this.m_bendConstraints[i].lambda = 0;
}
for (let i = 0; i < this.m_stretchCount; ++i) {
this.m_stretchConstraints[i].lambda = 0;
}
// Update position
for (let i = 0; i < this.m_count; ++i) {
this.m_ps[i].AddScaled(dt, this.m_vs[i]);
}
// Solve constraints
for (let i = 0; i < iterations; ++i) {
if (this.m_tuning.bendingModel === b2BendingModel.b2_pbdAngleBendingModel) {
this.SolveBend_PBD_Angle();
}
else if (this.m_tuning.bendingModel === b2BendingModel.b2_xpbdAngleBendingModel) {
this.SolveBend_XPBD_Angle(dt);
}
else if (this.m_tuning.bendingModel === b2BendingModel.b2_pbdDistanceBendingModel) {
this.SolveBend_PBD_Distance();
}
else if (this.m_tuning.bendingModel === b2BendingModel.b2_pbdHeightBendingModel) {
this.SolveBend_PBD_Height();
}
else if (this.m_tuning.bendingModel === b2BendingModel.b2_pbdTriangleBendingModel) {
this.SolveBend_PBD_Triangle();
}
if (this.m_tuning.stretchingModel === b2StretchingModel.b2_pbdStretchingModel) {
this.SolveStretch_PBD();
}
else if (this.m_tuning.stretchingModel === b2StretchingModel.b2_xpbdStretchingModel) {
this.SolveStretch_XPBD(dt);
}
}
// Constrain velocity
for (let i = 0; i < this.m_count; ++i) {
this.m_vs[i].x = inv_dt * (this.m_ps[i].x - this.m_p0s[i].x);
this.m_vs[i].y = inv_dt * (this.m_ps[i].y - this.m_p0s[i].y);
this.m_p0s[i].Copy(this.m_ps[i]);
}
}
Reset(position) {
this.m_position.Copy(position);
for (let i = 0; i < this.m_count; ++i) {
b2_math_1.b2Vec2.Add(this.m_bindPositions[i], this.m_position, this.m_ps[i]);
this.m_p0s[i].Copy(this.m_ps[i]);
this.m_vs[i].SetZero();
}
for (let i = 0; i < this.m_bendCount; ++i) {
this.m_bendConstraints[i].lambda = 0;
}
for (let i = 0; i < this.m_stretchCount; ++i) {
this.m_stretchConstraints[i].lambda = 0;
}
}
SolveStretch_PBD() {
const stiffness = this.m_tuning.stretchStiffness;
const { d } = temp;
for (let i = 0; i < this.m_stretchCount; ++i) {
const c = this.m_stretchConstraints[i];
const p1 = this.m_ps[c.i1];
const p2 = this.m_ps[c.i2];
b2_math_1.b2Vec2.Subtract(p2, p1, d);
const L = d.Normalize();
const sum = c.invMass1 + c.invMass2;
if (sum === 0) {
continue;
}
const s1 = c.invMass1 / sum;
const s2 = c.invMass2 / sum;
p1.SubtractScaled(stiffness * s1 * (c.L - L), d);
p2.AddScaled(stiffness * s2 * (c.L - L), d);
}
}
SolveStretch_XPBD(dt) {
// b2Assert(dt > 0);
const { dp1, dp2, u, J1 } = temp;
for (let i = 0; i < this.m_stretchCount; ++i) {
const c = this.m_stretchConstraints[i];
const p1 = this.m_ps[c.i1];
const p2 = this.m_ps[c.i2];
b2_math_1.b2Vec2.Subtract(p1, this.m_p0s[c.i1], dp1);
b2_math_1.b2Vec2.Subtract(p2, this.m_p0s[c.i2], dp2);
b2_math_1.b2Vec2.Subtract(p2, p1, u);
const L = u.Normalize();
b2_math_1.b2Vec2.Negate(u, J1);
const J2 = u;
const sum = c.invMass1 + c.invMass2;
if (sum === 0) {
continue;
}
const alpha = 1 / (c.spring * dt * dt); // 1 / kg
const beta = dt * dt * c.damper; // kg * s
const sigma = (alpha * beta) / dt; // non-dimensional
const C = L - c.L;
// This is using the initial velocities
const Cdot = b2_math_1.b2Vec2.Dot(J1, dp1) + b2_math_1.b2Vec2.Dot(J2, dp2);
const B = C + alpha * c.lambda + sigma * Cdot;
const sum2 = (1 + sigma) * sum + alpha;
const impulse = -B / sum2;
p1.AddScaled(c.invMass1 * impulse, J1);
p2.AddScaled(c.invMass2 * impulse, J2);
c.lambda += impulse;
}
}
SolveBend_PBD_Angle() {
const stiffness = this.m_tuning.bendStiffness;
const { Jd1, Jd2, J1, J2, d1, d2 } = temp;
for (let i = 0; i < this.m_bendCount; ++i) {
const c = this.m_bendConstraints[i];
const p1 = this.m_ps[c.i1];
const p2 = this.m_ps[c.i2];
const p3 = this.m_ps[c.i3];
b2_math_1.b2Vec2.Subtract(p2, p1, d1);
b2_math_1.b2Vec2.Subtract(p3, p2, d2);
const a = b2_math_1.b2Vec2.Cross(d1, d2);
const b = b2_math_1.b2Vec2.Dot(d1, d2);
const angle = Math.atan2(a, b);
let L1sqr;
let L2sqr;
if (this.m_tuning.isometric) {
L1sqr = c.L1 * c.L1;
L2sqr = c.L2 * c.L2;
}
else {
L1sqr = d1.LengthSquared();
L2sqr = d2.LengthSquared();
}
if (L1sqr * L2sqr === 0) {
continue;
}
b2_math_1.b2Vec2.Skew(d1, Jd1).Scale(-1 / L1sqr);
b2_math_1.b2Vec2.Skew(d2, Jd2).Scale(1 / L2sqr);
b2_math_1.b2Vec2.Negate(Jd1, J1);
b2_math_1.b2Vec2.Subtract(Jd1, Jd2, J2);
const J3 = Jd2;
let sum;
if (this.m_tuning.fixedEffectiveMass) {
sum = c.invEffectiveMass;
}
else {
sum =
c.invMass1 * b2_math_1.b2Vec2.Dot(J1, J1) + c.invMass2 * b2_math_1.b2Vec2.Dot(J2, J2) + c.invMass3 * b2_math_1.b2Vec2.Dot(J3, J3);
}
if (sum === 0) {
sum = c.invEffectiveMass;
}
const impulse = (-stiffness * angle) / sum;
p1.AddScaled(c.invMass1 * impulse, J1);
p2.AddScaled(c.invMass2 * impulse, J2);
p3.AddScaled(c.invMass3 * impulse, J3);
}
}
SolveBend_XPBD_Angle(dt) {
// b2Assert(dt > 0);
const { dp1, dp2, dp3, d1, d2, Jd1, Jd2, J1, J2 } = temp;
for (let i = 0; i < this.m_bendCount; ++i) {
const c = this.m_bendConstraints[i];
const p1 = this.m_ps[c.i1];
const p2 = this.m_ps[c.i2];
const p3 = this.m_ps[c.i3];
b2_math_1.b2Vec2.Subtract(p1, this.m_p0s[c.i1], dp1);
b2_math_1.b2Vec2.Subtract(p2, this.m_p0s[c.i2], dp2);
b2_math_1.b2Vec2.Subtract(p3, this.m_p0s[c.i3], dp3);
b2_math_1.b2Vec2.Subtract(p2, p1, d1);
b2_math_1.b2Vec2.Subtract(p3, p2, d2);
let L1sqr;
let L2sqr;
if (this.m_tuning.isometric) {
L1sqr = c.L1 * c.L1;
L2sqr = c.L2 * c.L2;
}
else {
L1sqr = d1.LengthSquared();
L2sqr = d2.LengthSquared();
}
if (L1sqr * L2sqr === 0) {
continue;
}
const a = b2_math_1.b2Vec2.Cross(d1, d2);
const b = b2_math_1.b2Vec2.Dot(d1, d2);
const angle = Math.atan2(a, b);
b2_math_1.b2Vec2.Skew(d1, Jd1).Scale(-1 / L1sqr);
b2_math_1.b2Vec2.Skew(d2, Jd2).Scale(1 / L2sqr);
b2_math_1.b2Vec2.Negate(Jd1, J1);
b2_math_1.b2Vec2.Subtract(Jd1, Jd2, J2);
const J3 = Jd2;
let sum;
if (this.m_tuning.fixedEffectiveMass) {
sum = c.invEffectiveMass;
}
else {
sum =
c.invMass1 * b2_math_1.b2Vec2.Dot(J1, J1) + c.invMass2 * b2_math_1.b2Vec2.Dot(J2, J2) + c.invMass3 * b2_math_1.b2Vec2.Dot(J3, J3);
}
if (sum === 0) {
continue;
}
const alpha = 1 / (c.spring * dt * dt);
const beta = dt * dt * c.damper;
const sigma = (alpha * beta) / dt;
const C = angle;
// This is using the initial velocities
const Cdot = b2_math_1.b2Vec2.Dot(J1, dp1) + b2_math_1.b2Vec2.Dot(J2, dp2) + b2_math_1.b2Vec2.Dot(J3, dp3);
const B = C + alpha * c.lambda + sigma * Cdot;
const sum2 = (1 + sigma) * sum + alpha;
const impulse = -B / sum2;
p1.AddScaled(c.invMass1 * impulse, J1);
p2.AddScaled(c.invMass2 * impulse, J2);
p3.AddScaled(c.invMass3 * impulse, J3);
c.lambda += impulse;
}
}
SolveBend_PBD_Distance() {
const stiffness = this.m_tuning.bendStiffness;
const { d } = temp;
for (let i = 0; i < this.m_bendCount; ++i) {
const c = this.m_bendConstraints[i];
const { i1 } = c;
const i2 = c.i3;
const p1 = this.m_ps[i1];
const p2 = this.m_ps[i2];
b2_math_1.b2Vec2.Subtract(p2, p1, d);
const L = d.Normalize();
const sum = c.invMass1 + c.invMass3;
if (sum === 0) {
continue;
}
const s1 = c.invMass1 / sum;
const s2 = c.invMass3 / sum;
p1.SubtractScaled(stiffness * s1 * (c.L1 + c.L2 - L), d);
p2.AddScaled(stiffness * s2 * (c.L1 + c.L2 - L), d);
}
}
/**
* Constraint based implementation of:
* P. Volino: Simple Linear Bending Stiffness in Particle Systems
*/
SolveBend_PBD_Height() {
const stiffness = this.m_tuning.bendStiffness;
const { dHat, J1, J2, J3, d } = temp;
for (let i = 0; i < this.m_bendCount; ++i) {
const c = this.m_bendConstraints[i];
const p1 = this.m_ps[c.i1];
const p2 = this.m_ps[c.i2];
const p3 = this.m_ps[c.i3];
// Barycentric coordinates are held constant
d.x = c.alpha1 * p1.x + c.alpha2 * p3.x - p2.x;
d.y = c.alpha1 * p1.y + c.alpha2 * p3.y - p2.y;
const dLen = d.Length();
if (dLen === 0) {
continue;
}
b2_math_1.b2Vec2.Scale(1 / dLen, d, dHat);
b2_math_1.b2Vec2.Scale(c.alpha1, dHat, J1);
b2_math_1.b2Vec2.Negate(dHat, J2);
b2_math_1.b2Vec2.Scale(c.alpha2, dHat, J3);
const sum = c.invMass1 * c.alpha1 * c.alpha1 + c.invMass2 + c.invMass3 * c.alpha2 * c.alpha2;
if (sum === 0) {
continue;
}
const C = dLen;
const mass = 1 / sum;
const impulse = -stiffness * mass * C;
p1.AddScaled(c.invMass1 * impulse, J1);
p2.AddScaled(c.invMass2 * impulse, J2);
p3.AddScaled(c.invMass3 * impulse, J3);
}
}
/** M. Kelager: A Triangle Bending Constraint Model for PBD */
SolveBend_PBD_Triangle() {
const stiffness = this.m_tuning.bendStiffness;
const { d } = temp;
for (let i = 0; i < this.m_bendCount; ++i) {
const c = this.m_bendConstraints[i];
const b0 = this.m_ps[c.i1];
const v = this.m_ps[c.i2];
const b1 = this.m_ps[c.i3];
const wb0 = c.invMass1;
const wv = c.invMass2;
const wb1 = c.invMass3;
const W = wb0 + wb1 + 2 * wv;
const invW = stiffness / W;
d.x = v.x - (1 / 3) * (b0.x + v.x + b1.x);
d.y = v.y - (1 / 3) * (b0.y + v.y + b1.y);
b0.AddScaled(2 * wb0 * invW, d);
v.AddScaled(-4 * wv * invW, d);
b1.AddScaled(2 * wb1 * invW, d);
}
}
ApplyBendForces(dt) {
// omega = 2 * pi * hz
const omega = 2 * Math.PI * this.m_tuning.bendHertz;
const { d1, d2, Jd1, Jd2, J1, J2 } = temp;
for (let i = 0; i < this.m_bendCount; ++i) {
const c = this.m_bendConstraints[i];
const p1 = this.m_ps[c.i1];
const p2 = this.m_ps[c.i2];
const p3 = this.m_ps[c.i3];
const v1 = this.m_vs[c.i1];
const v2 = this.m_vs[c.i2];
const v3 = this.m_vs[c.i3];
b2_math_1.b2Vec2.Subtract(p2, p1, d1);
b2_math_1.b2Vec2.Subtract(p3, p2, d2);
let L1sqr;
let L2sqr;
if (this.m_tuning.isometric) {
L1sqr = c.L1 * c.L1;
L2sqr = c.L2 * c.L2;
}
else {
L1sqr = d1.LengthSquared();
L2sqr = d2.LengthSquared();
}
if (L1sqr * L2sqr === 0) {
continue;
}
const a = b2_math_1.b2Vec2.Cross(d1, d2);
const b = b2_math_1.b2Vec2.Dot(d1, d2);
const angle = Math.atan2(a, b);
b2_math_1.b2Vec2.Skew(d1, Jd1).Scale(-1 / L1sqr);
b2_math_1.b2Vec2.Skew(d2, Jd2).Scale(1 / L2sqr);
b2_math_1.b2Vec2.Negate(Jd1, J1);
b2_math_1.b2Vec2.Subtract(Jd1, Jd2, J2);
const J3 = Jd2;
let sum;
if (this.m_tuning.fixedEffectiveMass) {
sum = c.invEffectiveMass;
}
else {
sum =
c.invMass1 * b2_math_1.b2Vec2.Dot(J1, J1) + c.invMass2 * b2_math_1.b2Vec2.Dot(J2, J2) + c.invMass3 * b2_math_1.b2Vec2.Dot(J3, J3);
}
if (sum === 0) {
continue;
}
const mass = 1 / sum;
const spring = mass * omega * omega;
const damper = 2 * mass * this.m_tuning.bendDamping * omega;
const C = angle;
const Cdot = b2_math_1.b2Vec2.Dot(J1, v1) + b2_math_1.b2Vec2.Dot(J2, v2) + b2_math_1.b2Vec2.Dot(J3, v3);
const impulse = -dt * (spring * C + damper * Cdot);
this.m_vs[c.i1].AddScaled(c.invMass1 * impulse, J1);
this.m_vs[c.i2].AddScaled(c.invMass2 * impulse, J2);
this.m_vs[c.i3].AddScaled(c.invMass3 * impulse, J3);
}
}
Draw(draw) {
for (let i = 0; i < this.m_count - 1; ++i) {
draw.DrawSegment(this.m_ps[i], this.m_ps[i + 1], b2_draw_1.debugColors.rope);
const pc = this.m_invMasses[i] > 0 ? b2_draw_1.debugColors.ropePointD : b2_draw_1.debugColors.ropePointG;
draw.DrawPoint(this.m_ps[i], 5, pc);
}
const pc = this.m_invMasses[this.m_count - 1] > 0 ? b2_draw_1.debugColors.ropePointD : b2_draw_1.debugColors.ropePointG;
draw.DrawPoint(this.m_ps[this.m_count - 1], 5, pc);
}
}
exports.b2Rope = b2Rope;