//################################################################################################//
//################################################################################################//
// //
// ██ ██████ ██████ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ ██ //
// █████ ██ ██ ██ ████ ██ █████ ████████ █████ █████ ██ █████ ██ █████ ██ █████ █████ //
// ██ ██ ██████ ██████ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ █████ ██ ██ ██ █████ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// █████ ██████ ██ ██ ██ █████ ██ ██ ██ █████ ████ ██ █████ █████ █████ ██ ██ ██ ████ //
// //
//################################################################################################//
//################################################################################################//
// CLASS CONSTRUCTOR
/**
* A prismatic joint. This joint provides one degree of freedom:
* translation along an axis fixed in bodyA. Relative rotation
* is prevented. You can use a joint limit to restrict the range
* of motion and a joint motor to drive the motion or to model
* joint friction.
*
* @class b2PrismaticJoint
* @constructor
* @param {b2PrismaticJointDef} prismaticJointDef
* @extends {b2Joint}
* @module Joints
*/
function b2PrismaticJoint( prismaticJointDef ) {
/**
* Invokes parent class constructor function reference.
*/
this.constructor( prismaticJointDef );
////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// ██████ ██ ██ //
// ██ ██ ██ //
// ██ ██ ████ █████ █████ █████ ████ █████ ██ █████ █████ //
// ██████ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ ██ █████ ██ ██ ██ █████ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ █████ █████ █████ ██ ████ ██ █████ █████ //
// ██ //
// ██ //
// //
////////////////////////////////////////////////////////////////////////////////////////////////////
// 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_impulse
* @type {b2Vec3}
*/
this.m_impulse = new b2Vec3;
/**
*
* @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_axis
* @type {b2Vec2}
*/
this.m_axis = new b2Vec2;
/**
*
* @public
* @property m_perp
* @type {b2Vec2}
*/
this.m_perp = new b2Vec2;
/**
*
* @public
* @property m_K
* @type {b2Mat33}
*/
this.m_K = new b2Mat33;
/**
*
* @public
* @property m_K2
* @type {b2Mat22}
*/
this.m_K2 = new b2Mat22;
/**
*
* @public
* @property m_K3
* @type {b2Mat33}
*/
this.m_K3 = new b2Mat33;
/**
*
* @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( prismaticJointDef.localAnchorA );
this.m_localAnchorB.copy( prismaticJointDef.localAnchorB );
this.m_localXAxisA.equals( prismaticJointDef.localAxisA ).normalized();
/*this.m_localYAxisA = */b2Vec2.numCross( 1.0, this.m_localXAxisA, this.m_localYAxisA );
/**
*
* @public
* @property m_refAngle
* @type {float}
*/
this.m_referenceAngle = prismaticJointDef.referenceAngle;
/**
*
* @public
* @property m_lowerTranslation
* @type {float}
*/
this.m_lowerTranslation = prismaticJointDef.lowerTranslation;
/**
*
* @public
* @property m_upperTranslation
* @type {float}
*/
this.m_upperTranslation = prismaticJointDef.upperTranslation;
/**
*
* @public
* @property m_maxMotorForce
* @type {float}
*/
this.m_maxMotorForce = prismaticJointDef.maxMotorForce;
/**
*
* @public
* @property m_motorSpeed
* @type {float}
*/
this.m_motorSpeed = prismaticJointDef.motorSpeed;
/**
*
* @public
* @property m_enableLimit
* @type {float}
*/
this.m_enableLimit = prismaticJointDef.enableLimit;
/**
*
* @public
* @property m_enableMotor
* @type {boolean}
*/
this.m_enableMotor = prismaticJointDef.enableMotor;
/**
*
* @public
* @property m_motorMass
* @type {float}
* @default 0.0
*/
this.m_motorMass = 0.0;
/**
*
* @public
* @property m_motorImpulse
* @type {float}
* @default 0.0
*/
this.m_motorImpulse = 0.0;
/**
*
* @public
* @property m_limitState
* @type {int}
* @default b2Joint.e_inactiveLimit
*/
this.m_limitState = b2Joint.e_inactiveLimit;
/**
*
* @public
* @property m_s1
* @type {float}
* @default 0.0
*/
this.m_s1 = 0.0;
/**
*
* @public
* @property m_s2
* @type {float}
* @default 0.0
*/
this.m_s2 = 0.0;
/**
*
* @public
* @property m_a1
* @type {float}
* @default 0.0
*/
this.m_a1 = 0.0;
/**
*
* @public
* @property m_a2
* @type {float}
* @default 0.0
*/
this.m_a2 = 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_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;
////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// ██ ██ ██ ██ //
// ██ ██ ██ //
// ██ █████ █████ █████ ████ ██ █████ █████ █████ █████ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ █████ ██ ██ ██ █████ ██ ██ ██ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ ██ ██ ██ █████ ██ ██ ████ █████ ██ ██ █████ █████ //
// //
////////////////////////////////////////////////////////////////////////////////////////////////////
} b2PrismaticJoint.prototype = p = new b2Joint; Box2D.b2PrismaticJoint = b2PrismaticJoint;
// STATIC CLASS PROPERTIES
/**
* Object pool for memory management.
*/
b2PrismaticJoint._B2VEC2_POOL0 = new b2Vec2;
b2PrismaticJoint._B2VEC2_POOL1 = new b2Vec2;
b2PrismaticJoint._B2VEC2_POOL2 = new b2Vec2;
b2PrismaticJoint._B2VEC2_POOL3 = new b2Vec2;
b2PrismaticJoint._B2VEC3_POOL0 = new b2Vec3;
b2PrismaticJoint._B2VEC3_POOL1 = new b2Vec3;
////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// ██ ██ ██ ██ ██ //
// ███ ███ ██ ██ ██ //
// ███████ █████ █████ █████ █████ █████ █████ //
// ██ █ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ █████ ██ ██ ██ ██ ██ ██ ██ █████ //
// ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ ██ //
// ██ ██ █████ ████ ██ ██ █████ █████ █████ //
// //
////////////////////////////////////////////////////////////////////////////////////////////////////
// INSTANCE METHODS
/**
* @public
* @method getAnchorB
* @param {b2Vec2|Object=} [out=b2Vec2] reusable object
* @return {b2Vec2|Object} out
*/
p.getAnchorA = function ( out ) {
out = out || b2PrismaticJoint._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 || b2PrismaticJoint._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 || b2PrismaticJoint._B2VEC2_POOL0;
out.x = invDeltaTime * (this.m_impulse.x * this.m_perp.x + (this.m_motorImpulse + this.m_impulse.z) * this.m_axis.x);
out.y = invDeltaTime * (this.m_impulse.x * this.m_perp.y + (this.m_motorImpulse + this.m_impulse.z) * this.m_axis.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_impulse.y;
};
/**
* 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 || b2PrismaticJoint._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 || b2PrismaticJoint._B2VEC2_POOL0;
return out.copy( this.m_localAnchorB );
};
/**
* The local joint axis relative to bodyA.
*
* @public
* @method getJointTranslation
* @param {b2Vec2|Object=} [out=b2Vec2] reusable object
* @return {b2Vec2|Object} out
*/
p.getJointTranslation = function ( out ) {
var pA = this.m_bodyA.getWorldPoint( this.m_localAnchorA, b2PrismaticJoint._B2VEC2_POOL0 );
var pB = this.m_bodyB.getWorldPoint( this.m_localAnchorB, b2PrismaticJoint._B2VEC2_POOL1 );
var d = b2Vec2.subtract( pB, pA, b2PrismaticJoint._B2VEC2_POOL2 );
var axis = this.m_bodyA.getWorldVector( this.m_localXAxisA, b2PrismaticJoint._B2VEC2_POOL3 );
var translation = b2Vec2.dot( d, axis );
return translation;
};
/**
* The local joint axis relative to bodyA.
*
* @public
* @method getLocalAxisA
* @param {b2Vec2|Object=} [out=b2Vec2] reusable object
* @return {b2Vec2|Object} out
*/
p.getLocalAxisA = function ( out ) {
out = out || b2PrismaticJoint._B2VEC2_POOL0;
return out.copy( this.m_localXAxisA );
};
/**
* The local joint axis relative to bodyA.
*
* @public
* @method getReferenceAngle
* @return {float}
*/
p.getReferenceAngle = function () {
return this.m_referenceAngle;
};
/**
* @public
* @method getJointSpeed
* @return {b2Vec2}
*/
p.getJointSpeed = function () {
/*b2Body*/
var bA = this.m_bodyA;
/*b2Body*/
var bB = this.m_bodyB;
b2Vec2.subtract( this.m_localAnchorA, bA.m_sweep.localCenter, this.m_lalcA );
var rA = b2Rot.timesV2( bA.m_xf.q, this.m_lalcA, this.m_rA );
b2Vec2.subtract( this.m_localAnchorB, bB.m_sweep.localCenter, this.m_lalcB );
var rB = b2Rot.timesV2( bB.m_xf.q, this.m_lalcB, this.m_rB );
var pA = b2Vec2.add( bA.m_sweep.c, rA, b2Vec2.POOL0 );
var pB = b2Vec2.add( bB.m_sweep.c, rB, b2Vec2.POOL1 );
var d = b2Vec2.subtract( pB, pA, b2Vec2.POOL2 );
var axis = bA.getWorldVector( this.m_localXAxisA, this.m_axis );
var vA = bA.m_linearVelocity;
var vB = bB.m_linearVelocity;
var wA = bA.m_angularVelocity;
var wB = bB.m_angularVelocity;
var speed =
b2Vec2.dot( d, b2Vec2.numCross( wA, axis, b2Vec2.POOL0 ) ) +
b2Vec2.dot(
axis,
b2Vec2.subtract(
b2Vec2.vPlusCrossFV( vB, wB, rB, b2Vec2.POOL0 ),
b2Vec2.vPlusCrossFV( vA, wA, rA, b2Vec2.POOL1 ),
b2Vec2.POOL0 ) );
return speed;
};
/**
* @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;
}
};
/**
*
* @public
* @method getLowerLimit
* @return {float}
*/
p.getLowerLimit = function () {
return this.m_lowerTranslation;
};
/**
*
* @public
* @method getUpperLimit
* @return {float}
*/
p.getUpperLimit = function () {
return this.m_upperTranslation;
};
/**
*
* @public
* @method setLimits
* @param {float} upper
* @param {float} lower
* @return {void}
*/
p.setLimits = function ( lower, upper ) {
if ( lower !== this.m_lowerTranslation || upper !== this.m_upperTranslation ) {
this.m_bodyA.setAwake( true );
this.m_bodyB.setAwake( true );
this.m_lowerTranslation = lower;
this.m_upperTranslation = upper;
this.m_impulse.z = 0;
}
};
/**
* @public
* @method isMotorEnabled
* @return {boolean}
*/
p.isMotorEnabled = function () {
return this.m_enableMotor;
};
/**
* @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;
};
/**
* @public
* @method setMotorSpeed
* @param {float} speed
* @return {void}
*/
p.setMotorSpeed = function ( speed ) {
speed = speed || 0;
this.m_bodyA.setAwake( true );
this.m_bodyB.setAwake( true );
this.m_motorSpeed = speed;
};
/**
* @public
* @method getMotorSpeed
* @return {float}
*/
p.getMotorSpeed = function () {
return this.m_motorSpeed;
};
/**
* @public
* @method setMaxMotorForce
* @param {float} force
* @return {void}
*/
p.setMaxMotorForce = function ( force ) {
force = force || 0;
this.m_bodyA.setAwake( true );
this.m_bodyB.setAwake( true );
this.m_maxMotorForce = force;
};
/**
* @public
* @method getMaxMotorForce
* @return {float}
*/
p.getMaxMotorForce = function () {
return this.m_maxMotorForce;
};
/**
* @public
* @method getMaxMotorForce
* @param {float} invDeltaTime
* @return {float}
*/
p.getMaxMotorForce = function ( invDeltaTime ) {
return invDeltaTime * this.m_motorImpulse;
};
/**
* @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;
/*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 - cA) + rB - rA;
var d = b2Vec2.add(
b2Vec2.subtract( cB, cA, b2Vec2.POOL0 ),
b2Vec2.subtract( rB, rA, b2Vec2.POOL1 ),
b2PrismaticJoint._B2VEC2_POOL0 );
/*float32*/
var mA = this.m_invMassA, mB = this.m_invMassB;
/*float32*/
var iA = this.m_invIA, iB = this.m_invIB;
// Compute motor Jacobian and effective mass.
{
// m_axis = b2Mul(qA, m_localXAxisA);
b2Rot.timesV2( qA, this.m_localXAxisA, this.m_axis );
// m_a1 = b2Cross(d + rA, m_axis);
this.m_a1 = b2Vec2.cross( b2Vec2.add( d, rA, b2Vec2.POOL0 ), this.m_axis );
// m_a2 = b2Cross(rB, m_axis);
this.m_a2 = b2Vec2.cross( rB, this.m_axis );
this.m_motorMass = mA + mB + iA * this.m_a1 * this.m_a1 + iB * this.m_a2 * this.m_a2;
if ( this.m_motorMass > 0 ) {
this.m_motorMass = 1 / this.m_motorMass;
}
}
// Prismatic constraint.
{
// m_perp = b2Mul(qA, m_localYAxisA);
b2Rot.timesV2( qA, this.m_localYAxisA, this.m_perp );
// m_s1 = b2Cross(d + rA, m_perp);
this.m_s1 = b2Vec2.cross( b2Vec2.add( d, rA, b2Vec2.POOL0 ), this.m_perp );
// m_s2 = b2Cross(rB, m_perp);
this.m_s2 = b2Vec2.cross( rB, this.m_perp );
// float32 s1test;
// s1test = b2Cross(rA, m_perp);
// float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
this.m_K.ex.x = mA + mB + iA * this.m_s1 * this.m_s1 + iB * this.m_s2 * this.m_s2;
// float32 k12 = iA * m_s1 + iB * m_s2;
this.m_K.ex.y = iA * this.m_s1 + iB * this.m_s2;
// float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
this.m_K.ex.z = iA * this.m_s1 * this.m_a1 + iB * this.m_s2 * this.m_a2;
this.m_K.ey.x = this.m_K.ex.y;
// float32 k22 = iA + iB;
this.m_K.ey.y = iA + iB;
if ( this.m_K.ey.y === 0 ) {
// For bodies with fixed rotation.
this.m_K.ey.y = 1;
}
// float32 k23 = iA * m_a1 + iB * m_a2;
this.m_K.ey.z = iA * this.m_a1 + iB * this.m_a2;
this.m_K.ez.x = this.m_K.ex.z;
this.m_K.ez.y = this.m_K.ey.z;
// float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
this.m_K.ez.z = mA + mB + iA * this.m_a1 * this.m_a1 + iB * this.m_a2 * this.m_a2;
// m_K.ex.set(k11, k12, k13);
// m_K.ey.set(k12, k22, k23);
// m_K.ez.set(k13, k23, k33);
}
// Compute motor and limit terms.
if ( this.m_enableLimit ) {
// float32 jointTranslation = b2Dot(m_axis, d);
var jointTranslation = b2Vec2.dot( this.m_axis, d );
if ( Math.abs( this.m_upperTranslation - this.m_lowerTranslation ) < 2 * b2Settings.b2_linearSlop ) {
this.m_limitState = b2Joint.e_equalLimits;
}
else if ( jointTranslation <= this.m_lowerTranslation ) {
if ( this.m_limitState !== b2Joint.e_atLowerLimit ) {
this.m_limitState = b2Joint.e_atLowerLimit;
this.m_impulse.z = 0;
}
}
else if ( jointTranslation >= this.m_upperTranslation ) {
if ( this.m_limitState !== b2Joint.e_atUpperLimit ) {
this.m_limitState = b2Joint.e_atUpperLimit;
this.m_impulse.z = 0;
}
}
else {
this.m_limitState = b2Joint.e_inactiveLimit;
this.m_impulse.z = 0;
}
}
else {
this.m_limitState = b2Joint.e_inactiveLimit;
this.m_impulse.z = 0;
}
if ( !this.m_enableMotor ) {
this.m_motorImpulse = 0;
}
if ( data.step.warmStarting ) {
// Account for variable time step.
// m_impulse *= data.step.dtRatio;
this.m_impulse.timesScalar( data.step.dtRatio );
this.m_motorImpulse *= data.step.dtRatio;
// b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;
var P = b2Vec2.add(
b2Vec2.numTimes( this.m_impulse.x, this.m_perp, b2Vec2.POOL0 ),
b2Vec2.numTimes( (this.m_motorImpulse + this.m_impulse.z), this.m_axis, b2Vec2.POOL1 ),
b2PrismaticJoint._B2VEC2_POOL1 );
// float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
var LA = this.m_impulse.x * this.m_s1 + this.m_impulse.y + (this.m_motorImpulse + this.m_impulse.z) * this.m_a1;
// float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
var LB = this.m_impulse.x * this.m_s2 + this.m_impulse.y + (this.m_motorImpulse + this.m_impulse.z) * this.m_a2;
// vA -= mA * P;
vA.minusEqualsMul( mA, P );
wA -= iA * LA;
// vB += mB * P;
vB.plusEqualsMul( mB, P );
wB += iB * LB;
}
else {
this.m_impulse.setZero();
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 ) {
/*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;
// Solve linear motor constraint.
if ( this.m_enableMotor && this.m_limitState !== b2Joint.e_equalLimits ) {
// float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
var Cdot = b2Vec2.dot( this.m_axis, b2Vec2.subtract( vB, vA, b2Vec2.POOL0 ) ) + this.m_a2 * wB - this.m_a1 * wA;
var impulse = this.m_motorMass * (this.m_motorSpeed - Cdot);
var oldImpulse = this.m_motorImpulse;
var maxImpulse = data.step.dt * this.m_maxMotorForce;
this.m_motorImpulse = b2Math.clamp( this.m_motorImpulse + impulse, (-maxImpulse), maxImpulse );
impulse = this.m_motorImpulse - oldImpulse;
// b2Vec2 P = impulse * m_axis;
var P = b2Vec2.numTimes( impulse, this.m_axis, b2PrismaticJoint._B2VEC2_POOL0 );
var LA = impulse * this.m_a1;
var LB = impulse * this.m_a2;
// vA -= mA * P;
vA.minusEqualsMul( mA, P );
wA -= iA * LA;
// vB += mB * P;
vB.plusEqualsMul( mB, P );
wB += iB * LB;
}
// b2Vec2 Cdot1;
// Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
var Cdot1_x = b2Vec2.dot( this.m_perp, b2Vec2.subtract( vB, vA, b2Vec2.POOL0 ) ) + this.m_s2 * wB - this.m_s1 * wA;
// Cdot1.y = wB - wA;
var Cdot1_y = wB - wA;
if ( this.m_enableLimit && this.m_limitState !== b2Joint.e_inactiveLimit ) {
// Solve prismatic and limit constraint in block form.
// float32 Cdot2;
// Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
var Cdot2 = b2Vec2.dot( this.m_axis, b2Vec2.subtract( vB, vA, b2Vec2.POOL0 ) ) + this.m_a2 * wB - this.m_a1 * wA;
// b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
// b2Vec3 f1 = m_impulse;
var f1 = b2PrismaticJoint._B2VEC3_POOL0.copy( this.m_impulse );
// b2Vec3 df = m_K.solve33(-Cdot);
var df = this.m_K.solve33( (-Cdot1_x), (-Cdot1_y), (-Cdot2), b2PrismaticJoint._B2VEC3_POOL1 );
// m_impulse += df;
this.m_impulse.plus( df );
if ( this.m_limitState === b2Joint.e_atLowerLimit ) {
this.m_impulse.z = Math.max( this.m_impulse.z, 0 );
}
else if ( this.m_limitState === b2Joint.e_atUpperLimit ) {
this.m_impulse.z = Math.min( this.m_impulse.z, 0 );
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
// b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y);
var b_x = (-Cdot1_x) - (this.m_impulse.z - f1.z) * this.m_K.ez.x;
var b_y = (-Cdot1_y) - (this.m_impulse.z - f1.z) * this.m_K.ez.y;
// b2Vec2 f2r = m_K.solve22(b) + b2Vec2(f1.x, f1.y);
var f2r = this.m_K.solve22( b_x, b_y, b2PrismaticJoint._B2VEC2_POOL1 );
f2r.x += f1.x;
f2r.y += f1.y;
// m_impulse.x = f2r.x;
this.m_impulse.x = f2r.x;
// m_impulse.y = f2r.y;
this.m_impulse.y = f2r.y;
// df = m_impulse - f1;
df.x = this.m_impulse.x - f1.x;
df.y = this.m_impulse.y - f1.y;
df.z = this.m_impulse.z - f1.z;
// b2Vec2 P = df.x * m_perp + df.z * m_axis;
var P = b2Vec2.add(
b2Vec2.numTimes( df.x, this.m_perp, b2Vec2.POOL0 ),
b2Vec2.numTimes( df.z, this.m_axis, b2Vec2.POOL1 ),
b2PrismaticJoint._B2VEC2_POOL0 );
// float32 LA = df.x * m_s1 + df.y + df.z * m_a1;
var LA = df.x * this.m_s1 + df.y + df.z * this.m_a1;
// float32 LB = df.x * m_s2 + df.y + df.z * m_a2;
var LB = df.x * this.m_s2 + df.y + df.z * this.m_a2;
// vA -= mA * P;
vA.minusEqualsMul( mA, P );
wA -= iA * LA;
// vB += mB * P;
vB.plusEqualsMul( mB, P );
wB += iB * LB;
}
else {
// Limit is inactive, just solve the prismatic constraint in block form.
// b2Vec2 df = m_K.solve22(-Cdot1);
var df = this.m_K.solve22( (-Cdot1_x), (-Cdot1_y), b2PrismaticJoint._B2VEC2_POOL2 );
this.m_impulse.x += df.x;
this.m_impulse.y += df.y;
// b2Vec2 P = df.x * m_perp;
var P = b2Vec2.numTimes( df.x, this.m_perp, b2PrismaticJoint._B2VEC2_POOL0 );
// float32 LA = df.x * m_s1 + df.y;
var LA = df.x * this.m_s1 + df.y;
// float32 LB = df.x * m_s2 + df.y;
var LB = df.x * this.m_s2 + df.y;
// 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 );
/*float32*/
var mA = this.m_invMassA, mB = this.m_invMassB;
/*float32*/
var iA = this.m_invIA, iB = this.m_invIB;
// b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
var rA = b2Rot.timesV2( qA, this.m_lalcA, this.m_rA );
// b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
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 ),
b2PrismaticJoint._B2VEC2_POOL0 );
// b2Vec2 axis = b2Mul(qA, m_localXAxisA);
var axis = b2Rot.timesV2( qA, this.m_localXAxisA, this.m_axis );
// float32 a1 = b2Cross(d + rA, axis);
var a1 = b2Vec2.cross( b2Vec2.add( d, rA, b2Vec2.POOL0 ), axis );
// float32 a2 = b2Cross(rB, axis);
var a2 = b2Vec2.cross( rB, axis );
// b2Vec2 perp = b2Mul(qA, m_localYAxisA);
var perp = b2Rot.timesV2( qA, this.m_localYAxisA, this.m_perp );
// float32 s1 = b2Cross(d + rA, perp);
var s1 = b2Vec2.cross( b2Vec2.add( d, rA, b2Vec2.POOL0 ), perp );
// float32 s2 = b2Cross(rB, perp);
var s2 = b2Vec2.cross( rB, perp );
// b2Vec3 impulse;
var impulse = b2PrismaticJoint._B2VEC3_POOL0;
// b2Vec2 C1;
// C1.x = b2Dot(perp, d);
var C1_x = b2Vec2.dot( perp, d );
// C1.y = aB - aA - m_referenceAngle;
var C1_y = aB - aA - this.m_referenceAngle;
var linearError = Math.abs( C1_x );
var angularError = Math.abs( C1_y );
var active = false;
var C2 = 0;
if ( this.m_enableLimit ) {
// float32 translation = b2Dot(axis, d);
var translation = b2Vec2.dot( axis, d );
if ( Math.abs( this.m_upperTranslation - this.m_lowerTranslation ) < 2 * b2Settings.b2_linearSlop ) {
// Prevent large angular corrections
C2 = b2Math.clamp( translation, (-b2Settings.b2_maxLinearCorrection), b2Settings.b2_maxLinearCorrection );
linearError = Math.max( linearError, Math.abs( translation ) );
active = true;
}
else if ( translation <= this.m_lowerTranslation ) {
// Prevent large linear corrections and allow some slop.
C2 = b2Math.clamp( translation - this.m_lowerTranslation + b2Settings.b2_linearSlop, (-b2Settings.b2_maxLinearCorrection), 0 );
linearError = Math.max( linearError, this.m_lowerTranslation - translation );
active = true;
}
else if ( translation >= this.m_upperTranslation ) {
// Prevent large linear corrections and allow some slop.
C2 = b2Math.clamp( translation - this.m_upperTranslation - b2Settings.b2_linearSlop, 0, b2Settings.b2_maxLinearCorrection );
linearError = Math.max( linearError, translation - this.m_upperTranslation );
active = true;
}
}
if ( active ) {
// float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
var k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
// float32 k12 = iA * s1 + iB * s2;
var k12 = iA * s1 + iB * s2;
// float32 k13 = iA * s1 * a1 + iB * s2 * a2;
var k13 = iA * s1 * a1 + iB * s2 * a2;
// float32 k22 = iA + iB;
var k22 = iA + iB;
if ( k22 === 0 ) {
// For fixed rotation
k22 = 1;
}
// float32 k23 = iA * a1 + iB * a2;
var k23 = iA * a1 + iB * a2;
// float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
var k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
// b2Mat33 K;
var K = this.m_K3;
K.ex.set( k11, k12, k13 );
K.ey.set( k12, k22, k23 );
K.ez.set( k13, k23, k33 );
// b2Vec3 C;
// C.x = C1.x;
// C.y = C1.y;
// C.z = C2;
// impulse = K.solve33(-C);
impulse = K.solve33( (-C1_x), (-C1_y), (-C2), impulse );
}
else {
// float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
var k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
// float32 k12 = iA * s1 + iB * s2;
var k12 = iA * s1 + iB * s2;
// float32 k22 = iA + iB;
var k22 = iA + iB;
if ( k22 === 0 ) {
k22 = 1;
}
// b2Mat22 K;
var K2 = this.m_K2;
// K.ex.set(k11, k12);
K2.ex.set( k11, k12 );
// K.ey.set(k12, k22);
K2.ey.set( k12, k22 );
// b2Vec2 impulse1 = K.solve(-C1);
var impulse1 = K2.solve( (-C1_x), (-C1_y), b2PrismaticJoint._B2VEC2_POOL1 );
impulse.x = impulse1.x;
impulse.y = impulse1.y;
impulse.z = 0;
}
// b2Vec2 P = impulse.x * perp + impulse.z * axis;
var P = b2Vec2.add(
b2Vec2.numTimes( impulse.x, perp, b2Vec2.POOL0 ),
b2Vec2.numTimes( impulse.z, axis, b2Vec2.POOL1 ),
b2PrismaticJoint._B2VEC2_POOL2 );
// float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1;
var LA = impulse.x * s1 + impulse.y + impulse.z * a1;
// float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2;
var LB = impulse.x * s2 + impulse.y + impulse.z * a2;
// cA -= mA * P;
cA.minusEqualsMul( mA, P );
aA -= iA * LA;
// cB += mB * P;
cB.plusEqualsMul( mB, P );
aB += iB * 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 linearError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop;
};
