@box2d/debug-draw
Version:
Debug drawing helper for @box2d
342 lines (341 loc) • 15.3 kB
JavaScript
"use strict";
// MIT License
Object.defineProperty(exports, "__esModule", { value: true });
exports.b2PulleyJoint = exports.b2PulleyJointDef = exports.b2_minPulleyLength = 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.
// DEBUG: import { b2Assert, b2_epsilon } from "../common/b2_common";
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 b2_joint_1 = require("./b2_joint");
// Pulley:
// length1 = norm(p1 - s1)
// length2 = norm(p2 - s2)
// C0 = (length1 + ratio * length2)_initial
// C = C0 - (length1 + ratio * length2)
// u1 = (p1 - s1) / norm(p1 - s1)
// u2 = (p2 - s2) / norm(p2 - s2)
// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
exports.b2_minPulleyLength = 2;
const temp = {
qA: new b2_math_1.b2Rot(),
qB: new b2_math_1.b2Rot(),
lalcA: new b2_math_1.b2Vec2(),
lalcB: new b2_math_1.b2Vec2(),
p: new b2_math_1.b2Vec2(),
PA: new b2_math_1.b2Vec2(),
PB: new b2_math_1.b2Vec2(),
vpA: new b2_math_1.b2Vec2(),
vpB: new b2_math_1.b2Vec2(),
pA: new b2_math_1.b2Vec2(),
pB: new b2_math_1.b2Vec2(),
};
/**
* Pulley joint definition. This requires two ground anchors,
* two dynamic body anchor points, and a pulley ratio.
*/
class b2PulleyJointDef extends b2_joint_1.b2JointDef {
constructor() {
super(b2_joint_1.b2JointType.e_pulleyJoint);
/** The first ground anchor in world coordinates. This point never moves. */
this.groundAnchorA = new b2_math_1.b2Vec2(-1, 1);
/** The second ground anchor in world coordinates. This point never moves. */
this.groundAnchorB = new b2_math_1.b2Vec2(1, 1);
/** The local anchor point relative to bodyA's origin. */
this.localAnchorA = new b2_math_1.b2Vec2(-1, 0);
/** The local anchor point relative to bodyB's origin. */
this.localAnchorB = new b2_math_1.b2Vec2(1, 0);
/** The a reference length for the segment attached to bodyA. */
this.lengthA = 0;
/** The a reference length for the segment attached to bodyB. */
this.lengthB = 0;
/** The pulley ratio, used to simulate a block-and-tackle. */
this.ratio = 1;
this.collideConnected = true;
}
/** Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. */
Initialize(bA, bB, groundA, groundB, anchorA, anchorB, r) {
this.bodyA = bA;
this.bodyB = bB;
this.groundAnchorA.Copy(groundA);
this.groundAnchorB.Copy(groundB);
this.bodyA.GetLocalPoint(anchorA, this.localAnchorA);
this.bodyB.GetLocalPoint(anchorB, this.localAnchorB);
this.lengthA = b2_math_1.b2Vec2.Distance(anchorA, groundA);
this.lengthB = b2_math_1.b2Vec2.Distance(anchorB, groundB);
this.ratio = r;
// DEBUG: b2Assert(this.ratio > b2_epsilon);
}
}
exports.b2PulleyJointDef = b2PulleyJointDef;
const defaultGroundAnchorA = new b2_math_1.b2Vec2(-1, 1);
const defaultGroundAnchorB = b2_math_1.b2Vec2.UNITX;
const defaultLocalAnchorA = new b2_math_1.b2Vec2(-1, 0);
const defaultLocalAnchorB = b2_math_1.b2Vec2.UNITX;
/**
* The pulley joint is connected to two bodies and two fixed ground points.
* The pulley supports a ratio such that:
* length1 + ratio * length2 <= constant
* Yes, the force transmitted is scaled by the ratio.
* Warning: the pulley joint can get a bit squirrelly by itself. They often
* work better when combined with prismatic joints. You should also cover the
* the anchor points with static shapes to prevent one side from going to
* zero length.
*/
class b2PulleyJoint extends b2_joint_1.b2Joint {
/** @internal protected */
constructor(def) {
var _a, _b, _c, _d, _e, _f, _g;
super(def);
this.m_groundAnchorA = new b2_math_1.b2Vec2();
this.m_groundAnchorB = new b2_math_1.b2Vec2();
this.m_lengthA = 0;
this.m_lengthB = 0;
// Solver shared
this.m_localAnchorA = new b2_math_1.b2Vec2();
this.m_localAnchorB = new b2_math_1.b2Vec2();
this.m_constant = 0;
this.m_ratio = 0;
this.m_impulse = 0;
// Solver temp
this.m_indexA = 0;
this.m_indexB = 0;
this.m_uA = new b2_math_1.b2Vec2();
this.m_uB = new b2_math_1.b2Vec2();
this.m_rA = new b2_math_1.b2Vec2();
this.m_rB = new b2_math_1.b2Vec2();
this.m_localCenterA = new b2_math_1.b2Vec2();
this.m_localCenterB = new b2_math_1.b2Vec2();
this.m_invMassA = 0;
this.m_invMassB = 0;
this.m_invIA = 0;
this.m_invIB = 0;
this.m_mass = 0;
this.m_groundAnchorA.Copy((_a = def.groundAnchorA) !== null && _a !== void 0 ? _a : defaultGroundAnchorA);
this.m_groundAnchorB.Copy((_b = def.groundAnchorB) !== null && _b !== void 0 ? _b : defaultGroundAnchorB);
this.m_localAnchorA.Copy((_c = def.localAnchorA) !== null && _c !== void 0 ? _c : defaultLocalAnchorA);
this.m_localAnchorB.Copy((_d = def.localAnchorB) !== null && _d !== void 0 ? _d : defaultLocalAnchorB);
this.m_lengthA = (_e = def.lengthA) !== null && _e !== void 0 ? _e : 0;
this.m_lengthB = (_f = def.lengthB) !== null && _f !== void 0 ? _f : 0;
// DEBUG: b2Assert((def.ratio ?? 1) !== 0);
this.m_ratio = (_g = def.ratio) !== null && _g !== void 0 ? _g : 1;
this.m_constant = this.m_lengthA + this.m_ratio * this.m_lengthB;
this.m_impulse = 0;
}
/** @internal protected */
InitVelocityConstraints(data) {
this.m_indexA = this.m_bodyA.m_islandIndex;
this.m_indexB = this.m_bodyB.m_islandIndex;
this.m_localCenterA.Copy(this.m_bodyA.m_sweep.localCenter);
this.m_localCenterB.Copy(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;
const cA = data.positions[this.m_indexA].c;
const aA = data.positions[this.m_indexA].a;
const vA = data.velocities[this.m_indexA].v;
let wA = data.velocities[this.m_indexA].w;
const cB = data.positions[this.m_indexB].c;
const aB = data.positions[this.m_indexB].a;
const vB = data.velocities[this.m_indexB].v;
let wB = data.velocities[this.m_indexB].w;
const { qA, qB, lalcA, lalcB } = temp;
qA.Set(aA);
qB.Set(aB);
b2_math_1.b2Rot.MultiplyVec2(qA, b2_math_1.b2Vec2.Subtract(this.m_localAnchorA, this.m_localCenterA, lalcA), this.m_rA);
b2_math_1.b2Rot.MultiplyVec2(qB, b2_math_1.b2Vec2.Subtract(this.m_localAnchorB, this.m_localCenterB, lalcB), this.m_rB);
// Get the pulley axes.
b2_math_1.b2Vec2.Add(cA, this.m_rA, this.m_uA).Subtract(this.m_groundAnchorA);
b2_math_1.b2Vec2.Add(cB, this.m_rB, this.m_uB).Subtract(this.m_groundAnchorB);
const lengthA = this.m_uA.Length();
const lengthB = this.m_uB.Length();
if (lengthA > 10 * b2_common_1.b2_linearSlop) {
this.m_uA.Scale(1 / lengthA);
}
else {
this.m_uA.SetZero();
}
if (lengthB > 10 * b2_common_1.b2_linearSlop) {
this.m_uB.Scale(1 / lengthB);
}
else {
this.m_uB.SetZero();
}
// Compute effective mass.
const ruA = b2_math_1.b2Vec2.Cross(this.m_rA, this.m_uA);
const ruB = b2_math_1.b2Vec2.Cross(this.m_rB, this.m_uB);
const mA = this.m_invMassA + this.m_invIA * ruA * ruA;
const mB = this.m_invMassB + this.m_invIB * ruB * ruB;
this.m_mass = mA + this.m_ratio * this.m_ratio * mB;
if (this.m_mass > 0) {
this.m_mass = 1 / this.m_mass;
}
if (data.step.warmStarting) {
// Scale impulses to support variable time steps.
this.m_impulse *= data.step.dtRatio;
// Warm starting.
const { PA, PB } = temp;
b2_math_1.b2Vec2.Scale(-this.m_impulse, this.m_uA, PA);
b2_math_1.b2Vec2.Scale(-this.m_ratio * this.m_impulse, this.m_uB, PB);
vA.AddScaled(this.m_invMassA, PA);
wA += this.m_invIA * b2_math_1.b2Vec2.Cross(this.m_rA, PA);
vB.AddScaled(this.m_invMassB, PB);
wB += this.m_invIB * b2_math_1.b2Vec2.Cross(this.m_rB, PB);
}
else {
this.m_impulse = 0;
}
data.velocities[this.m_indexA].w = wA;
data.velocities[this.m_indexB].w = wB;
}
/** @internal protected */
SolveVelocityConstraints(data) {
const vA = data.velocities[this.m_indexA].v;
let wA = data.velocities[this.m_indexA].w;
const vB = data.velocities[this.m_indexB].v;
let wB = data.velocities[this.m_indexB].w;
const { PA, PB, vpA, vpB } = temp;
b2_math_1.b2Vec2.AddCrossScalarVec2(vA, wA, this.m_rA, vpA);
b2_math_1.b2Vec2.AddCrossScalarVec2(vB, wB, this.m_rB, vpB);
const Cdot = -b2_math_1.b2Vec2.Dot(this.m_uA, vpA) - this.m_ratio * b2_math_1.b2Vec2.Dot(this.m_uB, vpB);
const impulse = -this.m_mass * Cdot;
this.m_impulse += impulse;
b2_math_1.b2Vec2.Scale(-impulse, this.m_uA, PA);
b2_math_1.b2Vec2.Scale(-this.m_ratio * impulse, this.m_uB, PB);
vA.AddScaled(this.m_invMassA, PA);
wA += this.m_invIA * b2_math_1.b2Vec2.Cross(this.m_rA, PA);
vB.AddScaled(this.m_invMassB, PB);
wB += this.m_invIB * b2_math_1.b2Vec2.Cross(this.m_rB, PB);
data.velocities[this.m_indexA].w = wA;
data.velocities[this.m_indexB].w = wB;
}
/** @internal protected */
SolvePositionConstraints(data) {
const cA = data.positions[this.m_indexA].c;
let aA = data.positions[this.m_indexA].a;
const cB = data.positions[this.m_indexB].c;
let aB = data.positions[this.m_indexB].a;
const { qA, qB, lalcA, lalcB, PA, PB } = temp;
qA.Set(aA);
qB.Set(aB);
const rA = b2_math_1.b2Rot.MultiplyVec2(qA, b2_math_1.b2Vec2.Subtract(this.m_localAnchorA, this.m_localCenterA, lalcA), this.m_rA);
const rB = b2_math_1.b2Rot.MultiplyVec2(qB, b2_math_1.b2Vec2.Subtract(this.m_localAnchorB, this.m_localCenterB, lalcB), this.m_rB);
// Get the pulley axes.
const uA = b2_math_1.b2Vec2.Add(cA, rA, this.m_uA).Subtract(this.m_groundAnchorA);
const uB = b2_math_1.b2Vec2.Add(cB, rB, this.m_uB).Subtract(this.m_groundAnchorB);
const lengthA = uA.Length();
const lengthB = uB.Length();
if (lengthA > 10 * b2_common_1.b2_linearSlop) {
uA.Scale(1 / lengthA);
}
else {
uA.SetZero();
}
if (lengthB > 10 * b2_common_1.b2_linearSlop) {
uB.Scale(1 / lengthB);
}
else {
uB.SetZero();
}
// Compute effective mass.
const ruA = b2_math_1.b2Vec2.Cross(rA, uA);
const ruB = b2_math_1.b2Vec2.Cross(rB, uB);
const mA = this.m_invMassA + this.m_invIA * ruA * ruA;
const mB = this.m_invMassB + this.m_invIB * ruB * ruB;
let mass = mA + this.m_ratio * this.m_ratio * mB;
if (mass > 0) {
mass = 1 / mass;
}
const C = this.m_constant - lengthA - this.m_ratio * lengthB;
const linearError = Math.abs(C);
const impulse = -mass * C;
b2_math_1.b2Vec2.Scale(-impulse, uA, PA);
b2_math_1.b2Vec2.Scale(-this.m_ratio * impulse, uB, PB);
cA.AddScaled(this.m_invMassA, PA);
aA += this.m_invIA * b2_math_1.b2Vec2.Cross(rA, PA);
cB.AddScaled(this.m_invMassB, PB);
aB += this.m_invIB * b2_math_1.b2Vec2.Cross(rB, PB);
data.positions[this.m_indexA].a = aA;
data.positions[this.m_indexB].a = aB;
return linearError < b2_common_1.b2_linearSlop;
}
GetAnchorA(out) {
return this.m_bodyA.GetWorldPoint(this.m_localAnchorA, out);
}
GetAnchorB(out) {
return this.m_bodyB.GetWorldPoint(this.m_localAnchorB, out);
}
GetReactionForce(inv_dt, out) {
out.x = inv_dt * this.m_impulse * this.m_uB.x;
out.y = inv_dt * this.m_impulse * this.m_uB.y;
return out;
}
GetReactionTorque(_inv_dt) {
return 0;
}
/** Get the first ground anchor. */
GetGroundAnchorA() {
return this.m_groundAnchorA;
}
/** Get the second ground anchor. */
GetGroundAnchorB() {
return this.m_groundAnchorB;
}
/** Get the current length of the segment attached to bodyA. */
GetLengthA() {
return this.m_lengthA;
}
/** Get the current length of the segment attached to bodyB. */
GetLengthB() {
return this.m_lengthB;
}
/** Get the pulley ratio. */
GetRatio() {
return this.m_ratio;
}
/** Get the current length of the segment attached to bodyA. */
GetCurrentLengthA() {
const p = this.m_bodyA.GetWorldPoint(this.m_localAnchorA, temp.p);
const s = this.m_groundAnchorA;
return b2_math_1.b2Vec2.Distance(p, s);
}
/** Get the current length of the segment attached to bodyB. */
GetCurrentLengthB() {
const p = this.m_bodyB.GetWorldPoint(this.m_localAnchorB, temp.p);
const s = this.m_groundAnchorB;
return b2_math_1.b2Vec2.Distance(p, s);
}
ShiftOrigin(newOrigin) {
this.m_groundAnchorA.Subtract(newOrigin);
this.m_groundAnchorB.Subtract(newOrigin);
}
Draw(draw) {
const p1 = this.GetAnchorA(temp.pA);
const p2 = this.GetAnchorB(temp.pB);
const s1 = this.GetGroundAnchorA();
const s2 = this.GetGroundAnchorB();
draw.DrawSegment(s1, p1, b2_draw_1.debugColors.joint6);
draw.DrawSegment(s2, p2, b2_draw_1.debugColors.joint6);
draw.DrawSegment(s1, s2, b2_draw_1.debugColors.joint6);
}
}
exports.b2PulleyJoint = b2PulleyJoint;