//################################################################################################//
//################################################################################################//
// //
// ██ ██████ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// █████ ██ ██ ██ ██ █████ █████ █████ ██ ██ █████ ██ █████ █████ //
// ██ ██ ██████ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ ██ ██ █████ █████ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// █████ ██████ ██████████ ██ ██ █████ █████ ██ █████ █████ ██ ██ ██ ████ //
// //
//################################################################################################//
//################################################################################################//
// CLASS CONSTRUCTOR
/**
* @class b2WheelJoint
* @constructor
* @param {b2WheelJointDef} wheelJointDef
* @extends {b2Joint}
* @module Joints
*/
//TODO: test b2WheelJoint. Ported but not yet tested.
function b2WheelJoint( wheelJointDef ) {
/**
* Invokes parent class constructor function reference.
*/
this.constructor( wheelJointDef );
////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// ██████ ██ ██ //
// ██ ██ ██ //
// ██ ██ ████ █████ █████ █████ ████ █████ ██ █████ █████ //
// ██████ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ █████ ██ ██ ██ █████ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ █████ █████ █████ ██ ████ ██ █████ █████ //
// ██ //
// ██ //
// //
////////////////////////////////////////////////////////////////////////////////////////////////////
// property DECLARATIONS
/**
*
* @public
* @property m_localAnchorA
* @type {b2Vec2}
*/
this.m_localAnchorA = new b2Vec2;
/**
*
* @public
* @property m_localAnchorB
* @type {b2Vec2}
*/
this.m_localAnchorB = new b2Vec2;
/**
*
* @public
* @property m_localXAxisA
* @type {b2Vec2}
*/
this.m_localXAxisA = new b2Vec2;
/**
*
* @public
* @property m_localYAxisA
* @type {b2Vec2}
*/
this.m_localYAxisA = new b2Vec2;
/**
*
* @public
* @property m_localCenterA
* @type {b2Vec2}
*/
this.m_localCenterA = new b2Vec2;
/**
*
* @public
* @property m_localCenterB
* @type {b2Vec2}
*/
this.m_localCenterB = new b2Vec2;
/**
*
* @public
* @property m_ax
* @type {b2Vec2}
*/
this.m_ax = new b2Vec2;
/**
*
* @public
* @property m_ay
* @type {b2Vec2}
*/
this.m_ay = new b2Vec2;
/**
*
* @public
* @property m_qA
* @type {b2Rot}
*/
this.m_qA = new b2Rot;
/**
*
* @public
* @property m_qB
* @type {b2Rot}
*/
this.m_qB = new b2Rot;
/**
*
* @public
* @property m_lalcA
* @type {b2Vec2}
*/
this.m_lalcA = new b2Vec2;
/**
*
* @public
* @property m_lalcB
* @type {b2Vec2}
*/
this.m_lalcB = new b2Vec2;
/**
*
* @public
* @property m_rA
* @type {b2Vec2}
*/
this.m_rA = new b2Vec2;
/**
*
* @public
* @property m_rB
* @type {b2Vec2}
*/
this.m_rB = new b2Vec2;
// property INITIALISATIONS
this.m_localAnchorA.copy( wheelJointDef.localAnchorA );
this.m_localAnchorB.copy( wheelJointDef.localAnchorB );
this.m_localXAxisA.copy( wheelJointDef.localAxisA );
/*this.m_localYAxisA = */b2Vec2.numCross( 1.0, this.m_localXAxisA, this.m_localYAxisA );
this.m_ax.setZero();
this.m_ay.setZero();
/**
*
* @public
* @property m_dampingRatio
* @type {float}
*/
this.m_dampingRatio = wheelJointDef.dampingRatio;
/**
*
* @public
* @property m_frequencyHz
* @type {float}
*/
this.m_frequencyHz = wheelJointDef.frequencyHz;
/**
*
* @public
* @property m_maxMotorForce
* @type {float}
*/
this.m_maxMotorTorque = wheelJointDef.maxMotorTorque;
/**
*
* @public
* @property m_motorSpeed
* @type {float}
*/
this.m_motorSpeed = wheelJointDef.motorSpeed;
/**
*
* @public
* @property m_enableLimit
* @type {boolean}
* @default false
*/
this.m_enableLimit = wheelJointDef.enableLimit;
/**
*
* @public
* @property m_enableMotor
* @type {boolean}
* @default false
*/
this.m_enableMotor = wheelJointDef.enableMotor;
/**
*
* @public
* @property m_limitState
* @type {int}
*/
this.m_limitState = b2Joint.e_inactiveLimit;
/**
*
* @public
* @property m_motorMass
* @type {float}
* @default 0.0
*/
this.m_motorMass = 0.0;
/**
*
* @public
* @property m_impulse
* @type {float}
* @default 0.0
*/
this.m_impulse = 0.0;
/**
*
* @public
* @property m_motorImpulse
* @type {float}
* @default 0.0
*/
this.m_motorImpulse = 0.0;
/**
*
* @public
* @property m_springImpulse
* @type {float}
* @default 0.0
*/
this.m_springImpulse = 0.0;
// Solver temp
/**
*
* @public
* @property m_indexA
* @type {int}
* @default 0
*/
this.m_indexA = 0;
/**
*
* @public
* @property m_indexB
* @type {int}
* @default 0
*/
this.m_indexB = 0;
/**
*
* @public
* @property m_invMassA
* @type {float}
* @default 0.0
*/
this.m_invMassA = 0.0;
/**
*
* @public
* @property m_invMassB
* @type {float}
* @default 0.0
*/
this.m_invMassB = 0.0;
/**
*
* @public
* @property m_invIA
* @type {float}
* @default 0.0
*/
this.m_invIA = 0.0;
/**
*
* @public
* @property m_invIB
* @type {float}
* @default 0.0
*/
this.m_invIB = 0.0;
/**
*
* @public
* @property m_sAx
* @type {float}
* @default 0.0
*/
this.m_sAx = 0.0;
/**
*
* @public
* @property m_sBx
* @type {float}
* @default 0.0
*/
this.m_sBx = 0.0;
/**
*
* @public
* @property m_sAy
* @type {float}
* @default 0.0
*/
this.m_sAy = 0.0;
/**
*
* @public
* @property m_sBy
* @type {float}
* @default 0.0
*/
this.m_sBy = 0.0;
/**
*
* @public
* @property m_mass
* @type {float}
* @default 0.0
*/
this.m_mass = 0.0;
/**
*
* @public
* @property m_springMass
* @type {float}
* @default 0.0
*/
this.m_springMass = 0.0;
/**
*
* @public
* @property m_bias
* @type {float}
* @default 0.0
*/
this.m_bias = 0.0;
/**
*
* @public
* @property m_gamma
* @type {float}
* @default 0.0
*/
this.m_gamma = 0.0;
////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// ██ ██ ██ ██ //
// ██ ██ ██ //
// ██ █████ █████ █████ ████ ██ █████ █████ █████ █████ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ █████ ██ ██ ██ █████ ██ ██ ██ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ █████ ██ ██ ████ █████ ██ ██ █████ █████ //
// //
////////////////////////////////////////////////////////////////////////////////////////////////////
} b2WheelJoint.prototype = p = new b2Joint(); Box2D.b2WheelJoint = b2WheelJoint;
// STATIC CLASS PROPERTIES
/**
* Object pool for memory management.
*/
b2WheelJoint._B2VEC2_POOL0 = new b2Vec2;
b2WheelJoint._B2VEC2_POOL1 = new b2Vec2;
b2WheelJoint._B2VEC2_POOL2 = new b2Vec2;
b2WheelJoint._B2VEC2_POOL3 = new b2Vec2;
////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// ██ ██ ██ ██ ██ //
// ███ ███ ██ ██ ██ //
// ███████ █████ █████ █████ █████ █████ █████ //
// ██ █ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ █████ ██ ██ ██ ██ ██ ██ ██ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ █████ ████ ██ ██ █████ █████ █████ //
// //
////////////////////////////////////////////////////////////////////////////////////////////////////
// INSTANCE METHODS
/**
* @public
* @method getAnchorB
* @param {b2Vec2|Object=} [out=b2Vec2] reusable object
* @return {b2Vec2|Object} out
*/
p.getAnchorA = function ( out ) {
out = out || b2WheelJoint._B2VEC2_POOL0;
return this.m_bodyA.getWorldPoint( this.m_localAnchorA, out );
};
/**
* @public
* @method getAnchorB
* @param {b2Vec2|Object=} [out=b2Vec2] reusable object
* @return {b2Vec2|Object} out
*/
p.getAnchorB = function ( out ) {
out = out || b2WheelJoint._B2VEC2_POOL0;
return this.m_bodyB.getWorldPoint( this.m_localAnchorB, out );
};
/**
* Get the reaction force given the inverse time step.
* Unit is N.
*
* @public
* @method getReactionForce
* @param {float} invDeltaTime
* @param {b2Vec2|Object=} [out=b2Vec2] reusable object
* @return {b2Vec2|Object} out
*/
p.getReactionForce = function ( invDeltaTime, out ) {
out = out || b2WheelJoint._B2VEC2_POOL0;
out.x = invDeltaTime * (this.m_impulse * this.m_ay.x + this.m_springImpulse * this.m_ax.x);
out.y = invDeltaTime * (this.m_impulse * this.m_ay.y + this.m_springImpulse * this.m_ax.y);
return out;
};
/**
* Get the reaction torque given the inverse time step.
* Unit is N*m. This is always zero for a distance joint.
*
* @public
* @method getReactionTorque
* @param {float} invDeltaTime
* @return {float}
*/
p.getReactionTorque = function ( invDeltaTime ) {
return invDeltaTime * this.m_motorImpulse;
};
/**
* The local anchor point relative to bodyA's origin.
*
* @public
* @method getLocalAnchorA
* @param {b2Vec2|Object=} [out=b2Vec2] reusable object
* @return {b2Vec2|Object} out
*/
p.getLocalAnchorA = function ( out ) {
out = out || b2WheelJoint._B2VEC2_POOL0;
return out.copy( this.m_localAnchorA );
};
/**
* The local anchor point relative to bodyA's origin.
*
* @public
* @method getLocalAnchorB
* @param {b2Vec2|Object=} [out=b2Vec2] reusable object
* @return {b2Vec2|Object} out
*/
p.getLocalAnchorB = function ( out ) {
out = out || b2WheelJoint._B2VEC2_POOL0;
return out.copy( this.m_localAnchorB );
};
/**
* The local joint axis relative to bodyA.
*
* @public
* @method getLocalAxisA
* @param {float} out
* @return {float}
*/
p.getLocalAxisA = function ( out ) {
out = out || b2WheelJoint._B2VEC2_POOL0;
return out.copy( this.m_localXAxisA );
};
/**
* Get the position correction factor in the range [0,1].
*
* @public
* @method getJointTranslation
* @return {float}
*/
p.getJointTranslation = function () {
/**b2Body*/ var bA = this.m_bodyA;
/**b2Body*/ var bB = this.m_bodyB;
/**b2Vec2*/ var pA = bA.getWorldPoint(this.m_localAnchorA, b2WheelJoint._B2VEC2_POOL0);
/**b2Vec2*/ var pB = bB.getWorldPoint(this.m_localAnchorB, b2WheelJoint._B2VEC2_POOL1);
/**b2Vec2*/ var d = b2Vec2.subtract(pB, pA, b2WheelJoint._B2VEC2_POOL2);
/**b2Vec2*/ var axis = bA.getWorldVector(this.m_localXAxisA, b2WheelJoint._B2VEC2_POOL3);
/**float32*/var translation = b2Vec2.dot(d, axis);
return translation;
};
/**
* @public
* @method getJointSpeed
* @return {float}
*/
p.getJointSpeed = function () {
/*float32*/var wA = this.m_bodyA.m_angularVelocity;
/*float32*/var wB = this.m_bodyB.m_angularVelocity;
return wB - wA;
};
/**
* @public
* @method getLinearOffset
* @return {boolean}
*/
p.isMotorEnabled = function () {
return this.m_enableMotor;
};
/**
* Set the target angular offset, in radians.
*
* @public
* @method enableMotor
* @param {boolean} flag
* @return {void}
*/
p.enableMotor = function ( flag ) {
this.m_bodyA.setAwake( true );
this.m_bodyB.setAwake( true );
this.m_enableMotor = flag;
};
/**
* Set the motor speed, usually in radians per second.
*
* @public
* @method setMotorSpeed
* @param {b2Vec2|Object=} speed
* @return {void}
*/
p.setMotorSpeed = function ( speed ) {
this.m_bodyA.setAwake( true );
this.m_bodyB.setAwake( true );
this.m_motorSpeed = speed;
};
/**
* Set the maximum motor force, usually in N-m.
*
* @public
* @method setMaxMotorTorque
* @param {float} torque
* @return {float}
*/
p.setMaxMotorTorque = function ( torque ) {
this.m_bodyA.setAwake( true );
this.m_bodyB.setAwake( true );
this.m_maxMotorTorque = torque;
};
/**
* Get the current motor torque given the inverse time step,
* usually in N-m.
*
* @public
* @method getMotorTorque
* @param {float} invDeltaTime
* @return {float}
*/
p.getMotorTorque = function ( invDeltaTime ) {
return invDeltaTime * this.m_motorImpulse;
};
/**
* Get the motor speed, usually in radians per second.
*
* @public
* @method getMotorSpeed
* @return {float}
*/
p.getMotorSpeed = function () {
return this.m_motorSpeed;
};
/**
* Get the maximum friction torque in N*m.
*
* @public
* @method getMaxMotorTorque
* @return {float}
*/
p.getMaxMotorTorque = function () {
return this.m_maxMotorTorque;
};
/**
* @public
* @method setSpringFrequencyHz
* @param {float} hz
* @return {float}
*/
p.setSpringFrequencyHz = function ( hz ) {
this.m_frequencyHz = hz;
};
/**
* @public
* @method getSpringFrequencyHz
* @return {float}
*/
p.getSpringFrequencyHz = function () {
return this.m_frequencyHz;
};
/**
* @public
* @method setSpringDampingRatio
* @param {float} ratio
* @return {float}
*/
p.setSpringDampingRatio = function ( ratio ) {
this.m_dampingRatio = ratio;
};
/**
* @public
* @method getSpringFrequencyHz
* @return {float}
*/
p.getSpringDampingRatio = function () {
return this.m_dampingRatio;
};
/**
* @public
* @override
* @method initVelocityConstraints
* @param {b2SolverData} data
* @return {void}
*/
// Implement b2Joint.initVelocityConstraints
p.initVelocityConstraints = function ( 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;
/*float32*/ var mA = this.m_invMassA, mB = this.m_invMassB;
/*float32*/ var iA = this.m_invIA, iB = this.m_invIB;
/*b2Vec2&*/ var cA = data.positions[this.m_indexA].c;
/*float32*/ var aA = data.positions[this.m_indexA].a;
/*b2Vec2&*/ var vA = data.velocities[this.m_indexA].v;
/*float32*/ var wA = data.velocities[this.m_indexA].w;
/*b2Vec2&*/ var cB = data.positions[this.m_indexB].c;
/*float32*/ var aB = data.positions[this.m_indexB].a;
/*b2Vec2&*/ var vB = data.velocities[this.m_indexB].v;
/*float32*/ var wB = data.velocities[this.m_indexB].w;
/*b2Rot*/ var qA = this.m_qA.setAngle(aA), qB = this.m_qB.setAngle(aB);
// Compute the effective masses.
// b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2.subtract(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
var rA = b2Rot.timesV2(qA, this.m_lalcA, this.m_rA);
// b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2.subtract(this.m_localAnchorB, this.m_localCenterB, this.m_lalcB);
var rB = b2Rot.timesV2(qB, this.m_lalcB, this.m_rB);
// b2Vec2 d = cB + rB - cA - rA;
var d = b2Vec2.subtract(
b2Vec2.add(cB, rB, b2Vec2.POOL0),
b2Vec2.add(cA, rA, b2Vec2.POOL1),
b2WheelJoint._B2VEC2_POOL0);
// Point to line constraint
{
// m_ay = b2Mul(qA, m_localYAxisA);
b2Rot.timesV2(qA, this.m_localYAxisA, this.m_ay);
// m_sAy = b2Cross(d + rA, m_ay);
this.m_sAy = b2Vec2.cross(b2Vec2.add(d, rA, b2Vec2.POOL0), this.m_ay);
// m_sBy = b2Cross(rB, m_ay);
this.m_sBy = b2Vec2.cross(rB, this.m_ay);
this.m_mass = mA + mB + iA * this.m_sAy * this.m_sAy + iB * this.m_sBy * this.m_sBy;
if ( this.m_mass > 0 ) {
this.m_mass = 1 / this.m_mass;
}
}
// Spring constraint
this.m_springMass = 0;
this.m_bias = 0;
this.m_gamma = 0;
if ( this.m_frequencyHz > 0 ) {
// m_ax = b2Mul(qA, m_localXAxisA);
b2Rot.timesV2(qA, this.m_localXAxisA, this.m_ax);
// m_sAx = b2Cross(d + rA, m_ax);
this.m_sAx = b2Vec2.cross(b2Vec2.add(d, rA, b2Vec2.POOL0), this.m_ax);
// m_sBx = b2Cross(rB, m_ax);
this.m_sBx = b2Vec2.cross(rB, this.m_ax);
/*float32*/ var invMass = mA + mB + iA * this.m_sAx * this.m_sAx + iB * this.m_sBx * this.m_sBx;
if ( invMass > 0 ) {
this.m_springMass = 1 / invMass;
/*float32*/ var C = b2Vec2.dot(d, this.m_ax);
// Frequency
/*float32*/ var omega = 2 * Math.PI * this.m_frequencyHz;
// Damping coefficient
/*float32*/ var dc = 2 * this.m_springMass * this.m_dampingRatio * omega;
// Spring stiffness
/*float32*/ var k = this.m_springMass * omega * omega;
// magic formulas
/*float32*/ var h = data.step.dt;
this.m_gamma = h * (dc + h * k);
if ( this.m_gamma > 0 ) {
this.m_gamma = 1 / this.m_gamma;
}
this.m_bias = C * h * k * this.m_gamma;
this.m_springMass = invMass + this.m_gamma;
if ( this.m_springMass > 0 ) {
this.m_springMass = 1 / this.m_springMass;
}
}
}
else {
this.m_springImpulse = 0;
}
// Rotational motor
if ( this.m_enableMotor ) {
this.m_motorMass = iA + iB;
if ( this.m_motorMass > 0 ) {
this.m_motorMass = 1 / this.m_motorMass;
}
}
else {
this.m_motorMass = 0;
this.m_motorImpulse = 0;
}
if ( data.step.warmStarting ) {
// Account for variable time step.
this.m_impulse *= data.step.dtRatio;
this.m_springImpulse *= data.step.dtRatio;
this.m_motorImpulse *= data.step.dtRatio;
// b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax;
var P = b2Vec2.add(
b2Vec2.numTimes(this.m_impulse, this.m_ay, b2Vec2.POOL0),
b2Vec2.numTimes(this.m_springImpulse, this.m_ax, b2Vec2.POOL1),
b2WheelJoint._B2VEC2_POOL1 );
// float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
/*float32*/ var LA = this.m_impulse * this.m_sAy + this.m_springImpulse * this.m_sAx + this.m_motorImpulse;
// float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;
/*float32*/ var LB = this.m_impulse * this.m_sBy + this.m_springImpulse * this.m_sBx + this.m_motorImpulse;
// vA -= m_invMassA * P;
vA.minusEqualsMul(this.m_invMassA, P);
wA -= this.m_invIA * LA;
// vB += m_invMassB * P;
vB.plusEqualsMul( this.m_invMassB, P );
wB += this.m_invIB * LB;
}
else {
this.m_impulse = 0;
this.m_springImpulse = 0;
this.m_motorImpulse = 0;
}
// data.velocities[this.m_indexA].v = vA;
data.velocities[this.m_indexA].w = wA;
// data.velocities[this.m_indexB].v = vB;
data.velocities[this.m_indexB].w = wB;
};
/**
*
* @public
* @override
* @method solveVelocityConstraints
* @param {b2SolverData} data
* @return {void}
*/
// Implement b2Joint.solveVelocityConstraints
p.solveVelocityConstraints = function ( data ) {
/*float32*/ var mA = this.m_invMassA, mB = this.m_invMassB;
/*float32*/ var iA = this.m_invIA, iB = this.m_invIB;
/*b2Vec2&*/ var vA = data.velocities[this.m_indexA].v;
/*float32*/ var wA = data.velocities[this.m_indexA].w;
/*b2Vec2&*/ var vB = data.velocities[this.m_indexB].v;
/*float32*/ var wB = data.velocities[this.m_indexB].w;
// Solve spring constraint
{
/**float*/ var Cdot = b2Vec2.dot(this.m_ax, b2Vec2.subtract(vB, vA, b2Vec2.POOL0)) + this.m_sBx * wB - this.m_sAx * wA;
/**float*/ var impulse = -this.m_springMass * (Cdot + this.m_bias + this.m_gamma * this.m_springImpulse);
this.m_springImpulse += impulse;
// b2Vec2 P = impulse * m_ax;
/**b2Vec2*/var P = b2Vec2.numTimes(impulse, this.m_ax, b2WheelJoint._B2VEC2_POOL0);
/**float*/var LA = impulse * this.m_sAx;
/**float*/var LB = impulse * this.m_sBx;
// vA -= mA * P;
vA.minusEqualsMul(mA, P);
wA -= iA * LA;
// vB += mB * P;
vB.plusEqualsMul(mB, P);
wB += iB * LB;
}
// Solve rotational motor constraint
{
Cdot = wB - wA - this.m_motorSpeed;
impulse = -this.m_motorMass * Cdot;
/**float*/ var oldImpulse = this.m_motorImpulse;
/**float*/ var maxImpulse = data.step.dt * this.m_maxMotorTorque;
this.m_motorImpulse = b2Math.clamp(this.m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = this.m_motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve point to line constraint
{
Cdot = b2Vec2.dot(this.m_ay, b2Vec2.subtract(vB, vA, b2Vec2.POOL0)) + this.m_sBy * wB - this.m_sAy * wA;
impulse = -this.m_mass * Cdot;
this.m_impulse += impulse;
// b2Vec2 P = impulse * m_ay;
P = b2Vec2.numTimes(impulse, this.m_ay, b2WheelJoint._B2VEC2_POOL0);
LA = impulse * this.m_sAy;
LB = impulse * this.m_sBy;
// vA -= mA * P;
vA.minusEqualsMul(mA, P);
wA -= iA * LA;
// vB += mB * P;
vB.plusEqualsMul(mB, P);
wB += iB * LB;
}
// data.velocities[this.m_indexA].v = vA;
data.velocities[this.m_indexA].w = wA;
// data.velocities[this.m_indexB].v = vB;
data.velocities[this.m_indexB].w = wB;
};
/**
* @public
* @override
* @method solvePositionConstraints
* @param {b2SolverData} data
* @return {void}
*/
// Implement b2Joint.solvePositionConstraints
p.solvePositionConstraints = function ( data ) {
/*b2Vec2&*/ var cA = data.positions[this.m_indexA].c;
/*float32*/ var aA = data.positions[this.m_indexA].a;
/*b2Vec2&*/ var cB = data.positions[this.m_indexB].c;
/*float32*/ var aB = data.positions[this.m_indexB].a;
/*b2Rot*/ var qA = this.m_qA.setAngle(aA), qB = this.m_qB.setAngle(aB);
// b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2.subtract(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
var rA = b2Rot.timesV2(qA, this.m_lalcA, this.m_rA);
// b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2.subtract(this.m_localAnchorB, this.m_localCenterB, this.m_lalcB);
var rB = b2Rot.timesV2(qB, this.m_lalcB, this.m_rB);
// b2Vec2 d = (cB - cA) + rB - rA;
var d = b2Vec2.add(
b2Vec2.subtract(cB, cA, b2Vec2.POOL0),
b2Vec2.subtract(rB, rA, b2Vec2.POOL1),
b2WheelJoint._B2VEC2_POOL0);
// b2Vec2 ay = b2Mul(qA, m_localYAxisA);
var ay = b2Rot.timesV2(qA, this.m_localYAxisA, this.m_ay);
// float32 sAy = b2Cross(d + rA, ay);
var sAy = b2Vec2.cross(b2Vec2.add(d, rA, b2Vec2.POOL0), ay);
// float32 sBy = b2Cross(rB, ay);
var sBy = b2Vec2.cross(rB, ay);
// float32 C = b2Dot(d, ay);
var C = b2Vec2.dot(d, this.m_ay);
/*float32*/ var k = this.m_invMassA + this.m_invMassB + this.m_invIA * this.m_sAy * this.m_sAy + this.m_invIB * this.m_sBy * this.m_sBy;
/*float32*/ var impulse;
if ( k !== 0 ) {
impulse = -C / k;
}
else {
impulse = 0;
}
// b2Vec2 P = impulse * ay;
var P = b2Vec2.numTimes(impulse, ay, b2WheelJoint._B2VEC2_POOL1 );
/*float32*/ var LA = impulse * sAy;
/*float32*/ var LB = impulse * sBy;
// cA -= m_invMassA * P;
cA.minusEqualsMul(this.m_invMassA, P);
aA -= this.m_invIA * LA;
// cB += m_invMassB * P;
cB.plusEqualsMul(this.m_invMassB, P);
aB += this.m_invIB * LB;
// data.positions[this.m_indexA].c = cA;
data.positions[this.m_indexA].a = aA;
// data.positions[this.m_indexB].c = cB;
data.positions[this.m_indexB].a = aB;
return Math.abs(C) <= b2Settings.b2_linearSlop;
};
