the-world-engine
Version:
three.js based, unity like game engine for browser
622 lines (611 loc) • 23.4 kB
JavaScript
import { b2Vec2, b2Atan2 } from "../common/b2_math.js";
import { b2Color } from "../common/b2_draw.js";
import { b2_pi } from "../common/b2_settings.js";
export var b2StretchingModel;
(function(t) {
t[t["b2_pbdStretchingModel"] = 0] = "b2_pbdStretchingModel";
t[t["b2_xpbdStretchingModel"] = 1] = "b2_xpbdStretchingModel";
})(b2StretchingModel || (b2StretchingModel = {}));
export var b2BendingModel;
(function(t) {
t[t["b2_springAngleBendingModel"] = 0] = "b2_springAngleBendingModel";
t[t["b2_pbdAngleBendingModel"] = 1] = "b2_pbdAngleBendingModel";
t[t["b2_xpbdAngleBendingModel"] = 2] = "b2_xpbdAngleBendingModel";
t[t["b2_pbdDistanceBendingModel"] = 3] = "b2_pbdDistanceBendingModel";
t[t["b2_pbdHeightBendingModel"] = 4] = "b2_pbdHeightBendingModel";
t[t["b2_pbdTriangleBendingModel"] = 5] = "b2_pbdTriangleBendingModel";
})(b2BendingModel || (b2BendingModel = {}));
export class b2RopeTuning {
constructor() {
this.stretchingModel = b2StretchingModel.b2_pbdStretchingModel;
this.bendingModel = b2BendingModel.b2_pbdAngleBendingModel;
this.damping = 0;
this.stretchStiffness = 1;
this.stretchHertz = 0;
this.stretchDamping = 0;
this.bendStiffness = .5;
this.bendHertz = 1;
this.bendDamping = 0;
this.isometric = false;
this.fixedEffectiveMass = false;
this.warmStart = false;
}
Copy(t) {
this.stretchingModel = t.stretchingModel;
this.bendingModel = t.bendingModel;
this.damping = t.damping;
this.stretchStiffness = t.stretchStiffness;
this.stretchHertz = t.stretchHertz;
this.stretchDamping = t.stretchDamping;
this.bendStiffness = t.bendStiffness;
this.bendHertz = t.bendHertz;
this.bendDamping = t.bendDamping;
this.isometric = t.isometric;
this.fixedEffectiveMass = t.fixedEffectiveMass;
this.warmStart = t.warmStart;
return this;
}
}
export class b2RopeDef {
constructor() {
this.position = new b2Vec2;
this.vertices = [];
this.count = 0;
this.masses = [];
this.gravity = new b2Vec2;
this.tuning = new 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;
}
}
export class b2Rope {
constructor() {
this.m_position = new b2Vec2;
this.m_count = 0;
this.m_stretchCount = 0;
this.m_bendCount = 0;
this.m_stretchConstraints = [];
this.m_bendConstraints = [];
this.m_bindPositions = [];
this.m_ps = [];
this.m_p0s = [];
this.m_vs = [];
this.m_invMasses = [];
this.m_gravity = new b2Vec2;
this.m_tuning = new b2RopeTuning;
}
Create(t) {
this.m_position.Copy(t.position);
this.m_count = t.count;
function make_array(t, s, i) {
for (let n = 0; n < s; ++n) {
t[n] = i(n);
}
}
make_array(this.m_bindPositions, this.m_count, (() => new b2Vec2));
make_array(this.m_ps, this.m_count, (() => new b2Vec2));
make_array(this.m_p0s, this.m_count, (() => new b2Vec2));
make_array(this.m_vs, this.m_count, (() => new b2Vec2));
make_array(this.m_invMasses, this.m_count, (() => 0));
for (let s = 0; s < this.m_count; ++s) {
this.m_bindPositions[s].Copy(t.vertices[s]);
this.m_ps[s].Copy(t.vertices[s]).SelfAdd(this.m_position);
this.m_p0s[s].Copy(t.vertices[s]).SelfAdd(this.m_position);
this.m_vs[s].SetZero();
const i = t.masses[s];
if (i > 0) {
this.m_invMasses[s] = 1 / i;
} else {
this.m_invMasses[s] = 0;
}
}
this.m_stretchCount = this.m_count - 1;
this.m_bendCount = this.m_count - 2;
make_array(this.m_stretchConstraints, this.m_stretchCount, (() => new b2RopeStretch));
make_array(this.m_bendConstraints, this.m_bendCount, (() => new b2RopeBend));
for (let t = 0; t < this.m_stretchCount; ++t) {
const s = this.m_stretchConstraints[t];
const i = this.m_ps[t];
const n = this.m_ps[t + 1];
s.i1 = t;
s.i2 = t + 1;
s.L = b2Vec2.DistanceVV(i, n);
s.invMass1 = this.m_invMasses[t];
s.invMass2 = this.m_invMasses[t + 1];
s.lambda = 0;
s.damper = 0;
s.spring = 0;
}
for (let t = 0; t < this.m_bendCount; ++t) {
const s = this.m_bendConstraints[t];
const i = this.m_ps[t];
const n = this.m_ps[t + 1];
const h = this.m_ps[t + 2];
s.i1 = t;
s.i2 = t + 1;
s.i3 = t + 2;
s.invMass1 = this.m_invMasses[t];
s.invMass2 = this.m_invMasses[t + 1];
s.invMass3 = this.m_invMasses[t + 2];
s.invEffectiveMass = 0;
s.L1 = b2Vec2.DistanceVV(i, n);
s.L2 = b2Vec2.DistanceVV(n, h);
s.lambda = 0;
const o = b2Vec2.SubVV(n, i, new b2Vec2);
const c = b2Vec2.SubVV(h, n, new b2Vec2);
const e = o.LengthSquared();
const b = c.LengthSquared();
if (e * b === 0) {
continue;
}
const l = (new b2Vec2).Copy(o).SelfSkew().SelfMul(-1 / e);
const r = (new b2Vec2).Copy(c).SelfSkew().SelfMul(1 / b);
const f = l.Clone().SelfNeg();
const V = l.Clone().SelfSub(r);
const d = r.Clone();
s.invEffectiveMass = s.invMass1 * b2Vec2.DotVV(f, f) + s.invMass2 * b2Vec2.DotVV(V, V) + s.invMass3 * b2Vec2.DotVV(d, d);
const a = b2Vec2.SubVV(h, i, new b2Vec2);
const g = a.LengthSquared();
if (g === 0) {
continue;
}
s.alpha1 = b2Vec2.DotVV(c, a) / g;
s.alpha2 = b2Vec2.DotVV(o, a) / g;
}
this.m_gravity.Copy(t.gravity);
this.SetTuning(t.tuning);
}
SetTuning(t) {
this.m_tuning.Copy(t);
const s = 2 * b2_pi * this.m_tuning.bendHertz;
for (let t = 0; t < this.m_bendCount; ++t) {
const i = this.m_bendConstraints[t];
const n = i.L1 * i.L1;
const h = i.L2 * i.L2;
if (n * h === 0) {
i.spring = 0;
i.damper = 0;
continue;
}
const o = 1 / i.L1 + 1 / i.L2;
const c = i.invMass1 / n + i.invMass2 * o * o + i.invMass3 / h;
if (c === 0) {
i.spring = 0;
i.damper = 0;
continue;
}
const e = 1 / c;
i.spring = e * s * s;
i.damper = 2 * e * this.m_tuning.bendDamping * s;
}
const i = 2 * b2_pi * this.m_tuning.stretchHertz;
for (let t = 0; t < this.m_stretchCount; ++t) {
const s = this.m_stretchConstraints[t];
const n = s.invMass1 + s.invMass2;
if (n === 0) {
continue;
}
const h = 1 / n;
s.spring = h * i * i;
s.damper = 2 * h * this.m_tuning.stretchDamping * i;
}
}
Step(t, s, i) {
if (t === 0) {
return;
}
const n = 1 / t;
const h = Math.exp(-t * this.m_tuning.damping);
for (let s = 0; s < this.m_count; ++s) {
if (this.m_invMasses[s] > 0) {
this.m_vs[s].x *= h;
this.m_vs[s].y *= h;
this.m_vs[s].x += t * this.m_gravity.x;
this.m_vs[s].y += t * this.m_gravity.y;
} else {
this.m_vs[s].x = n * (this.m_bindPositions[s].x + i.x - this.m_p0s[s].x);
this.m_vs[s].y = n * (this.m_bindPositions[s].y + i.y - this.m_p0s[s].y);
}
}
if (this.m_tuning.bendingModel === b2BendingModel.b2_springAngleBendingModel) {
this.ApplyBendForces(t);
}
for (let t = 0; t < this.m_bendCount; ++t) {
this.m_bendConstraints[t].lambda = 0;
}
for (let t = 0; t < this.m_stretchCount; ++t) {
this.m_stretchConstraints[t].lambda = 0;
}
for (let s = 0; s < this.m_count; ++s) {
this.m_ps[s].x += t * this.m_vs[s].x;
this.m_ps[s].y += t * this.m_vs[s].y;
}
for (let i = 0; i < s; ++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(t);
} 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(t);
}
}
for (let t = 0; t < this.m_count; ++t) {
this.m_vs[t].x = n * (this.m_ps[t].x - this.m_p0s[t].x);
this.m_vs[t].y = n * (this.m_ps[t].y - this.m_p0s[t].y);
this.m_p0s[t].Copy(this.m_ps[t]);
}
}
Reset(t) {
this.m_position.Copy(t);
for (let t = 0; t < this.m_count; ++t) {
this.m_ps[t].x = this.m_bindPositions[t].x + this.m_position.x;
this.m_ps[t].y = this.m_bindPositions[t].y + this.m_position.y;
this.m_p0s[t].x = this.m_bindPositions[t].x + this.m_position.x;
this.m_p0s[t].y = this.m_bindPositions[t].y + this.m_position.y;
this.m_vs[t].SetZero();
}
for (let t = 0; t < this.m_bendCount; ++t) {
this.m_bendConstraints[t].lambda = 0;
}
for (let t = 0; t < this.m_stretchCount; ++t) {
this.m_stretchConstraints[t].lambda = 0;
}
}
Draw(t) {
const s = new b2Color(.4, .5, .7);
const i = new b2Color(.1, .8, .1);
const n = new b2Color(.7, .2, .4);
for (let h = 0; h < this.m_count - 1; ++h) {
t.DrawSegment(this.m_ps[h], this.m_ps[h + 1], s);
const o = this.m_invMasses[h] > 0 ? n : i;
t.DrawPoint(this.m_ps[h], 5, o);
}
const h = this.m_invMasses[this.m_count - 1] > 0 ? n : i;
t.DrawPoint(this.m_ps[this.m_count - 1], 5, h);
}
SolveStretch_PBD() {
const t = this.m_tuning.stretchStiffness;
for (let s = 0; s < this.m_stretchCount; ++s) {
const i = this.m_stretchConstraints[s];
const n = this.m_ps[i.i1].Clone();
const h = this.m_ps[i.i2].Clone();
const o = h.Clone().SelfSub(n);
const c = o.Normalize();
const e = i.invMass1 + i.invMass2;
if (e === 0) {
continue;
}
const b = i.invMass1 / e;
const l = i.invMass2 / e;
n.x -= t * b * (i.L - c) * o.x;
n.y -= t * b * (i.L - c) * o.y;
h.x += t * l * (i.L - c) * o.x;
h.y += t * l * (i.L - c) * o.y;
this.m_ps[i.i1].Copy(n);
this.m_ps[i.i2].Copy(h);
}
}
SolveStretch_XPBD(t) {
for (let s = 0; s < this.m_stretchCount; ++s) {
const i = this.m_stretchConstraints[s];
const n = this.m_ps[i.i1].Clone();
const h = this.m_ps[i.i2].Clone();
const o = n.Clone().SelfSub(this.m_p0s[i.i1]);
const c = h.Clone().SelfSub(this.m_p0s[i.i2]);
const e = h.Clone().SelfSub(n);
const b = e.Normalize();
const l = e.Clone().SelfNeg();
const r = e;
const f = i.invMass1 + i.invMass2;
if (f === 0) {
continue;
}
const V = 1 / (i.spring * t * t);
const d = t * t * i.damper;
const a = V * d / t;
const g = b - i.L;
const _ = b2Vec2.DotVV(l, o) + b2Vec2.DotVV(r, c);
const p = g + V * i.lambda + a * _;
const w = (1 + a) * f + V;
const B = -p / w;
n.x += i.invMass1 * B * l.x;
n.y += i.invMass1 * B * l.y;
h.x += i.invMass2 * B * r.x;
h.y += i.invMass2 * B * r.y;
this.m_ps[i.i1].Copy(n);
this.m_ps[i.i2].Copy(h);
i.lambda += B;
}
}
SolveBend_PBD_Angle() {
const t = this.m_tuning.bendStiffness;
for (let s = 0; s < this.m_bendCount; ++s) {
const i = this.m_bendConstraints[s];
const n = this.m_ps[i.i1];
const h = this.m_ps[i.i2];
const o = this.m_ps[i.i3];
const c = h.Clone().SelfSub(n);
const e = o.Clone().SelfSub(h);
const b = b2Vec2.CrossVV(c, e);
const l = b2Vec2.DotVV(c, e);
const r = b2Atan2(b, l);
let f = 0, V = 0;
if (this.m_tuning.isometric) {
f = i.L1 * i.L1;
V = i.L2 * i.L2;
} else {
f = c.LengthSquared();
V = e.LengthSquared();
}
if (f * V === 0) {
continue;
}
const d = (new b2Vec2).Copy(c).SelfSkew().SelfMul(-1 / f);
const a = (new b2Vec2).Copy(e).SelfSkew().SelfMul(1 / V);
const g = d.Clone().SelfNeg();
const _ = d.Clone().SelfSub(a);
const p = a;
let w = 0;
if (this.m_tuning.fixedEffectiveMass) {
w = i.invEffectiveMass;
} else {
w = i.invMass1 * b2Vec2.DotVV(g, g) + i.invMass2 * b2Vec2.DotVV(_, _) + i.invMass3 * b2Vec2.DotVV(p, p);
}
if (w === 0) {
w = i.invEffectiveMass;
}
const B = -t * r / w;
n.x += i.invMass1 * B * g.x;
n.y += i.invMass1 * B * g.y;
h.x += i.invMass2 * B * _.x;
h.y += i.invMass2 * B * _.y;
o.x += i.invMass3 * B * p.x;
o.y += i.invMass3 * B * p.y;
this.m_ps[i.i1].Copy(n);
this.m_ps[i.i2].Copy(h);
this.m_ps[i.i3].Copy(o);
}
}
SolveBend_XPBD_Angle(t) {
for (let s = 0; s < this.m_bendCount; ++s) {
const i = this.m_bendConstraints[s];
const n = this.m_ps[i.i1];
const h = this.m_ps[i.i2];
const o = this.m_ps[i.i3];
const c = n.Clone().SelfSub(this.m_p0s[i.i1]);
const e = h.Clone().SelfSub(this.m_p0s[i.i2]);
const b = o.Clone().SelfSub(this.m_p0s[i.i3]);
const l = h.Clone().SelfSub(n);
const r = o.Clone().SelfSub(h);
let f, V;
if (this.m_tuning.isometric) {
f = i.L1 * i.L1;
V = i.L2 * i.L2;
} else {
f = l.LengthSquared();
V = r.LengthSquared();
}
if (f * V === 0) {
continue;
}
const d = b2Vec2.CrossVV(l, r);
const a = b2Vec2.DotVV(l, r);
const g = b2Atan2(d, a);
const _ = (new b2Vec2).Copy(l).SelfSkew().SelfMul(-1 / f);
const p = (new b2Vec2).Copy(r).SelfSkew().SelfMul(1 / V);
const w = _.Clone().SelfNeg();
const B = _.Clone().SelfSub(p);
const u = p;
let M;
if (this.m_tuning.fixedEffectiveMass) {
M = i.invEffectiveMass;
} else {
M = i.invMass1 * b2Vec2.DotVV(w, w) + i.invMass2 * b2Vec2.DotVV(B, B) + i.invMass3 * b2Vec2.DotVV(u, u);
}
if (M === 0) {
continue;
}
const m = 1 / (i.spring * t * t);
const S = t * t * i.damper;
const D = m * S / t;
const y = g;
const A = b2Vec2.DotVV(w, c) + b2Vec2.DotVV(B, e) + b2Vec2.DotVV(u, b);
const R = y + m * i.lambda + D * A;
const v = (1 + D) * M + m;
const k = -R / v;
n.x += i.invMass1 * k * w.x;
n.y += i.invMass1 * k * w.y;
h.x += i.invMass2 * k * B.x;
h.y += i.invMass2 * k * B.y;
o.x += i.invMass3 * k * u.x;
o.y += i.invMass3 * k * u.y;
this.m_ps[i.i1].Copy(n);
this.m_ps[i.i2].Copy(h);
this.m_ps[i.i3].Copy(o);
i.lambda += k;
}
}
SolveBend_PBD_Distance() {
const t = this.m_tuning.bendStiffness;
for (let s = 0; s < this.m_bendCount; ++s) {
const i = this.m_bendConstraints[s];
const n = i.i1;
const h = i.i3;
const o = this.m_ps[n].Clone();
const c = this.m_ps[h].Clone();
const e = c.Clone().SelfSub(o);
const b = e.Normalize();
const l = i.invMass1 + i.invMass3;
if (l === 0) {
continue;
}
const r = i.invMass1 / l;
const f = i.invMass3 / l;
o.x -= t * r * (i.L1 + i.L2 - b) * e.x;
o.y -= t * r * (i.L1 + i.L2 - b) * e.y;
c.x += t * f * (i.L1 + i.L2 - b) * e.x;
c.y += t * f * (i.L1 + i.L2 - b) * e.y;
this.m_ps[n].Copy(o);
this.m_ps[h].Copy(c);
}
}
SolveBend_PBD_Height() {
const t = this.m_tuning.bendStiffness;
for (let s = 0; s < this.m_bendCount; ++s) {
const i = this.m_bendConstraints[s];
const n = this.m_ps[i.i1].Clone();
const h = this.m_ps[i.i2].Clone();
const o = this.m_ps[i.i3].Clone();
const c = new b2Vec2;
c.x = i.alpha1 * n.x + i.alpha2 * o.x - h.x;
c.y = i.alpha1 * n.y + i.alpha2 * o.y - h.y;
const e = c.Length();
if (e === 0) {
continue;
}
const b = c.Clone().SelfMul(1 / e);
const l = b.Clone().SelfMul(i.alpha1);
const r = b.Clone().SelfNeg();
const f = b.Clone().SelfMul(i.alpha2);
const V = i.invMass1 * i.alpha1 * i.alpha1 + i.invMass2 + i.invMass3 * i.alpha2 * i.alpha2;
if (V === 0) {
continue;
}
const d = e;
const a = 1 / V;
const g = -t * a * d;
n.x += i.invMass1 * g * l.x;
n.y += i.invMass1 * g * l.y;
h.x += i.invMass2 * g * r.x;
h.y += i.invMass2 * g * r.y;
o.x += i.invMass3 * g * f.x;
o.y += i.invMass3 * g * f.y;
this.m_ps[i.i1].Copy(n);
this.m_ps[i.i2].Copy(h);
this.m_ps[i.i3].Copy(o);
}
}
SolveBend_PBD_Triangle() {
const t = this.m_tuning.bendStiffness;
for (let s = 0; s < this.m_bendCount; ++s) {
const i = this.m_bendConstraints[s];
const n = this.m_ps[i.i1].Clone();
const h = this.m_ps[i.i2].Clone();
const o = this.m_ps[i.i3].Clone();
const c = i.invMass1;
const e = i.invMass2;
const b = i.invMass3;
const l = c + b + 2 * e;
const r = t / l;
const f = new b2Vec2;
f.x = h.x - 1 / 3 * (n.x + h.x + o.x);
f.y = h.y - 1 / 3 * (n.y + h.y + o.y);
const V = new b2Vec2;
V.x = 2 * c * r * f.x;
V.y = 2 * c * r * f.y;
const d = new b2Vec2;
d.x = -4 * e * r * f.x;
d.y = -4 * e * r * f.y;
const a = new b2Vec2;
a.x = 2 * b * r * f.x;
a.y = 2 * b * r * f.y;
n.SelfAdd(V);
h.SelfAdd(d);
o.SelfAdd(a);
this.m_ps[i.i1].Copy(n);
this.m_ps[i.i2].Copy(h);
this.m_ps[i.i3].Copy(o);
}
}
ApplyBendForces(t) {
const s = 2 * b2_pi * this.m_tuning.bendHertz;
for (let i = 0; i < this.m_bendCount; ++i) {
const n = this.m_bendConstraints[i];
const h = this.m_ps[n.i1].Clone();
const o = this.m_ps[n.i2].Clone();
const c = this.m_ps[n.i3].Clone();
const e = this.m_vs[n.i1];
const b = this.m_vs[n.i2];
const l = this.m_vs[n.i3];
const r = h.Clone().SelfSub(h);
const f = c.Clone().SelfSub(o);
let V, d;
if (this.m_tuning.isometric) {
V = n.L1 * n.L1;
d = n.L2 * n.L2;
} else {
V = r.LengthSquared();
d = f.LengthSquared();
}
if (V * d === 0) {
continue;
}
const a = b2Vec2.CrossVV(r, f);
const g = b2Vec2.DotVV(r, f);
const _ = b2Atan2(a, g);
const p = (new b2Vec2).Copy(r).SelfSkew().SelfMul(-1 / V);
const w = (new b2Vec2).Copy(f).SelfSkew().SelfMul(1 / d);
const B = p.Clone().SelfNeg();
const u = p.Clone().SelfSub(w);
const M = w;
let m = 0;
if (this.m_tuning.fixedEffectiveMass) {
m = n.invEffectiveMass;
} else {
m = n.invMass1 * b2Vec2.DotVV(B, B) + n.invMass2 * b2Vec2.DotVV(u, u) + n.invMass3 * b2Vec2.DotVV(M, M);
}
if (m === 0) {
continue;
}
const S = 1 / m;
const D = S * s * s;
const y = 2 * S * this.m_tuning.bendDamping * s;
const A = _;
const R = b2Vec2.DotVV(B, e) + b2Vec2.DotVV(u, b) + b2Vec2.DotVV(M, l);
const v = -t * (D * A + y * R);
this.m_vs[n.i1].x += n.invMass1 * v * B.x;
this.m_vs[n.i1].y += n.invMass1 * v * B.y;
this.m_vs[n.i2].x += n.invMass2 * v * u.x;
this.m_vs[n.i2].y += n.invMass2 * v * u.y;
this.m_vs[n.i3].x += n.invMass3 * v * M.x;
this.m_vs[n.i3].y += n.invMass3 * v * M.y;
}
}
}