//################################################################################################//
//################################################################################################//
// //
// ██ ██████ ██████ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ ██ ██ //
// █████ ██ ██ ██ █████ ██ ██ █████ ██ ██ ██ █████ █████ ██ █████ ██ █████ █████ //
// ██ ██ ██████ ██████ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ █████ ██ ██ ██ ██ ██ ██ ██ ██ █████ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ ███ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// █████ ██████ ██ ██ █████ ███ █████ ██ █████ ████ █████ █████ █████ ██ ██ ██ ████ //
// //
//################################################################################################//
//################################################################################################//
// CLASS CONSTRUCTOR
/**
* A revolute joint constrains two bodies to share a common
* point while they are free to rotate about the point. The
* relative rotation about the shared point is the joint angle.
* You can limit the relative rotation with a joint limit that
* specifies a lower and upper angle. You can use a motor to
* drive the relative rotation about the shared point. A maximum
* motor torque is provided so that infinite forces are not
* generated.
*
* @class b2RevoluteJoint
* @constructor
* @param {b2RevoluteJointDef} revoluteJointDef
* @extends {b2Joint}
* @module Joints
*/
function b2RevoluteJoint( revoluteJointDef ) {
/**
*Invoke parent class constructor function reference.
*/
this.constructor( revoluteJointDef );
////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// ██████ ██ ██ //
// ██ ██ ██ //
// ██ ██ ████ █████ █████ █████ ████ █████ ██ █████ █████ //
// ██████ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ █████ ██ ██ ██ █████ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ █████ █████ █████ ██ ████ ██ █████ █████ //
// ██ //
// ██ //
// //
////////////////////////////////////////////////////////////////////////////////////////////////////
// property DECLARATIONS
/**
* @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_K
* @type {b2Mat22}
*/
this.m_K = new b2Mat22;
/**
* @public
* @property m_rA
* @type {b2Vec2}
*/
this.m_rA = new b2Vec2;
/**
* @public
* @property m_rB
* @type {b2Vec2}
*/
this.m_rB = new b2Vec2;
/**
* @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_localCenterA
* @type {b2Vec2}
*/
this.m_localCenterA = new b2Vec2;
/**
* @public
* @property m_localCenterB
* @type {b2Vec2}
*/
this.m_localCenterB = new b2Vec2;
/**
* @public
* @property m_impulse
* @type {b2Vec3}
*/
this.m_impulse = new b2Vec3;
/**
* Effective mass for point-to-point constraint.
*
* @public
* @property m_mass
* @type {b2Mat33}
*/
this.m_mass = new b2Mat33;
// property INITIALISATIONS
this.m_localAnchorA.copy( revoluteJointDef.localAnchorA );
this.m_localAnchorB.copy( revoluteJointDef.localAnchorB );
this.m_impulse.setZero();
/**
* @public
* @property m_referenceAngle
* @type {float}
*/
this.m_referenceAngle = revoluteJointDef.referenceAngle;
/**
* @public
* @property m_motorImpulse
* @type {float}
* @default 0.0
*/
this.m_motorImpulse = 0.0;
/**
* @public
* @property m_lowerAngle
* @type {float}
*/
this.m_lowerAngle = revoluteJointDef.lowerAngle;
/**
* @public
* @property m_upperAngle
* @type {float}
*/
this.m_upperAngle = revoluteJointDef.upperAngle;
/**
* @public
* @property m_maxMotorTorque
* @type {float}
*/
this.m_maxMotorTorque = revoluteJointDef.maxMotorTorque;
/**
* @public
* @property m_motorSpeed
* @type {float}
*/
this.m_motorSpeed = revoluteJointDef.motorSpeed;
/**
* @public
* @property m_enableLimit
* @type {float}
*/
this.m_enableLimit = revoluteJointDef.enableLimit;
/**
* @public
* @property m_enableMotor
* @type {float}
*/
this.m_enableMotor = revoluteJointDef.enableMotor;
/**
* Effective mass for motor/limit angular constraint.
*
* @public
* @property m_enableMotor
* @type {float}
* @default 0.0
*/
this.m_motorMass = 0.0;
/**
* @public
* @property m_limitState
* @type {int}
* default b2Joint.e_inactiveLimit
*/
this.m_limitState = b2Joint.e_inactiveLimit;
/**
* @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_invMassA
* @type {float}
* @default 0.0
*/
this.m_invIA = 0.0;
/**
* @public
* @property m_invMassB
* @type {float}
* @default 0.0
*/
this.m_invIB = 0.0;
////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// ██ ██ ██ ██ //
// ██ ██ ██ //
// ██ █████ █████ █████ ████ ██ █████ █████ █████ █████ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ █████ ██ ██ ██ █████ ██ ██ ██ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ █████ ██ ██ ████ █████ ██ ██ █████ █████ //
// //
////////////////////////////////////////////////////////////////////////////////////////////////////
} b2RevoluteJoint.prototype = p = new b2Joint; Box2D.b2RevoluteJoint = b2RevoluteJoint;
// STATIC CLASS PROPERTIES
/**
* Object pool for memory management.
*/
b2RevoluteJoint._B2VEC2_POOL0 = new b2Vec2;
b2RevoluteJoint._B2VEC2_POOL1 = new b2Vec2;
b2RevoluteJoint._B2VEC2_POOL2 = new b2Vec2;
b2RevoluteJoint._B2VEC2_POOL3 = new b2Vec2;
b2RevoluteJoint._B2VEC2_POOL4 = new b2Vec2;
b2RevoluteJoint._B2VEC2_POOL5 = new b2Vec2;
b2RevoluteJoint._B2VEC3_POOL0 = new b2Vec3;
////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// ██ ██ ██ ██ ██ //
// ███ ███ ██ ██ ██ //
// ███████ █████ █████ █████ █████ █████ █████ //
// ██ █ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ █████ ██ ██ ██ ██ ██ ██ ██ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ █████ ████ ██ ██ █████ █████ █████ //
// //
////////////////////////////////////////////////////////////////////////////////////////////////////
// INSTANCE METHODS
/**
* @public
* @method getAnchorB
* @param {b2Vec2|Object=} [out=b2Vec2] reusable object
* @return {b2Vec2|Object} out
*/
p.getAnchorA = function ( out ) {
out = out || b2RevoluteJoint._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 || b2RevoluteJoint._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 || b2RevoluteJoint._B2VEC2_POOL0;
return out.set( invDeltaTime * this.m_impulse.x, invDeltaTime * this.m_impulse.y );
};
/**
* Get the reaction torque due to the joint limit given the
* inverse time step. Unit is N*m.
*
* @public
* @method getReactionTorque
* @param {float} invDeltaTime
* @return {float}
*/
p.getReactionTorque = function ( invDeltaTime ) {
return invDeltaTime * this.m_impulse.z;
};
/**
* 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 || b2DistanceJoint._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 || b2DistanceJoint._B2VEC2_POOL0;
return out.copy( this.m_localAnchorB );
};
/**
* Get the reference angle.
*
* @public
* @method getReferenceAngle
* @return {float}
*/
p.getReferenceAngle = function () {
return this.m_referenceAngle;
};
/**
* @public
* @method getJointAngle
* @return {float}
*/
p.getJointAngle = function () {
return this.m_bodyB.m_sweep.a - this.m_bodyA.m_sweep.a - this.m_referenceAngle;
};
/**
* @public
* @method getJointSpeed
* @return {float}
*/
p.getJointSpeed = function () {
return this.m_bodyB.m_angularVelocity - this.m_bodyA.m_angularVelocity;
};
/**
* @public
* @method isMotorEnabled
* @return {boolean}
*/
p.isMotorEnabled = function () {
return this.m_enableMotor;
};
/**
* @public
* @method enableMotor
* @param {boolean} flag
* @return {void}
*/
p.enableMotor = function ( flag ) {
if ( this.m_enableMotor !== flag ) {
this.m_bodyA.setAwake( true );
this.m_bodyB.setAwake( true );
this.m_enableMotor = flag;
}
};
/**
* Get the current motor torque given the inverse time step.
* Unit is N*m.
*
* @public
* @method getMotorTorque
* @param {b2Vec2|Object=} invDeltaTime
* @return {float}
*/
p.getMotorTorque = function ( invDeltaTime ) {
return invDeltaTime * this.m_motorImpulse;
};
/**
* @public
* @method getMotorSpeed
* @return {float}
*/
p.getMotorSpeed = function () {
return this.m_motorSpeed;
};
/**
* @public
* @method setMaxMotorTorque
* @param {float} torque
* @return {void}
*/
p.setMaxMotorTorque = function ( torque ) {
this.m_maxMotorTorque = torque;
};
/**
* Set the maximum friction force in N.
*
* @public
* @method getMaxMotorTorque
* @return {float}
*/
p.getMaxMotorTorque = function () {
return this.m_maxMotorTorque;
};
/**
* @public
* @method isLimitEnabled
* @return {boolean}
*/
p.isLimitEnabled = function () {
return this.m_enableLimit;
};
/**
* @public
* @method enableLimit
* @param {boolean} flag
* @return {void}
*/
p.enableLimit = function ( flag ) {
if ( flag !== this.m_enableLimit ) {
this.m_bodyA.setAwake( true );
this.m_bodyB.setAwake( true );
this.m_enableLimit = flag;
this.m_impulse.z = 0.0;
}
};
/**
* @public
* @method getLowerLimit
* @return {float}
*/
p.getLowerLimit = function () {
return this.m_lowerAngle;
};
/**
* @public
* @method getUpperLimit
* @return {float}
*/
p.getUpperLimit = function () {
return this.m_lowerAngle;
};
/**
* @public
* @method setLimits
* @param {number} lower
* @param {number} upper
* @return {void}
*/
p.setLimits = function ( lower, upper ) {
this.m_bodyA.setAwake( true );
this.m_bodyB.setAwake( true );
this.m_impulse.z = 0.0;
this.m_lowerAngle = lower;
this.m_upperAngle = upper;
};
/**
* @public
* @method setMotorSpeed
* @param {float} speed
* @return {void}
*/
p.setMotorSpeed = function ( speed ) {
if ( this.m_motorSpeed !== speed ) {
this.m_bodyA.setAwake( true );
this.m_bodyB.setAwake( true );
this.m_motorSpeed = speed;
}
};
/**
* @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 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;
// b2Rot qA(aA), qB(aB);
/*b2Rot*/var qA = this.m_qA.setAngle( aA ), qB = this.m_qB.setAngle( aB );
// m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2.subtract( this.m_localAnchorA, this.m_localCenterA, this.m_lalcA );
b2Rot.timesV2( qA, this.m_lalcA, this.m_rA );
// m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2.subtract( this.m_localAnchorB, this.m_localCenterB, this.m_lalcB );
b2Rot.timesV2( qB, this.m_lalcB, this.m_rB );
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
/*float32*/var mA = this.m_invMassA, mB = this.m_invMassB;
/*float32*/var iA = this.m_invIA, iB = this.m_invIB;
/*bool*/var fixedRotation = (iA + iB === 0);
this.m_mass.ex.x = mA + mB + this.m_rA.y * this.m_rA.y * iA + this.m_rB.y * this.m_rB.y * iB;
this.m_mass.ey.x = -this.m_rA.y * this.m_rA.x * iA - this.m_rB.y * this.m_rB.x * iB;
this.m_mass.ez.x = -this.m_rA.y * iA - this.m_rB.y * iB;
this.m_mass.ex.y = this.m_mass.ey.x;
this.m_mass.ey.y = mA + mB + this.m_rA.x * this.m_rA.x * iA + this.m_rB.x * this.m_rB.x * iB;
this.m_mass.ez.y = this.m_rA.x * iA + this.m_rB.x * iB;
this.m_mass.ex.z = this.m_mass.ez.x;
this.m_mass.ey.z = this.m_mass.ez.y;
this.m_mass.ez.z = iA + iB;
this.m_motorMass = iA + iB;
if ( this.m_motorMass > 0 ) {
this.m_motorMass = 1 / this.m_motorMass;
}
if ( !this.m_enableMotor || fixedRotation ) {
this.m_motorImpulse = 0.0;
}
if ( this.m_enableLimit && !fixedRotation ) {
/*float32*/var jointAngle = aB - aA - this.m_referenceAngle;
if ( Math.abs( this.m_upperAngle - this.m_lowerAngle ) < 2 * b2Settings.b2_angularSlop ) {
this.m_limitState = b2Joint.e_equalLimits;
}
else if ( jointAngle <= this.m_lowerAngle ) {
if ( this.m_limitState !== b2Joint.e_atLowerLimit ) {
this.m_impulse.z = 0;
}
this.m_limitState = b2Joint.e_atLowerLimit;
}
else if ( jointAngle >= this.m_upperAngle ) {
if ( this.m_limitState !== b2Joint.e_atUpperLimit ) {
this.m_impulse.z = 0;
}
this.m_limitState = b2Joint.e_atUpperLimit;
}
else {
this.m_limitState = b2Joint.e_inactiveLimit;
this.m_impulse.z = 0;
}
}
else {
this.m_limitState = b2Joint.e_inactiveLimit;
}
if ( data.step.warmStarting ) {
// Scale impulses to support a variable time step.
this.m_impulse.timesScalar( data.step.dtRatio );
this.m_motorImpulse *= data.step.dtRatio;
// b2Vec2 P(m_impulse.x, m_impulse.y);
var P = b2RevoluteJoint._B2VEC2_POOL0.set( this.m_impulse.x, this.m_impulse.y );
// vA -= mA * P;
vA.minusEqualsMul( mA, P );
wA -= iA * (b2Vec2.cross( this.m_rA, P ) + this.m_motorImpulse + this.m_impulse.z);
// vB += mB * P;
vB.plusEqualsMul( mB, P );
wB += iB * (b2Vec2.cross( this.m_rB, P ) + this.m_motorImpulse + this.m_impulse.z);
}
else {
this.m_impulse.setZero();
this.m_motorImpulse = 0.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 ) {
/*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;
/*float32*/var mA = this.m_invMassA, mB = this.m_invMassB;
/*float32*/var iA = this.m_invIA, iB = this.m_invIB;
/*bool*/var fixedRotation = (iA + iB === 0);
// Solve motor constraint.
if ( this.m_enableMotor && this.m_limitState !== b2Joint.e_equalLimits && !fixedRotation ) {
/*float32*/var Cdot = wB - wA - this.m_motorSpeed;
/*float32*/var impulse = -this.m_motorMass * Cdot;
/*float32*/var oldImpulse = this.m_motorImpulse;
/*float32*/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 limit constraint.
if ( this.m_enableLimit && this.m_limitState !== b2Joint.e_inactiveLimit && !fixedRotation ) {
// b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
var Cdot1 = b2Vec2.subtract(
b2Vec2.vPlusCrossFV( vB, wB, this.m_rB, b2Vec2.POOL0 ),
b2Vec2.vPlusCrossFV( vA, wA, this.m_rA, b2Vec2.POOL1 ),
b2RevoluteJoint._B2VEC2_POOL2 );
/*float32*/var Cdot2 = wB - wA;
// b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
// b2Vec3 impulse = -this.m_mass.solve33(Cdot);
var impulse = this.m_mass.solve33( Cdot1.x, Cdot1.y, Cdot2, b2RevoluteJoint._B2VEC3_POOL0 ).negative();
if ( this.m_limitState === b2Joint.e_equalLimits ) {
this.m_impulse.plus( impulse );
}
else if ( this.m_limitState === b2Joint.e_atLowerLimit ) {
/*float32*/var newImpulse = this.m_impulse.z + impulse.z;
if ( newImpulse < 0 ) {
// b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
var rhs_x = -Cdot1.x + this.m_impulse.z * this.m_mass.ez.x;
var rhs_y = -Cdot1.y + this.m_impulse.z * this.m_mass.ez.y;
/*b2Vec2*/var reduced = this.m_mass.solve22( rhs_x, rhs_y, b2RevoluteJoint._B2VEC2_POOL3 );
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -this.m_impulse.z;
this.m_impulse.x += reduced.x;
this.m_impulse.y += reduced.y;
this.m_impulse.z = 0;
}
else {
this.m_impulse.plus( impulse );
}
}
else if ( this.m_limitState === b2Joint.e_atUpperLimit ) {
/*float32*/var newImpulse = this.m_impulse.z + impulse.z;
if ( newImpulse > 0 ) {
// b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
var rhs_x = -Cdot1.x + this.m_impulse.z * this.m_mass.ez.x;
var rhs_y = -Cdot1.y + this.m_impulse.z * this.m_mass.ez.y;
/*b2Vec2*/var reduced = this.m_mass.solve22( rhs_x, rhs_y, b2RevoluteJoint._B2VEC2_POOL4 );
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -this.m_impulse.z;
this.m_impulse.x += reduced.x;
this.m_impulse.y += reduced.y;
this.m_impulse.z = 0;
}
else {
this.m_impulse.plus( impulse );
}
}
// P(impulse.x, impulse.y);
var P = b2RevoluteJoint._B2VEC2_POOL0.set( impulse.x, impulse.y );
// -= mA * P;
vA.minusEqualsMul( mA, P );
wA -= iA * (b2Vec2.cross( this.m_rA, P ) + impulse.z);
// vB += mB * P;
vB.plusEqualsMul( mB, P );
wB += iB * (b2Vec2.cross( this.m_rB, P ) + impulse.z);
}
else {
// Solve point-to-point constraint
// b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
var Cdot = b2Vec2.subtract(
b2Vec2.vPlusCrossFV( vB, wB, this.m_rB, b2Vec2.POOL0 ),
b2Vec2.vPlusCrossFV( vA, wA, this.m_rA, b2Vec2.POOL1 ),
b2RevoluteJoint._B2VEC2_POOL1 );
// b2Vec2 impulse = m_mass.solve22(-Cdot);
/*b2Vec2*/var impulse = this.m_mass.solve22( -Cdot.x, -Cdot.y, b2RevoluteJoint._B2VEC2_POOL5 );
this.m_impulse.x += impulse.x;
this.m_impulse.y += impulse.y;
// vA -= mA * impulse;
vA.minusEqualsMul( mA, impulse );
wA -= iA * b2Vec2.cross( this.m_rA, impulse );
// vB += mB * impulse;
vB.plusEqualsMul( mB, impulse );
wB += iB * b2Vec2.cross( this.m_rB, impulse );
}
// 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 qA(aA), qB(aB);
/*b2Rot*/var qA = this.m_qA.setAngle( aA ), qB = this.m_qB.setAngle( aB );
/*float32*/var angularError = 0;
/*float32*/var positionError = 0;
/*bool*/var fixedRotation = (this.m_invIA + this.m_invIB === 0);
// Solve angular limit constraint.
if ( this.m_enableLimit && this.m_limitState !== b2Joint.e_inactiveLimit && !fixedRotation ) {
/*float32*/var angle = aB - aA - this.m_referenceAngle;
/*float32*/var limitImpulse = 0;
if ( this.m_limitState === b2Joint.e_equalLimits ) {
// Prevent large angular corrections
/*float32*/var C = b2Math.clamp( angle - this.m_lowerAngle, -b2Settings.b2_maxAngularCorrection, b2Settings.b2_maxAngularCorrection );
limitImpulse = -this.m_motorMass * C;
angularError = Math.abs( C );
}
else if ( this.m_limitState === b2Joint.e_atLowerLimit ) {
/*float32*/var C = angle - this.m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = b2Math.clamp( C + b2Settings.b2_angularSlop, -b2Settings.b2_maxAngularCorrection, 0 );
limitImpulse = -this.m_motorMass * C;
}
else if ( this.m_limitState === b2Joint.e_atUpperLimit ) {
/*float32*/var C = angle - this.m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = b2Math.clamp( C - b2Settings.b2_angularSlop, 0, b2Settings.b2_maxAngularCorrection );
limitImpulse = -this.m_motorMass * C;
}
aA -= this.m_invIA * limitImpulse;
aB += this.m_invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.setAngle( aA );
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 C = cB + rB - cA - rA;
var C =
b2Vec2.subtract(
b2Vec2.add( cB, rB, b2Vec2.POOL0 ),
b2Vec2.add( cA, rA, b2Vec2.POOL1 ),
b2RevoluteJoint._B2VEC2_POOL0 );
positionError = C.length();
/*float32*/var mA = this.m_invMassA, mB = this.m_invMassB;
/*float32*/var iA = this.m_invIA, iB = this.m_invIB;
var K = this.m_K;
K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
// b2Vec2 impulse = -K.solve(C);
/*b2Vec2*/var impulse = K.solve( C.x, C.y, b2RevoluteJoint._B2VEC2_POOL1 ).negative();
// cA -= mA * impulse;
cA.minusEqualsMul( mA, impulse );
aA -= iA * b2Vec2.cross( rA, impulse );
// cB += mB * impulse;
cB.plusEqualsMul( mB, impulse );
aB += iB * b2Vec2.cross( rB, impulse );
}
// 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 positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop;
};
