the-world-engine
Version:
three.js based, unity like game engine for browser
561 lines (540 loc) • 22.7 kB
JavaScript
import { b2_linearSlop, b2Maybe } from "../common/b2_settings.js";
import { b2Abs, b2Clamp, b2Vec2, b2Rot, b2Max, b2Min, b2Transform } from "../common/b2_math.js";
import { b2Joint, b2JointDef, b2JointType } from "./b2_joint.js";
import { b2Color } from "../common/b2_draw.js";
export class b2WheelJointDef extends b2JointDef {
constructor() {
super(b2JointType.e_wheelJoint);
this.localAnchorA = new b2Vec2(0, 0);
this.localAnchorB = new b2Vec2(0, 0);
this.localAxisA = new b2Vec2(1, 0);
this.enableLimit = false;
this.lowerTranslation = 0;
this.upperTranslation = 0;
this.enableMotor = false;
this.maxMotorTorque = 0;
this.motorSpeed = 0;
this.stiffness = 0;
this.damping = 0;
}
Initialize(t, s, i, h) {
this.bodyA = t;
this.bodyB = s;
this.bodyA.GetLocalPoint(i, this.localAnchorA);
this.bodyB.GetLocalPoint(i, this.localAnchorB);
this.bodyA.GetLocalVector(h, this.localAxisA);
}
}
export class b2WheelJoint extends b2Joint {
constructor(t) {
super(t);
this.m_localAnchorA = new b2Vec2;
this.m_localAnchorB = new b2Vec2;
this.m_localXAxisA = new b2Vec2;
this.m_localYAxisA = new b2Vec2;
this.m_impulse = 0;
this.m_motorImpulse = 0;
this.m_springImpulse = 0;
this.m_lowerImpulse = 0;
this.m_upperImpulse = 0;
this.m_translation = 0;
this.m_lowerTranslation = 0;
this.m_upperTranslation = 0;
this.m_maxMotorTorque = 0;
this.m_motorSpeed = 0;
this.m_enableLimit = false;
this.m_enableMotor = false;
this.m_stiffness = 0;
this.m_damping = 0;
this.m_indexA = 0;
this.m_indexB = 0;
this.m_localCenterA = new b2Vec2;
this.m_localCenterB = new b2Vec2;
this.m_invMassA = 0;
this.m_invMassB = 0;
this.m_invIA = 0;
this.m_invIB = 0;
this.m_ax = new b2Vec2;
this.m_ay = new b2Vec2;
this.m_sAx = 0;
this.m_sBx = 0;
this.m_sAy = 0;
this.m_sBy = 0;
this.m_mass = 0;
this.m_motorMass = 0;
this.m_axialMass = 0;
this.m_springMass = 0;
this.m_bias = 0;
this.m_gamma = 0;
this.m_qA = new b2Rot;
this.m_qB = new b2Rot;
this.m_lalcA = new b2Vec2;
this.m_lalcB = new b2Vec2;
this.m_rA = new b2Vec2;
this.m_rB = new b2Vec2;
this.m_localAnchorA.Copy(b2Maybe(t.localAnchorA, b2Vec2.ZERO));
this.m_localAnchorB.Copy(b2Maybe(t.localAnchorB, b2Vec2.ZERO));
this.m_localXAxisA.Copy(b2Maybe(t.localAxisA, b2Vec2.UNITX));
b2Vec2.CrossOneV(this.m_localXAxisA, this.m_localYAxisA);
this.m_lowerTranslation = b2Maybe(t.lowerTranslation, 0);
this.m_upperTranslation = b2Maybe(t.upperTranslation, 0);
this.m_enableLimit = b2Maybe(t.enableLimit, false);
this.m_maxMotorTorque = b2Maybe(t.maxMotorTorque, 0);
this.m_motorSpeed = b2Maybe(t.motorSpeed, 0);
this.m_enableMotor = b2Maybe(t.enableMotor, false);
this.m_ax.SetZero();
this.m_ay.SetZero();
this.m_stiffness = b2Maybe(t.stiffness, 0);
this.m_damping = b2Maybe(t.damping, 0);
}
GetMotorSpeed() {
return this.m_motorSpeed;
}
GetMaxMotorTorque() {
return this.m_maxMotorTorque;
}
SetSpringFrequencyHz(t) {
this.m_stiffness = t;
}
GetSpringFrequencyHz() {
return this.m_stiffness;
}
SetSpringDampingRatio(t) {
this.m_damping = t;
}
GetSpringDampingRatio() {
return this.m_damping;
}
InitVelocityConstraints(t) {
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 s = this.m_invMassA, i = this.m_invMassB;
const h = this.m_invIA, e = this.m_invIB;
const n = t.positions[this.m_indexA].c;
const o = t.positions[this.m_indexA].a;
const c = t.velocities[this.m_indexA].v;
let b = t.velocities[this.m_indexA].w;
const r = t.positions[this.m_indexB].c;
const V = t.positions[this.m_indexB].a;
const l = t.velocities[this.m_indexB].v;
let a = t.velocities[this.m_indexB].w;
const f = this.m_qA.SetAngle(o), J = this.m_qB.SetAngle(V);
b2Vec2.SubVV(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
const u = b2Rot.MulRV(f, this.m_lalcA, this.m_rA);
b2Vec2.SubVV(this.m_localAnchorB, this.m_localCenterB, this.m_lalcB);
const w = b2Rot.MulRV(J, this.m_lalcB, this.m_rB);
const W = b2Vec2.SubVV(b2Vec2.AddVV(r, w, b2Vec2.s_t0), b2Vec2.AddVV(n, u, b2Vec2.s_t1), b2WheelJoint.InitVelocityConstraints_s_d);
{
b2Rot.MulRV(f, this.m_localYAxisA, this.m_ay);
this.m_sAy = b2Vec2.CrossVV(b2Vec2.AddVV(W, u, b2Vec2.s_t0), this.m_ay);
this.m_sBy = b2Vec2.CrossVV(w, this.m_ay);
this.m_mass = s + i + h * this.m_sAy * this.m_sAy + e * this.m_sBy * this.m_sBy;
if (this.m_mass > 0) {
this.m_mass = 1 / this.m_mass;
}
}
b2Rot.MulRV(f, this.m_localXAxisA, this.m_ax);
this.m_sAx = b2Vec2.CrossVV(b2Vec2.AddVV(W, u, b2Vec2.s_t0), this.m_ax);
this.m_sBx = b2Vec2.CrossVV(w, this.m_ax);
const d = s + i + h * this.m_sAx * this.m_sAx + e * this.m_sBx * this.m_sBx;
if (d > 0) {
this.m_axialMass = 1 / d;
} else {
this.m_axialMass = 0;
}
this.m_springMass = 0;
this.m_bias = 0;
this.m_gamma = 0;
if (this.m_stiffness > 0 && d > 0) {
this.m_springMass = 1 / d;
const s = b2Vec2.DotVV(W, this.m_ax);
const i = t.step.dt;
this.m_gamma = i * (this.m_damping + i * this.m_stiffness);
if (this.m_gamma > 0) {
this.m_gamma = 1 / this.m_gamma;
}
this.m_bias = s * i * this.m_stiffness * this.m_gamma;
this.m_springMass = d + this.m_gamma;
if (this.m_springMass > 0) {
this.m_springMass = 1 / this.m_springMass;
}
} else {
this.m_springImpulse = 0;
}
if (this.m_enableLimit) {
this.m_translation = b2Vec2.DotVV(this.m_ax, W);
} else {
this.m_lowerImpulse = 0;
this.m_upperImpulse = 0;
}
if (this.m_enableMotor) {
this.m_motorMass = h + e;
if (this.m_motorMass > 0) {
this.m_motorMass = 1 / this.m_motorMass;
}
} else {
this.m_motorMass = 0;
this.m_motorImpulse = 0;
}
if (t.step.warmStarting) {
this.m_impulse *= t.step.dtRatio;
this.m_springImpulse *= t.step.dtRatio;
this.m_motorImpulse *= t.step.dtRatio;
const s = this.m_springImpulse + this.m_lowerImpulse - this.m_upperImpulse;
const i = b2Vec2.AddVV(b2Vec2.MulSV(this.m_impulse, this.m_ay, b2Vec2.s_t0), b2Vec2.MulSV(s, this.m_ax, b2Vec2.s_t1), b2WheelJoint.InitVelocityConstraints_s_P);
const h = this.m_impulse * this.m_sAy + s * this.m_sAx + this.m_motorImpulse;
const e = this.m_impulse * this.m_sBy + s * this.m_sBx + this.m_motorImpulse;
c.SelfMulSub(this.m_invMassA, i);
b -= this.m_invIA * h;
l.SelfMulAdd(this.m_invMassB, i);
a += this.m_invIB * e;
} else {
this.m_impulse = 0;
this.m_springImpulse = 0;
this.m_motorImpulse = 0;
this.m_lowerImpulse = 0;
this.m_upperImpulse = 0;
}
t.velocities[this.m_indexA].w = b;
t.velocities[this.m_indexB].w = a;
}
SolveVelocityConstraints(t) {
const s = this.m_invMassA, i = this.m_invMassB;
const h = this.m_invIA, e = this.m_invIB;
const n = t.velocities[this.m_indexA].v;
let o = t.velocities[this.m_indexA].w;
const c = t.velocities[this.m_indexB].v;
let b = t.velocities[this.m_indexB].w;
{
const t = b2Vec2.DotVV(this.m_ax, b2Vec2.SubVV(c, n, b2Vec2.s_t0)) + this.m_sBx * b - this.m_sAx * o;
const r = -this.m_springMass * (t + this.m_bias + this.m_gamma * this.m_springImpulse);
this.m_springImpulse += r;
const V = b2Vec2.MulSV(r, this.m_ax, b2WheelJoint.SolveVelocityConstraints_s_P);
const l = r * this.m_sAx;
const a = r * this.m_sBx;
n.SelfMulSub(s, V);
o -= h * l;
c.SelfMulAdd(i, V);
b += e * a;
}
{
const s = b - o - this.m_motorSpeed;
let i = -this.m_motorMass * s;
const n = this.m_motorImpulse;
const c = t.step.dt * this.m_maxMotorTorque;
this.m_motorImpulse = b2Clamp(this.m_motorImpulse + i, -c, c);
i = this.m_motorImpulse - n;
o -= h * i;
b += e * i;
}
if (this.m_enableLimit) {
{
const r = this.m_translation - this.m_lowerTranslation;
const V = b2Vec2.DotVV(this.m_ax, b2Vec2.SubVV(c, n, b2Vec2.s_t0)) + this.m_sBx * b - this.m_sAx * o;
let l = -this.m_axialMass * (V + b2Max(r, 0) * t.step.inv_dt);
const a = this.m_lowerImpulse;
this.m_lowerImpulse = b2Max(this.m_lowerImpulse + l, 0);
l = this.m_lowerImpulse - a;
const f = b2Vec2.MulSV(l, this.m_ax, b2WheelJoint.SolveVelocityConstraints_s_P);
const J = l * this.m_sAx;
const u = l * this.m_sBx;
n.SelfMulSub(s, f);
o -= h * J;
c.SelfMulAdd(i, f);
b += e * u;
}
{
const r = this.m_upperTranslation - this.m_translation;
const V = b2Vec2.DotVV(this.m_ax, b2Vec2.SubVV(n, c, b2Vec2.s_t0)) + this.m_sAx * o - this.m_sBx * b;
let l = -this.m_axialMass * (V + b2Max(r, 0) * t.step.inv_dt);
const a = this.m_upperImpulse;
this.m_upperImpulse = b2Max(this.m_upperImpulse + l, 0);
l = this.m_upperImpulse - a;
const f = b2Vec2.MulSV(l, this.m_ax, b2WheelJoint.SolveVelocityConstraints_s_P);
const J = l * this.m_sAx;
const u = l * this.m_sBx;
n.SelfMulAdd(s, f);
o += h * J;
c.SelfMulSub(i, f);
b -= e * u;
}
}
{
const t = b2Vec2.DotVV(this.m_ay, b2Vec2.SubVV(c, n, b2Vec2.s_t0)) + this.m_sBy * b - this.m_sAy * o;
const r = -this.m_mass * t;
this.m_impulse += r;
const V = b2Vec2.MulSV(r, this.m_ay, b2WheelJoint.SolveVelocityConstraints_s_P);
const l = r * this.m_sAy;
const a = r * this.m_sBy;
n.SelfMulSub(s, V);
o -= h * l;
c.SelfMulAdd(i, V);
b += e * a;
}
t.velocities[this.m_indexA].w = o;
t.velocities[this.m_indexB].w = b;
}
SolvePositionConstraints(t) {
const s = t.positions[this.m_indexA].c;
let i = t.positions[this.m_indexA].a;
const h = t.positions[this.m_indexB].c;
let e = t.positions[this.m_indexB].a;
let n = 0;
if (this.m_enableLimit) {
const t = this.m_qA.SetAngle(i), o = this.m_qB.SetAngle(e);
b2Vec2.SubVV(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
const c = b2Rot.MulRV(t, this.m_lalcA, this.m_rA);
b2Vec2.SubVV(this.m_localAnchorB, this.m_localCenterB, this.m_lalcB);
const b = b2Rot.MulRV(o, this.m_lalcB, this.m_rB);
const r = b2Vec2.AddVV(b2Vec2.SubVV(h, s, b2Vec2.s_t0), b2Vec2.SubVV(b, c, b2Vec2.s_t1), b2WheelJoint.SolvePositionConstraints_s_d);
const V = b2Rot.MulRV(t, this.m_localXAxisA, this.m_ax);
const l = b2Vec2.CrossVV(b2Vec2.AddVV(r, c, b2Vec2.s_t0), this.m_ax);
const a = b2Vec2.CrossVV(b, this.m_ax);
let f = 0;
const J = b2Vec2.DotVV(V, r);
if (b2Abs(this.m_upperTranslation - this.m_lowerTranslation) < 2 * b2_linearSlop) {
f = J;
} else if (J <= this.m_lowerTranslation) {
f = b2Min(J - this.m_lowerTranslation, 0);
} else if (J >= this.m_upperTranslation) {
f = b2Max(J - this.m_upperTranslation, 0);
}
if (f !== 0) {
const t = this.m_invMassA + this.m_invMassB + this.m_invIA * l * l + this.m_invIB * a * a;
let o = 0;
if (t !== 0) {
o = -f / t;
}
const c = b2Vec2.MulSV(o, V, b2WheelJoint.SolvePositionConstraints_s_P);
const b = o * l;
const r = o * a;
s.SelfMulSub(this.m_invMassA, c);
i -= this.m_invIA * b;
h.SelfMulAdd(this.m_invMassB, c);
e += this.m_invIB * r;
n = b2Abs(f);
}
}
{
const t = this.m_qA.SetAngle(i), o = this.m_qB.SetAngle(e);
b2Vec2.SubVV(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
const c = b2Rot.MulRV(t, this.m_lalcA, this.m_rA);
b2Vec2.SubVV(this.m_localAnchorB, this.m_localCenterB, this.m_lalcB);
const b = b2Rot.MulRV(o, this.m_lalcB, this.m_rB);
const r = b2Vec2.AddVV(b2Vec2.SubVV(h, s, b2Vec2.s_t0), b2Vec2.SubVV(b, c, b2Vec2.s_t1), b2WheelJoint.SolvePositionConstraints_s_d);
const V = b2Rot.MulRV(t, this.m_localYAxisA, this.m_ay);
const l = b2Vec2.CrossVV(b2Vec2.AddVV(r, c, b2Vec2.s_t0), V);
const a = b2Vec2.CrossVV(b, V);
const f = b2Vec2.DotVV(r, V);
const J = this.m_invMassA + this.m_invMassB + this.m_invIA * this.m_sAy * this.m_sAy + this.m_invIB * this.m_sBy * this.m_sBy;
let u = 0;
if (J !== 0) {
u = -f / J;
}
const w = b2Vec2.MulSV(u, V, b2WheelJoint.SolvePositionConstraints_s_P);
const W = u * l;
const d = u * a;
s.SelfMulSub(this.m_invMassA, w);
i -= this.m_invIA * W;
h.SelfMulAdd(this.m_invMassB, w);
e += this.m_invIB * d;
n = b2Max(n, b2Abs(f));
}
t.positions[this.m_indexA].a = i;
t.positions[this.m_indexB].a = e;
return n <= b2_linearSlop;
}
GetDefinition(t) {
return t;
}
GetAnchorA(t) {
return this.m_bodyA.GetWorldPoint(this.m_localAnchorA, t);
}
GetAnchorB(t) {
return this.m_bodyB.GetWorldPoint(this.m_localAnchorB, t);
}
GetReactionForce(t, s) {
s.x = t * (this.m_impulse * this.m_ay.x + (this.m_springImpulse + this.m_lowerImpulse - this.m_upperImpulse) * this.m_ax.x);
s.y = t * (this.m_impulse * this.m_ay.y + (this.m_springImpulse + this.m_lowerImpulse - this.m_upperImpulse) * this.m_ax.y);
return s;
}
GetReactionTorque(t) {
return t * this.m_motorImpulse;
}
GetLocalAnchorA() {
return this.m_localAnchorA;
}
GetLocalAnchorB() {
return this.m_localAnchorB;
}
GetLocalAxisA() {
return this.m_localXAxisA;
}
GetJointTranslation() {
return this.GetPrismaticJointTranslation();
}
GetJointLinearSpeed() {
return this.GetPrismaticJointSpeed();
}
GetJointAngle() {
return this.GetRevoluteJointAngle();
}
GetJointAngularSpeed() {
return this.GetRevoluteJointSpeed();
}
GetPrismaticJointTranslation() {
const t = this.m_bodyA;
const s = this.m_bodyB;
const i = t.GetWorldPoint(this.m_localAnchorA, new b2Vec2);
const h = s.GetWorldPoint(this.m_localAnchorB, new b2Vec2);
const e = b2Vec2.SubVV(h, i, new b2Vec2);
const n = t.GetWorldVector(this.m_localXAxisA, new b2Vec2);
const o = b2Vec2.DotVV(e, n);
return o;
}
GetPrismaticJointSpeed() {
const t = this.m_bodyA;
const s = this.m_bodyB;
b2Vec2.SubVV(this.m_localAnchorA, t.m_sweep.localCenter, this.m_lalcA);
const i = b2Rot.MulRV(t.m_xf.q, this.m_lalcA, this.m_rA);
b2Vec2.SubVV(this.m_localAnchorB, s.m_sweep.localCenter, this.m_lalcB);
const h = b2Rot.MulRV(s.m_xf.q, this.m_lalcB, this.m_rB);
const e = b2Vec2.AddVV(t.m_sweep.c, i, b2Vec2.s_t0);
const n = b2Vec2.AddVV(s.m_sweep.c, h, b2Vec2.s_t1);
const o = b2Vec2.SubVV(n, e, b2Vec2.s_t2);
const c = t.GetWorldVector(this.m_localXAxisA, new b2Vec2);
const b = t.m_linearVelocity;
const r = s.m_linearVelocity;
const V = t.m_angularVelocity;
const l = s.m_angularVelocity;
const a = b2Vec2.DotVV(o, b2Vec2.CrossSV(V, c, b2Vec2.s_t0)) + b2Vec2.DotVV(c, b2Vec2.SubVV(b2Vec2.AddVCrossSV(r, l, h, b2Vec2.s_t0), b2Vec2.AddVCrossSV(b, V, i, b2Vec2.s_t1), b2Vec2.s_t0));
return a;
}
GetRevoluteJointAngle() {
return this.m_bodyB.m_sweep.a - this.m_bodyA.m_sweep.a;
}
GetRevoluteJointSpeed() {
const t = this.m_bodyA.m_angularVelocity;
const s = this.m_bodyB.m_angularVelocity;
return s - t;
}
IsMotorEnabled() {
return this.m_enableMotor;
}
EnableMotor(t) {
if (t !== this.m_enableMotor) {
this.m_bodyA.SetAwake(true);
this.m_bodyB.SetAwake(true);
this.m_enableMotor = t;
}
}
SetMotorSpeed(t) {
if (t !== this.m_motorSpeed) {
this.m_bodyA.SetAwake(true);
this.m_bodyB.SetAwake(true);
this.m_motorSpeed = t;
}
}
SetMaxMotorTorque(t) {
if (t !== this.m_maxMotorTorque) {
this.m_bodyA.SetAwake(true);
this.m_bodyB.SetAwake(true);
this.m_maxMotorTorque = t;
}
}
GetMotorTorque(t) {
return t * this.m_motorImpulse;
}
IsLimitEnabled() {
return this.m_enableLimit;
}
EnableLimit(t) {
if (t !== this.m_enableLimit) {
this.m_bodyA.SetAwake(true);
this.m_bodyB.SetAwake(true);
this.m_enableLimit = t;
this.m_lowerImpulse = 0;
this.m_upperImpulse = 0;
}
}
GetLowerLimit() {
return this.m_lowerTranslation;
}
GetUpperLimit() {
return this.m_upperTranslation;
}
SetLimits(t, s) {
if (t !== this.m_lowerTranslation || s !== this.m_upperTranslation) {
this.m_bodyA.SetAwake(true);
this.m_bodyB.SetAwake(true);
this.m_lowerTranslation = t;
this.m_upperTranslation = s;
this.m_lowerImpulse = 0;
this.m_upperImpulse = 0;
}
}
Dump(t) {
const s = this.m_bodyA.m_islandIndex;
const i = this.m_bodyB.m_islandIndex;
t(" const jd: b2WheelJointDef = new b2WheelJointDef();\n");
t(" jd.bodyA = bodies[%d];\n", s);
t(" jd.bodyB = bodies[%d];\n", i);
t(" jd.collideConnected = %s;\n", this.m_collideConnected ? "true" : "false");
t(" jd.localAnchorA.Set(%.15f, %.15f);\n", this.m_localAnchorA.x, this.m_localAnchorA.y);
t(" jd.localAnchorB.Set(%.15f, %.15f);\n", this.m_localAnchorB.x, this.m_localAnchorB.y);
t(" jd.localAxisA.Set(%.15f, %.15f);\n", this.m_localXAxisA.x, this.m_localXAxisA.y);
t(" jd.enableMotor = %s;\n", this.m_enableMotor ? "true" : "false");
t(" jd.motorSpeed = %.15f;\n", this.m_motorSpeed);
t(" jd.maxMotorTorque = %.15f;\n", this.m_maxMotorTorque);
t(" jd.stiffness = %.15f;\n", this.m_stiffness);
t(" jd.damping = %.15f;\n", this.m_damping);
t(" joints[%d] = this.m_world.CreateJoint(jd);\n", this.m_index);
}
Draw(t) {
const s = this.m_bodyA.GetTransform();
const i = this.m_bodyB.GetTransform();
const h = b2Transform.MulXV(s, this.m_localAnchorA, b2WheelJoint.Draw_s_pA);
const e = b2Transform.MulXV(i, this.m_localAnchorB, b2WheelJoint.Draw_s_pB);
const n = b2Rot.MulRV(s.q, this.m_localXAxisA, b2WheelJoint.Draw_s_axis);
const o = b2WheelJoint.Draw_s_c1;
const c = b2WheelJoint.Draw_s_c2;
const b = b2WheelJoint.Draw_s_c3;
const r = b2WheelJoint.Draw_s_c4;
const V = b2WheelJoint.Draw_s_c5;
t.DrawSegment(h, e, V);
if (this.m_enableLimit) {
const i = b2Vec2.AddVMulSV(h, this.m_lowerTranslation, n, b2WheelJoint.Draw_s_lower);
const e = b2Vec2.AddVMulSV(h, this.m_upperTranslation, n, b2WheelJoint.Draw_s_upper);
const r = b2Rot.MulRV(s.q, this.m_localYAxisA, b2WheelJoint.Draw_s_perp);
t.DrawSegment(i, e, o);
t.DrawSegment(b2Vec2.AddVMulSV(i, -.5, r, b2Vec2.s_t0), b2Vec2.AddVMulSV(i, .5, r, b2Vec2.s_t1), c);
t.DrawSegment(b2Vec2.AddVMulSV(e, -.5, r, b2Vec2.s_t0), b2Vec2.AddVMulSV(e, .5, r, b2Vec2.s_t1), b);
} else {
t.DrawSegment(b2Vec2.AddVMulSV(h, -1, n, b2Vec2.s_t0), b2Vec2.AddVMulSV(h, 1, n, b2Vec2.s_t1), o);
}
t.DrawPoint(h, 5, o);
t.DrawPoint(e, 5, r);
}
}
b2WheelJoint.InitVelocityConstraints_s_d = new b2Vec2;
b2WheelJoint.InitVelocityConstraints_s_P = new b2Vec2;
b2WheelJoint.SolveVelocityConstraints_s_P = new b2Vec2;
b2WheelJoint.SolvePositionConstraints_s_d = new b2Vec2;
b2WheelJoint.SolvePositionConstraints_s_P = new b2Vec2;
b2WheelJoint.Draw_s_pA = new b2Vec2;
b2WheelJoint.Draw_s_pB = new b2Vec2;
b2WheelJoint.Draw_s_axis = new b2Vec2;
b2WheelJoint.Draw_s_c1 = new b2Color(.7, .7, .7);
b2WheelJoint.Draw_s_c2 = new b2Color(.3, .9, .3);
b2WheelJoint.Draw_s_c3 = new b2Color(.9, .3, .3);
b2WheelJoint.Draw_s_c4 = new b2Color(.3, .3, .9);
b2WheelJoint.Draw_s_c5 = new b2Color(.4, .4, .4);
b2WheelJoint.Draw_s_lower = new b2Vec2;
b2WheelJoint.Draw_s_upper = new b2Vec2;
b2WheelJoint.Draw_s_perp = new b2Vec2;