@box2d/debug-draw
Version:
Debug drawing helper for @box2d
439 lines (438 loc) • 19.7 kB
JavaScript
"use strict";
// MIT License
Object.defineProperty(exports, "__esModule", { value: true });
exports.b2GearJoint = exports.b2GearJointDef = 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 } from "../common/b2_common";
const b2_common_1 = require("../common/b2_common");
const b2_math_1 = require("../common/b2_math");
const b2_joint_1 = require("./b2_joint");
// Gear Joint:
// C0 = (coordinate1 + ratio * coordinate2)_initial
// C = (coordinate1 + ratio * coordinate2) - C0 = 0
// J = [J1 ratio * J2]
// K = J * invM * JT
// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
// Revolute:
// coordinate = rotation
// Cdot = angularVelocity
// J = [0 0 1]
// K = J * invM * JT = invI
// Prismatic:
// coordinate = dot(p - pg, ug)
// Cdot = dot(v + cross(w, r), ug)
// J = [ug cross(r, ug)]
// K = J * invM * JT = invMass + invI * cross(r, ug)^2
const temp = {
qA: new b2_math_1.b2Rot(),
qB: new b2_math_1.b2Rot(),
qC: new b2_math_1.b2Rot(),
qD: new b2_math_1.b2Rot(),
lalcA: new b2_math_1.b2Vec2(),
lalcB: new b2_math_1.b2Vec2(),
lalcC: new b2_math_1.b2Vec2(),
lalcD: new b2_math_1.b2Vec2(),
u: new b2_math_1.b2Vec2(),
rA: new b2_math_1.b2Vec2(),
rB: new b2_math_1.b2Vec2(),
rC: new b2_math_1.b2Vec2(),
rD: new b2_math_1.b2Vec2(),
JvAC: new b2_math_1.b2Vec2(),
JvBD: new b2_math_1.b2Vec2(),
};
/**
* Gear joint definition. This definition requires two existing
* revolute or prismatic joints (any combination will work).
*
* @warning bodyB on the input joints must both be dynamic
*/
class b2GearJointDef extends b2_joint_1.b2JointDef {
constructor() {
super(b2_joint_1.b2JointType.e_gearJoint);
/**
* The gear ratio.
*
* @see b2GearJoint for explanation.
*/
this.ratio = 1;
}
}
exports.b2GearJointDef = b2GearJointDef;
/**
* A gear joint is used to connect two joints together. Either joint
* can be a revolute or prismatic joint. You specify a gear ratio
* to bind the motions together:
* coordinate1 + ratio * coordinate2 = constant
* The ratio can be negative or positive. If one joint is a revolute joint
* and the other joint is a prismatic joint, then the ratio will have units
* of length or units of 1/length.
*
* @warning You have to manually destroy the gear joint if joint1 or joint2
* is destroyed.
*/
class b2GearJoint extends b2_joint_1.b2Joint {
/** @internal protected */
constructor(def) {
var _a;
super(def);
this.m_typeA = b2_joint_1.b2JointType.e_unknownJoint;
this.m_typeB = b2_joint_1.b2JointType.e_unknownJoint;
// Solver shared
this.m_localAnchorA = new b2_math_1.b2Vec2();
this.m_localAnchorB = new b2_math_1.b2Vec2();
this.m_localAnchorC = new b2_math_1.b2Vec2();
this.m_localAnchorD = new b2_math_1.b2Vec2();
this.m_localAxisC = new b2_math_1.b2Vec2();
this.m_localAxisD = new b2_math_1.b2Vec2();
this.m_referenceAngleA = 0;
this.m_referenceAngleB = 0;
this.m_constant = 0;
this.m_ratio = 0;
this.m_tolerance = 0;
this.m_impulse = 0;
// Solver temp
this.m_indexA = 0;
this.m_indexB = 0;
this.m_indexC = 0;
this.m_indexD = 0;
this.m_lcA = new b2_math_1.b2Vec2();
this.m_lcB = new b2_math_1.b2Vec2();
this.m_lcC = new b2_math_1.b2Vec2();
this.m_lcD = new b2_math_1.b2Vec2();
this.m_mA = 0;
this.m_mB = 0;
this.m_mC = 0;
this.m_mD = 0;
this.m_iA = 0;
this.m_iB = 0;
this.m_iC = 0;
this.m_iD = 0;
this.m_JvAC = new b2_math_1.b2Vec2();
this.m_JvBD = new b2_math_1.b2Vec2();
this.m_JwA = 0;
this.m_JwB = 0;
this.m_JwC = 0;
this.m_JwD = 0;
this.m_mass = 0;
this.m_joint1 = def.joint1;
this.m_joint2 = def.joint2;
this.m_typeA = this.m_joint1.GetType();
this.m_typeB = this.m_joint2.GetType();
// DEBUG: b2Assert(this.m_typeA === b2JointType.e_revoluteJoint || this.m_typeA === b2JointType.e_prismaticJoint);
// DEBUG: b2Assert(this.m_typeB === b2JointType.e_revoluteJoint || this.m_typeB === b2JointType.e_prismaticJoint);
let coordinateA;
let coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
this.m_bodyC = this.m_joint1.GetBodyA();
this.m_bodyA = this.m_joint1.GetBodyB();
// Body B on joint1 must be dynamic
// DEBUG: b2Assert(this.m_bodyA.m_type === b2BodyType.b2_dynamicBody);
// Get geometry of joint1
const xfA = this.m_bodyA.m_xf;
const aA = this.m_bodyA.m_sweep.a;
const xfC = this.m_bodyC.m_xf;
const aC = this.m_bodyC.m_sweep.a;
if (this.m_typeA === b2_joint_1.b2JointType.e_revoluteJoint) {
const revolute = def.joint1;
this.m_localAnchorC.Copy(revolute.m_localAnchorA);
this.m_localAnchorA.Copy(revolute.m_localAnchorB);
this.m_referenceAngleA = revolute.m_referenceAngle;
this.m_localAxisC.SetZero();
coordinateA = aA - aC - this.m_referenceAngleA;
// position error is measured in radians
this.m_tolerance = b2_common_1.b2_angularSlop;
}
else {
const prismatic = def.joint1;
this.m_localAnchorC.Copy(prismatic.m_localAnchorA);
this.m_localAnchorA.Copy(prismatic.m_localAnchorB);
this.m_referenceAngleA = prismatic.m_referenceAngle;
this.m_localAxisC.Copy(prismatic.m_localXAxisA);
const pC = this.m_localAnchorC;
const pA = b2_math_1.b2Rot.TransposeMultiplyVec2(xfC.q, b2_math_1.b2Rot.MultiplyVec2(xfA.q, this.m_localAnchorA, b2_math_1.b2Vec2.s_t0).Add(xfA.p).Subtract(xfC.p), b2_math_1.b2Vec2.s_t0);
coordinateA = b2_math_1.b2Vec2.Dot(pA.Subtract(pC), this.m_localAxisC);
// position error is measured in meters
this.m_tolerance = b2_common_1.b2_linearSlop;
}
this.m_bodyD = this.m_joint2.GetBodyA();
this.m_bodyB = this.m_joint2.GetBodyB();
// Body B on joint2 must be dynamic
// DEBUG: b2Assert(this.m_bodyB.m_type === b2BodyType.b2_dynamicBody);
// Get geometry of joint2
const xfB = this.m_bodyB.m_xf;
const aB = this.m_bodyB.m_sweep.a;
const xfD = this.m_bodyD.m_xf;
const aD = this.m_bodyD.m_sweep.a;
if (this.m_typeB === b2_joint_1.b2JointType.e_revoluteJoint) {
const revolute = def.joint2;
this.m_localAnchorD.Copy(revolute.m_localAnchorA);
this.m_localAnchorB.Copy(revolute.m_localAnchorB);
this.m_referenceAngleB = revolute.m_referenceAngle;
this.m_localAxisD.SetZero();
coordinateB = aB - aD - this.m_referenceAngleB;
}
else {
const prismatic = def.joint2;
this.m_localAnchorD.Copy(prismatic.m_localAnchorA);
this.m_localAnchorB.Copy(prismatic.m_localAnchorB);
this.m_referenceAngleB = prismatic.m_referenceAngle;
this.m_localAxisD.Copy(prismatic.m_localXAxisA);
const pD = this.m_localAnchorD;
const pB = b2_math_1.b2Rot.TransposeMultiplyVec2(xfD.q, b2_math_1.b2Rot.MultiplyVec2(xfB.q, this.m_localAnchorB, b2_math_1.b2Vec2.s_t0).Add(xfB.p).Subtract(xfD.p), b2_math_1.b2Vec2.s_t0);
coordinateB = b2_math_1.b2Vec2.Dot(pB.Subtract(pD), this.m_localAxisD);
}
this.m_ratio = (_a = def.ratio) !== null && _a !== void 0 ? _a : 1;
this.m_constant = coordinateA + this.m_ratio * coordinateB;
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_indexC = this.m_bodyC.m_islandIndex;
this.m_indexD = this.m_bodyD.m_islandIndex;
this.m_lcA.Copy(this.m_bodyA.m_sweep.localCenter);
this.m_lcB.Copy(this.m_bodyB.m_sweep.localCenter);
this.m_lcC.Copy(this.m_bodyC.m_sweep.localCenter);
this.m_lcD.Copy(this.m_bodyD.m_sweep.localCenter);
this.m_mA = this.m_bodyA.m_invMass;
this.m_mB = this.m_bodyB.m_invMass;
this.m_mC = this.m_bodyC.m_invMass;
this.m_mD = this.m_bodyD.m_invMass;
this.m_iA = this.m_bodyA.m_invI;
this.m_iB = this.m_bodyB.m_invI;
this.m_iC = this.m_bodyC.m_invI;
this.m_iD = this.m_bodyD.m_invI;
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 aB = data.positions[this.m_indexB].a;
const vB = data.velocities[this.m_indexB].v;
let wB = data.velocities[this.m_indexB].w;
const aC = data.positions[this.m_indexC].a;
const vC = data.velocities[this.m_indexC].v;
let wC = data.velocities[this.m_indexC].w;
const aD = data.positions[this.m_indexD].a;
const vD = data.velocities[this.m_indexD].v;
let wD = data.velocities[this.m_indexD].w;
const { qA, qB, qC, qD } = temp;
qA.Set(aA);
qB.Set(aB);
qC.Set(aC);
qD.Set(aD);
this.m_mass = 0;
if (this.m_typeA === b2_joint_1.b2JointType.e_revoluteJoint) {
this.m_JvAC.SetZero();
this.m_JwA = 1;
this.m_JwC = 1;
this.m_mass += this.m_iA + this.m_iC;
}
else {
const { u, rC, rA, lalcA, lalcC } = temp;
b2_math_1.b2Rot.MultiplyVec2(qC, this.m_localAxisC, u);
b2_math_1.b2Rot.MultiplyVec2(qC, b2_math_1.b2Vec2.Subtract(this.m_localAnchorC, this.m_lcC, lalcC), rC);
b2_math_1.b2Rot.MultiplyVec2(qA, b2_math_1.b2Vec2.Subtract(this.m_localAnchorA, this.m_lcA, lalcA), rA);
this.m_JvAC.Copy(u);
this.m_JwC = b2_math_1.b2Vec2.Cross(rC, u);
this.m_JwA = b2_math_1.b2Vec2.Cross(rA, u);
this.m_mass +=
this.m_mC + this.m_mA + this.m_iC * this.m_JwC * this.m_JwC + this.m_iA * this.m_JwA * this.m_JwA;
}
if (this.m_typeB === b2_joint_1.b2JointType.e_revoluteJoint) {
this.m_JvBD.SetZero();
this.m_JwB = this.m_ratio;
this.m_JwD = this.m_ratio;
this.m_mass += this.m_ratio * this.m_ratio * (this.m_iB + this.m_iD);
}
else {
const { u, rB, rD, lalcB, lalcD } = temp;
b2_math_1.b2Rot.MultiplyVec2(qD, this.m_localAxisD, u);
b2_math_1.b2Rot.MultiplyVec2(qD, b2_math_1.b2Vec2.Subtract(this.m_localAnchorD, this.m_lcD, lalcD), rD);
b2_math_1.b2Rot.MultiplyVec2(qB, b2_math_1.b2Vec2.Subtract(this.m_localAnchorB, this.m_lcB, lalcB), rB);
b2_math_1.b2Vec2.Scale(this.m_ratio, u, this.m_JvBD);
this.m_JwD = this.m_ratio * b2_math_1.b2Vec2.Cross(rD, u);
this.m_JwB = this.m_ratio * b2_math_1.b2Vec2.Cross(rB, u);
this.m_mass +=
this.m_ratio * this.m_ratio * (this.m_mD + this.m_mB) +
this.m_iD * this.m_JwD * this.m_JwD +
this.m_iB * this.m_JwB * this.m_JwB;
}
// Compute effective mass.
this.m_mass = this.m_mass > 0 ? 1 / this.m_mass : 0;
if (data.step.warmStarting) {
vA.AddScaled(this.m_mA * this.m_impulse, this.m_JvAC);
wA += this.m_iA * this.m_impulse * this.m_JwA;
vB.AddScaled(this.m_mB * this.m_impulse, this.m_JvBD);
wB += this.m_iB * this.m_impulse * this.m_JwB;
vC.SubtractScaled(this.m_mC * this.m_impulse, this.m_JvAC);
wC -= this.m_iC * this.m_impulse * this.m_JwC;
vD.SubtractScaled(this.m_mD * this.m_impulse, this.m_JvBD);
wD -= this.m_iD * this.m_impulse * this.m_JwD;
}
else {
this.m_impulse = 0;
}
data.velocities[this.m_indexA].w = wA;
data.velocities[this.m_indexB].w = wB;
data.velocities[this.m_indexC].w = wC;
data.velocities[this.m_indexD].w = wD;
}
/** @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 vC = data.velocities[this.m_indexC].v;
let wC = data.velocities[this.m_indexC].w;
const vD = data.velocities[this.m_indexD].v;
let wD = data.velocities[this.m_indexD].w;
let Cdot = b2_math_1.b2Vec2.Dot(this.m_JvAC, b2_math_1.b2Vec2.Subtract(vA, vC, b2_math_1.b2Vec2.s_t0)) +
b2_math_1.b2Vec2.Dot(this.m_JvBD, b2_math_1.b2Vec2.Subtract(vB, vD, b2_math_1.b2Vec2.s_t0));
Cdot += this.m_JwA * wA - this.m_JwC * wC + (this.m_JwB * wB - this.m_JwD * wD);
const impulse = -this.m_mass * Cdot;
this.m_impulse += impulse;
vA.AddScaled(this.m_mA * impulse, this.m_JvAC);
wA += this.m_iA * impulse * this.m_JwA;
vB.AddScaled(this.m_mB * impulse, this.m_JvBD);
wB += this.m_iB * impulse * this.m_JwB;
vC.SubtractScaled(this.m_mC * impulse, this.m_JvAC);
wC -= this.m_iC * impulse * this.m_JwC;
vD.SubtractScaled(this.m_mD * impulse, this.m_JvBD);
wD -= this.m_iD * impulse * this.m_JwD;
data.velocities[this.m_indexA].w = wA;
data.velocities[this.m_indexB].w = wB;
data.velocities[this.m_indexC].w = wC;
data.velocities[this.m_indexD].w = wD;
}
/** @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 cC = data.positions[this.m_indexC].c;
let aC = data.positions[this.m_indexC].a;
const cD = data.positions[this.m_indexD].c;
let aD = data.positions[this.m_indexD].a;
const { qA, qB, qC, qD, JvAC, JvBD } = temp;
qA.Set(aA);
qB.Set(aB);
qC.Set(aC);
qD.Set(aD);
let coordinateA;
let coordinateB;
let JwA;
let JwB;
let JwC;
let JwD;
let mass = 0;
if (this.m_typeA === b2_joint_1.b2JointType.e_revoluteJoint) {
JvAC.SetZero();
JwA = 1;
JwC = 1;
mass += this.m_iA + this.m_iC;
coordinateA = aA - aC - this.m_referenceAngleA;
}
else {
const { u, rC, rA, lalcC, lalcA } = temp;
b2_math_1.b2Rot.MultiplyVec2(qC, this.m_localAxisC, u);
b2_math_1.b2Rot.MultiplyVec2(qC, b2_math_1.b2Vec2.Subtract(this.m_localAnchorC, this.m_lcC, lalcC), rC);
b2_math_1.b2Rot.MultiplyVec2(qA, b2_math_1.b2Vec2.Subtract(this.m_localAnchorA, this.m_lcA, lalcA), rA);
JvAC.Copy(u);
JwC = b2_math_1.b2Vec2.Cross(rC, u);
JwA = b2_math_1.b2Vec2.Cross(rA, u);
mass += this.m_mC + this.m_mA + this.m_iC * JwC * JwC + this.m_iA * JwA * JwA;
const pC = lalcC;
const pA = b2_math_1.b2Rot.TransposeMultiplyVec2(qC, b2_math_1.b2Vec2.Add(rA, cA, b2_math_1.b2Vec2.s_t0).Subtract(cC), b2_math_1.b2Vec2.s_t0);
coordinateA = b2_math_1.b2Vec2.Dot(b2_math_1.b2Vec2.Subtract(pA, pC, b2_math_1.b2Vec2.s_t0), this.m_localAxisC);
}
if (this.m_typeB === b2_joint_1.b2JointType.e_revoluteJoint) {
JvBD.SetZero();
JwB = this.m_ratio;
JwD = this.m_ratio;
mass += this.m_ratio * this.m_ratio * (this.m_iB + this.m_iD);
coordinateB = aB - aD - this.m_referenceAngleB;
}
else {
const { u, rD, rB, lalcD, lalcB } = temp;
b2_math_1.b2Rot.MultiplyVec2(qD, this.m_localAxisD, u);
b2_math_1.b2Rot.MultiplyVec2(qD, b2_math_1.b2Vec2.Subtract(this.m_localAnchorD, this.m_lcD, lalcD), rD);
b2_math_1.b2Rot.MultiplyVec2(qB, b2_math_1.b2Vec2.Subtract(this.m_localAnchorB, this.m_lcB, lalcB), rB);
b2_math_1.b2Vec2.Scale(this.m_ratio, u, JvBD);
JwD = this.m_ratio * b2_math_1.b2Vec2.Cross(rD, u);
JwB = this.m_ratio * b2_math_1.b2Vec2.Cross(rB, u);
mass +=
this.m_ratio * this.m_ratio * (this.m_mD + this.m_mB) + this.m_iD * JwD * JwD + this.m_iB * JwB * JwB;
const pD = lalcD;
const pB = b2_math_1.b2Rot.TransposeMultiplyVec2(qD, b2_math_1.b2Vec2.Add(rB, cB, b2_math_1.b2Vec2.s_t0).Subtract(cD), b2_math_1.b2Vec2.s_t0);
coordinateB = b2_math_1.b2Vec2.Dot(pB.Subtract(pD), this.m_localAxisD);
}
const C = coordinateA + this.m_ratio * coordinateB - this.m_constant;
let impulse = 0;
if (mass > 0) {
impulse = -C / mass;
}
cA.AddScaled(this.m_mA * impulse, JvAC);
aA += this.m_iA * impulse * JwA;
cB.AddScaled(this.m_mB * impulse, JvBD);
aB += this.m_iB * impulse * JwB;
cC.SubtractScaled(this.m_mC * impulse, JvAC);
aC -= this.m_iC * impulse * JwC;
cD.SubtractScaled(this.m_mD * impulse, JvBD);
aD -= this.m_iD * impulse * JwD;
data.positions[this.m_indexA].a = aA;
data.positions[this.m_indexB].a = aB;
data.positions[this.m_indexC].a = aC;
data.positions[this.m_indexD].a = aD;
if (Math.abs(C) < this.m_tolerance) {
return true;
}
return false;
}
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) {
return b2_math_1.b2Vec2.Scale(inv_dt * this.m_impulse, this.m_JvAC, out);
}
GetReactionTorque(inv_dt) {
return inv_dt * this.m_impulse * this.m_JwA;
}
/** Get the first joint. */
GetJoint1() {
return this.m_joint1;
}
/** Get the second joint. */
GetJoint2() {
return this.m_joint2;
}
/** Get the gear ratio. */
GetRatio() {
return this.m_ratio;
}
/** Set the gear ratio. */
SetRatio(ratio) {
// DEBUG: b2Assert(Number.isFinite(ratio));
this.m_ratio = ratio;
}
}
exports.b2GearJoint = b2GearJoint;