//################################################################################################//
//################################################################################################//
// //
// ██ ██████ ██████ ██ ██ ██ //
// ██ ██ ██ ██ ██ //
// █████ ██ ██ █████ █████ ████ ██ █████ ██ █████ █████ //
// ██ ██ ██████ ██ ███ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ █████ █████ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// █████ ██████ ██████ █████ █████ ██ █████ █████ ██ ██ ██ ████ //
// //
//################################################################################################//
//################################################################################################//
// CLASS CONSTRUCTOR
/**
* 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:
* coordinateA + ratio * coordinateB = 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 jointA
* or jointB is destroyed.
*
* @class b2GearJoint
* @constructor
* @extends {b2Joint}
* @param {b2GearJointDef} gearJointDef
* @module Joints
*/
//TODO: test b2GearJoint. Ported but not yet tested.
function b2GearJoint( gearJointDef ) {
/**
* Invokes parent class constructor function reference.
*/
this.constructor( gearJointDef );
////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// ██████ ██ ██ //
// ██ ██ ██ //
// ██ ██ ████ █████ █████ █████ ████ █████ ██ █████ █████ //
// ██████ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ █████ ██ ██ ██ █████ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ █████ █████ █████ ██ ████ ██ █████ █████ //
// ██ //
// ██ //
// //
////////////////////////////////////////////////////////////////////////////////////////////////////
// property DECLARATIONS
// Solver shared
/**
* @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_localAnchorC
* @type {b2Vec2}
*/
this.m_localAnchorC = new b2Vec2;
/**
* @public
* @property m_localAnchorD
* @type {b2Vec2}
*/
this.m_localAnchorD = new b2Vec2;
/**
* @public
* @property m_localAxisC
* @type {b2Vec2}
*/
this.m_localAxisC = new b2Vec2;
/**
* @public
* @property m_localAxisD
* @type {b2Vec2}
*/
this.m_localAxisD = new b2Vec2;
/**
* @public
* @property m_lcA
* @type {b2Vec2}
*/
this.m_lcA = new b2Vec2;
/**
* @public
* @property m_lcB
* @type {b2Vec2}
*/
this.m_lcB = new b2Vec2;
/**
* @public
* @property m_lcC
* @type {b2Vec2}
*/
this.m_lcC = new b2Vec2;
/**
* @public
* @property m_lcD
* @type {b2Vec2}
*/
this.m_lcD = new b2Vec2;
/**
* @public
* @property m_JvAC
* @type {b2Vec2}
*/
this.m_JvAC = new b2Vec2;
/**
* @public
* @property m_JvBD
* @type {b2Vec2}
*/
this.m_JvBD = 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_qC
* @type {b2Rot}
*/
this.m_qC = new b2Rot;
/**
* @public
* @property m_qD
* @type {b2Rot}
*/
this.m_qD = 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_lalcC
* @type {b2Vec2}
*/
this.m_lalcC = new b2Vec2;
/**
* @public
* @property m_lalcD
* @type {b2Vec2}
*/
this.m_lalcD = new b2Vec2;
// property INITIALISATIONS
/**
* @public
* @property m_referenceAngleA
* @type {float}
* @default 0.0
*/
this.m_referenceAngleA = 0.0;
/**
* @public
* @property m_referenceAngleB
* @type {float}
* @default 0.0
*/
this.m_referenceAngleB = 0.0;
/**
* @public
* @property m_referenceAngleB
* @type {float}
* @default 0.0
*/
this.m_constant = 0.0;
/**
* @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_indexC
* @type {int}
* @default 0
*/
this.m_indexC = 0;
/**
* @public
* @property m_indexD
* @type {int}
* @default 0
*/
this.m_indexD = 0;
/**
* @public
* @property m_mA
* @type {int}
* @default 0.0
*/
this.m_mA = 0.0;
/**
* @public
* @property m_mB
* @type {int}
* @default 0.0
*/
this.m_mB = 0.0;
/**
* @public
* @property m_mC
* @type {int}
* @default 0.0
*/
this.m_mC = 0.0;
/**
* @public
* @property m_mD
* @type {int}
* @default 0.0
*/
this.m_mD = 0.0;
/**
* @public
* @property m_iA
* @type {int}
* @default 0.0
*/
this.m_iA = 0.0;
/**
* @public
* @property m_iB
* @type {int}
* @default 0.0
*/
this.m_iB = 0.0;
/**
* @public
* @property m_iB
* @type {int}
* @default 0.0
*/
this.m_iB = 0.0;
/**
* @public
* @property m_iC
* @type {int}
* @default 0.0
*/
this.m_iC = 0.0;
/**
* @public
* @property m_iD
* @type {int}
* @default 0.0
*/
this.m_iD = 0.0;
/**
* @public
* @property m_JvAC
* @type {float}
* @default 0.0
*/
this.m_JvAC = 0.0;
/**
* @public
* @property m_JvBD
* @type {float}
* @default 0.0
*/
this.m_JvBD = 0.0;
/**
* @public
* @property m_JwA
* @type {float}
* @default 0.0
*/
this.m_JwA = 0.0;
/**
* @public
* @property m_JwB
* @type {float}
* @default 0.0
*/
this.m_JwB = 0.0;
/**
* @public
* @property m_JwC
* @type {float}
* @default 0.0
*/
this.m_JwC = 0.0;
/**
* @public
* @property m_JwD
* @type {float}
* @default 0.0
*/
this.m_JwD = 0.0;
/**
* @public
* @property m_mass
* @type {float}
* @default 0.0
*/
this.m_mass = 0.0;
/**
* @public
* @property m_mass
* @type {b2Rot}
* @default 0.0
*/
this.m_qA = 0.0;
/**
* @public
* @property m_qB
* @type {b2Rot}
* @default 0.0
*/
this.m_qB = 0.0;
/**
* @public
* @property m_qC
* @type {b2Rot}
* @default 0.0
*/
this.m_qC = 0.0;
/**
* @public
* @property m_qD
* @type {b2Rot}
* @default 0.0
*/
this.m_qD = 0.0;
/**
* @public
* @property m_joint1
* @type {b2Joint}
*/
this.m_joint1 = gearJointDef.joint1;
/**
* @public
* @property m_joint1
* @type {b2Joint}
*/
this.m_joint2 = gearJointDef.joint2;
/**
*
* @public
* @property m_bodyA
* @type {b2Body}
*/
this.m_bodyA = gearJointDef.joint1.getBodyB();
/**
*
* @public
* @property m_ratio
* @type {float}
*/
this.m_ratio = gearJointDef.ratio;
/**
*
* @public
* @property m_impulse
* @type {number}
* @default 0.0
*/
this.m_impulse = 0.0;
// constructor LOGIC
/**
* @public
* @property m_typeA
* @type {int}
*/
this.m_typeA = this.m_joint1.joint1.m_type;
/**
* @public
* @property m_typeA
* @type {int}
*/
this.m_typeB = this.m_joint2.joint1.m_type;
/**
* Body A is connected to body C.
*
* @public
* @property m_bodyC
* @type {b2Body}
* @default
*/
this.m_bodyC = this.m_joint1.m_bodyB;
/**
* Body A is connected to body C.
*
* @public
* @property m_bodyA
* @type {b2Body}
* @default
*/
this.m_bodyA = this.m_joint1.m_bodyB;
/**
* Body B is connected to body D.
*
* @public
* @property m_bodyD
* @type {b2Body}
* @default
*/
this.m_bodyD = this.m_joint2.m_bodyA;
/**
* Body B is connected to body D.
*
* @public
* @property m_bodyB
* @type {b2Body}
* @default
*/
this.m_bodyB = this.m_joint2.m_bodyB;
/*##if EXPORT_ASSERTS */
if ( b2Settings.ASSERTS_ENABLED ) {
b2Assert( this.m_typeA === b2Joint.e_revoluteJoint || this.m_typeA === b2Joint.e_prismaticJoint );
b2Assert( this.m_typeB === b2Joint.e_revoluteJoint || this.m_typeB === b2Joint.e_prismaticJoint );
}/*#*/
/*float32*/var coordinateA;
/*float32*/var coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
// Get geometry of joint1
/**b2Transform*/var xfA = this.m_bodyA.m_xf;
/**float32*/var aA = this.m_bodyA.m_sweep.a;
/**b2Transform*/var xfC = this.m_bodyC.m_xf;
/**float32*/var aC = this.m_bodyC.m_sweep.a;
if ( this.m_typeA === b2Joint.e_revoluteJoint ) {
/**b2RevoluteJoint*/var revolute = gearJointDef.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;
}
else {
/*b2PrismaticJoint*/var prismatic = gearJointDef.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 );
// b2Vec2 pC = m_localAnchorC;
var pC = this.m_localAnchorC;
// b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
var pA = b2Rot.invRotV2(
xfC.q,
b2Vec2.add(
b2Rot.timesV2( xfA.q, this.m_localAnchorA, b2Vec2.POOL0 ),
b2Vec2.subtract( xfA.p, xfC.p, b2Vec2.POOL1 ),
b2Vec2.POOL0 ),
b2Vec2.POOL0 ); // pA uses s_t0
// coordinateA = b2Dot(pA - pC, m_localAxisC);
coordinateA = b2Vec2.dot( b2Vec2.subtract( pA, pC, b2Vec2.POOL0 ), this.m_localAxisC );
}
// Get geometry of joint2
/*b2Transform*/var xfB = this.m_bodyB.m_xf;
/*float32*/var aB = this.m_bodyB.m_sweep.a;
/*b2Transform*/var xfD = this.m_bodyD.m_xf;
/*float32*/var aD = this.m_bodyD.m_sweep.a;
if ( this.m_typeB === b2Joint.e_revoluteJoint ) {
/*b2RevoluteJoint*/var revolute = gearJointDef.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 {
/*b2PrismaticJoint*/var prismatic = gearJointDef.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 );
// b2Vec2 pD = m_localAnchorD;
var pD = this.m_localAnchorD;
// b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
var pB = b2Rot.invRotV2(
xfD.q,
b2Vec2.add(
b2Rot.timesV2( xfB.q, this.m_localAnchorB, b2Vec2.POOL0 ),
b2Vec2.subtract( xfB.p, xfD.p, b2Vec2.POOL1 ),
b2Vec2.POOL0 ),
b2Vec2.POOL0 );
// coordinateB = b2Dot(pB - pD, m_localAxisD);
coordinateB = b2Vec2.dot( b2Vec2.subtract( pB, pD, b2Vec2.POOL0 ), this.m_localAxisD );
}
this.m_ratio = gearJointDef.ratio;
this.m_constant = coordinateA + this.m_ratio * coordinateB;
this.m_impulse = 0;
////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// ██ ██ ██ ██ //
// ██ ██ ██ //
// ██ █████ █████ █████ ████ ██ █████ █████ █████ █████ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ █████ ██ ██ ██ █████ ██ ██ ██ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ █████ ██ ██ ████ █████ ██ ██ █████ █████ //
// //
////////////////////////////////////////////////////////////////////////////////////////////////////
} b2GearJoint.prototype = p = new b2Joint; Box2D.b2GearJoint = b2GearJoint;
// STATIC CLASS PROPERTIES
/**
* Object pool for memory management.
*/
b2GearJoint._B2VEC2_POOL0 = new b2Vec2;
b2GearJoint._B2VEC2_POOL1 = new b2Vec2;
b2GearJoint._B2VEC2_POOL2 = new b2Vec2;
b2GearJoint._B2VEC2_POOL3 = new b2Vec2;
b2GearJoint._B2VEC2_POOL4 = new b2Vec2;
////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// ██ ██ ██ ██ ██ //
// ███ ███ ██ ██ ██ //
// ███████ █████ █████ █████ █████ █████ █████ //
// ██ █ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ █████ ██ ██ ██ ██ ██ ██ ██ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ █████ ████ ██ ██ █████ █████ █████ //
// //
////////////////////////////////////////////////////////////////////////////////////////////////////
// INSTANCE METHODS
/**
* @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_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;
/**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;
/**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;
/**float32*/var aC = data.positions[this.m_indexC].a;
/**b2Vec2&*/var vC = data.velocities[this.m_indexC].v;
/**float32*/var wC = data.velocities[this.m_indexC].w;
/**float32*/var aD = data.positions[this.m_indexD].a;
/**b2Vec2&*/var vD = data.velocities[this.m_indexD].v;
/**float32*/var wD = data.velocities[this.m_indexD].w;
// b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
var qA = this.m_qA.setAngle( aA ),
qB = this.m_qB.setAngle( aB ),
qC = this.m_qC.setAngle( aC ),
qD = this.m_qD.setAngle( aD );
this.m_mass = 0;
if ( this.m_typeA === b2Joint.e_revoluteJoint ) {
this.m_JvAC.setZero();
this.m_JwA = 1;
this.m_JwC = 1;
this.m_mass += this.m_iA + this.m_iC;
}
else {
// b2Vec2 u = b2Mul(qC, m_localAxisC);
var u = b2Rot.timesV2( qC, this.m_localAxisC, b2GearJoint._B2VEC2_POOL1 );
// b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
b2Vec2.subtract( this.m_localAnchorC, this.m_lcC, this.m_lalcC );
var rC = b2Rot.timesV2( qC, this.m_lalcC, b2GearJoint._B2VEC2_POOL2 );
// b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
b2Vec2.subtract( this.m_localAnchorA, this.m_lcA, this.m_lalcA );
var rA = b2Rot.timesV2( qA, this.m_lalcA, b2GearJoint._B2VEC2_POOL2 );
// m_JvAC = u;
this.m_JvAC.copy( u );
// m_JwC = b2Cross(rC, u);
this.m_JwC = b2Vec2.cross( rC, u );
// m_JwA = b2Cross(rA, u);
this.m_JwA = 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 === b2Joint.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 {
// b2Vec2 u = b2Mul(qD, m_localAxisD);
var u = b2Rot.timesV2( qD, this.m_localAxisD, b2GearJoint._B2VEC2_POOL3 );
// b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
b2Vec2.subtract( this.m_localAnchorD, this.m_lcD, this.m_lalcD );
var rD = b2Rot.timesV2( qD, this.m_lalcD, b2GearJoint._B2VEC2_POOL4 );
// b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
b2Vec2.subtract( this.m_localAnchorB, this.m_lcB, this.m_lalcB );
var rB = b2Rot.timesV2( qB, this.m_lalcB, b2GearJoint._B2VEC2_POOL5 );
// m_JvBD = m_ratio * u;
b2Vec2.numTimes( this.m_ratio, u, this.m_JvBD );
// m_JwD = m_ratio * b2Cross(rD, u);
this.m_JwD = this.m_ratio * b2Vec2.cross( rD, u );
// m_JwB = m_ratio * b2Cross(rB, u);
this.m_JwB = this.m_ratio * 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 += (m_mA * m_impulse) * m_JvAC;
vA.plusEqualsMul( this.m_mA * this.m_impulse, this.m_JvAC );
wA += this.m_iA * this.m_impulse * this.m_JwA;
// vB += (m_mB * m_impulse) * m_JvBD;
vB.plusEqualsMul( this.m_mB * this.m_impulse, this.m_JvBD );
wB += this.m_iB * this.m_impulse * this.m_JwB;
// vC -= (m_mC * m_impulse) * m_JvAC;
vC.minusEqualsMul( this.m_mC * this.m_impulse, this.m_JvAC );
wC -= this.m_iC * this.m_impulse * this.m_JwC;
// vD -= (m_mD * m_impulse) * m_JvBD;
vD.minusEqualsMul( 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].v = vA;
data.velocities[this.m_indexA].w = wA;
// data.velocities[this.m_indexB].v = vB;
data.velocities[this.m_indexB].w = wB;
// data.velocities[this.m_indexC].v = vC;
data.velocities[this.m_indexC].w = wC;
// data.velocities[this.m_indexD].v = vD;
data.velocities[this.m_indexD].w = wD;
};
/**
*
* @public
* @override
* @method solveVelocityConstraints
* @param {b2SolverData} data
* @return {void}
*/
// Implement b2Joint.solveVelocityConstraints
p.solveVelocityConstraints = function ( data ) {
/*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;
/*b2Vec2&*/var vC = data.velocities[this.m_indexC].v;
/*float32*/var wC = data.velocities[this.m_indexC].w;
/*b2Vec2&*/var vD = data.velocities[this.m_indexD].v;
/*float32*/var wD = data.velocities[this.m_indexD].w;
// float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD);
var Cdot =
b2Vec2.dot( this.m_JvAC, b2Vec2.subtract( vA, vC, b2Vec2.POOL0 ) ) +
b2Vec2.dot( this.m_JvBD, b2Vec2.subtract( vB, vD, b2Vec2.POOL0 ) );
Cdot += (this.m_JwA * wA - this.m_JwC * wC) + (this.m_JwB * wB - this.m_JwD * wD);
/*float32*/var impulse = -this.m_mass * Cdot;
this.m_impulse += impulse;
// vA += (m_mA * impulse) * m_JvAC;
vA.plusEqualsMul( (this.m_mA * impulse), this.m_JvAC );
wA += this.m_iA * impulse * this.m_JwA;
// vB += (m_mB * impulse) * m_JvBD;
vB.plusEqualsMul( (this.m_mB * impulse), this.m_JvBD );
wB += this.m_iB * impulse * this.m_JwB;
// vC -= (m_mC * impulse) * m_JvAC;
vC.minusEqualsMul( (this.m_mC * impulse), this.m_JvAC );
wC -= this.m_iC * impulse * this.m_JwC;
// vD -= (m_mD * impulse) * m_JvBD;
vD.minusEqualsMul( (this.m_mD * impulse), this.m_JvBD );
wD -= this.m_iD * impulse * this.m_JwD;
// 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;
// data.velocities[this.m_indexC].v = vC;
data.velocities[this.m_indexC].w = wC;
// data.velocities[this.m_indexD].v = vD;
data.velocities[this.m_indexD].w = wD;
};
/**
* @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;
/*b2Vec2&*/var cC = data.positions[this.m_indexC].c;
/*float32*/var aC = data.positions[this.m_indexC].a;
/*b2Vec2&*/var cD = data.positions[this.m_indexD].c;
/*float32*/var aD = data.positions[this.m_indexD].a;
// b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
var qA = this.m_qA.setAngle( aA ),
qB = this.m_qB.setAngle( aB ),
qC = this.m_qC.setAngle( aC ),
qD = this.m_qD.setAngle( aD );
/*float32*/var linearError = 0;
/*float32*/var coordinateA, coordinateB;
/*b2Vec2*/var JvAC = this.m_JvAC, JvBD = this.m_JvBD;
/*float32*/var JwA, JwB, JwC, JwD;
/*float32*/var mass = 0;
if ( this.m_typeA === b2Joint.e_revoluteJoint ) {
JvAC.setZero();
JwA = 1;
JwC = 1;
mass += this.m_iA + this.m_iC;
coordinateA = aA - aC - this.m_referenceAngleA;
}
else {
// b2Vec2 u = b2Mul(qC, m_localAxisC);
var u = b2Rot.timesV2( qC, this.m_localAxisC, b2GearJoint._B2VEC2_POOL0 );
// b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
var rC = b2Rot.timesV2( qC, this.m_lalcC, b2GearJoint._B2VEC2_POOL1 );
// b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
var rA = b2Rot.timesV2( qA, this.m_lalcA, b2GearJoint._B2VEC2_POOL2 );
// JvAC = u;
JvAC.copy( u );
// JwC = b2Cross(rC, u);
JwC = b2Vec2.cross( rC, u );
// JwA = b2Cross(rA, u);
JwA = b2Vec2.cross( rA, u );
mass += this.m_mC + this.m_mA + this.m_iC * JwC * JwC + this.m_iA * JwA * JwA;
// b2Vec2 pC = m_localAnchorC - m_lcC;
var pC = this.m_lalcC;
// b2Vec2 pA = b2MulT(qC, rA + (cA - cC));
var pA = b2Rot.invRotV2(
qC,
b2Vec2.add(
rA,
b2Vec2.subtract( cA, cC, b2Vec2.POOL0 ),
b2Vec2.POOL0 ),
b2Vec2.POOL0 ); // pA uses s_t0
// coordinateA = b2Dot(pA - pC, m_localAxisC);
coordinateA = b2Vec2.dot( b2Vec2.subtract( pA, pC, b2Vec2.POOL0 ), this.m_localAxisC );
}
if ( this.m_typeB === b2Joint.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 {
// b2Vec2 u = b2Mul(qD, m_localAxisD);
var u = b2Rot.timesV2( qD, this.m_localAxisD, b2GearJoint._B2VEC2_POOL3 );
// b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
var rD = b2Rot.timesV2( qD, this.m_lalcD, b2GearJoint._B2VEC2_POOL4 );
// b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
var rB = b2Rot.timesV2( qB, this.m_lalcB, b2GearJoint._B2VEC2_POOL5 );
// JvBD = m_ratio * u;
b2Vec2.numTimes( this.m_ratio, u, JvBD );
// JwD = m_ratio * b2Cross(rD, u);
JwD = this.m_ratio * b2Vec2.cross( rD, u );
// JwB = m_ratio * b2Cross(rB, u);
JwB = this.m_ratio * 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;
// b2Vec2 pD = m_localAnchorD - m_lcD;
var pD = this.m_lalcD;
// b2Vec2 pB = b2MulT(qD, rB + (cB - cD));
var pB = b2Rot.invRotV2(
qD,
b2Vec2.add(
rB,
b2Vec2.subtract( cB, cD, b2Vec2.POOL0 ),
b2Vec2.POOL0 ),
b2Vec2.POOL0 ); // pB uses s_t0
// coordinateB = b2Dot(pB - pD, m_localAxisD);
coordinateB = b2Vec2.dot( b2Vec2.subtract( pB, pD, b2Vec2.POOL0 ), this.m_localAxisD );
}
/*float32*/var C = (coordinateA + this.m_ratio * coordinateB) - this.m_constant;
/*float32*/var impulse = 0;
if ( mass > 0 ) {
impulse = -C / mass;
}
// cA += m_mA * impulse * JvAC;
cA.plusEqualsMul( this.m_mA * impulse, JvAC );
aA += this.m_iA * impulse * JwA;
// cB += m_mB * impulse * JvBD;
cB.plusEqualsMul( this.m_mB * impulse, JvBD );
aB += this.m_iB * impulse * JwB;
// cC -= m_mC * impulse * JvAC;
cC.minusEqualsMul( this.m_mC * impulse, JvAC );
aC -= this.m_iC * impulse * JwC;
// cD -= m_mD * impulse * JvBD;
cD.minusEqualsMul( this.m_mD * impulse, JvBD );
aD -= this.m_iD * impulse * JwD;
// 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;
// data.positions[this.m_indexC].c = cC;
data.positions[this.m_indexC].a = aC;
// data.positions[this.m_indexD].c = cD;
data.positions[this.m_indexD].a = aD;
// TODO_ERIN not implemented
return linearError < b2Settings.b2_linearSlop;
};
/**
* @public
* @method getAnchorA
* @param {b2Vec2|Object=} [out=b2Vec2] reusable object
* @return {b2Vec2|Object} out
*/
p.getAnchorA = function ( out ) {
out = out || b2GearJoint._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 || b2GearJoint._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 || b2GearJoint._B2VEC2_POOL0;
return b2Vec2.numTimes( invDeltaTime * this.m_impulse, this.m_JvAC, 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_impulse * this.m_JwA;
};
/**
* Get the first joint.
*
* @public
* @method getJoint1
* @return {b2Joint}
*/
p.getJoint1 = function () {
return this.m_joint1;
};
/**
* Get the second joint.
*
* @public
* @method getJoint1
* @return {b2Joint}
*/
p.getJoint2 = function () {
return this.m_joint2;
};
/**
* @public
* @method getRatio
* @return {float}
*/
p.getRatio = function () {
return this.m_ratio;
};
/**
* @public
* @method setRatio
* @param {float} ratio
* @return {float}
*/
p.setRatio = function ( ratio ) {
/*##if EXPORT_ASSERTS */
if ( b2Settings.ASSERTS_ENABLED ) { b2Assert( isFinite( ratio ) ); }
/*#end EXPORT_ASSERTS */
this.m_ratio = ratio;
};
