public PrismaticJoint(PrismaticJointDef def)
: base(def)
{
_localAnchor1 = def.LocalAnchor1;
_localAnchor2 = def.LocalAnchor2;
_localXAxis1 = def.LocalAxis1;
_localYAxis1 = Vec2.Cross(1.0f, _localXAxis1);
_refAngle = def.ReferenceAngle;
_linearJacobian.SetZero();
_linearMass = 0.0f;
_force = 0.0f;
_angularMass = 0.0f;
_torque = 0.0f;
_motorJacobian.SetZero();
_motorMass = 0.0f;
_motorForce = 0.0f;
_limitForce = 0.0f;
_limitPositionImpulse = 0.0f;
_lowerTranslation = def.LowerTranslation;
_upperTranslation = def.UpperTranslation;
_maxMotorForce = Settings.FORCE_INV_SCALE(def.MaxMotorForce);
_motorSpeed = def.MotorSpeed;
_enableLimit = def.EnableLimit;
_enableMotor = def.EnableMotor;
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body g1 = _ground1;
Body g2 = _ground2;
Body b1 = _body1;
Body b2 = _body2;
float K = 0.0f;
_J.SetZero();
if (_revolute1 != null)
{
_J.Angular1 = -1.0f;
K += b1._invI;
}
else
{
Vec2 ug = Common.Math.Mul(g1.GetXForm().R, _prismatic1._localXAxis1);
Vec2 r = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
float crug = Vec2.Cross(r, ug);
_J.Linear1 = -ug;
_J.Angular1 = -crug;
K += b1._invMass + b1._invI * crug * crug;
}
if (_revolute2 != null)
{
_J.Angular2 = -_ratio;
K += _ratio * _ratio * b2._invI;
}
else
{
Vec2 ug = Common.Math.Mul(g2.GetXForm().R, _prismatic2._localXAxis1);
Vec2 r = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
float crug = Vec2.Cross(r, ug);
_J.Linear2 = -_ratio * ug;
_J.Angular2 = -_ratio * crug;
K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
}
// Compute effective mass.
Box2DXDebug.Assert(K > 0.0f);
_mass = 1.0f / K;
if (step.WarmStarting)
{
// Warm starting.
float P = Settings.FORCE_SCALE(step.Dt) * _force;
b1._linearVelocity += b1._invMass * P * _J.Linear1;
b1._angularVelocity += b1._invI * P * _J.Angular1;
b2._linearVelocity += b2._invMass * P * _J.Linear2;
b2._angularVelocity += b2._invI * P * _J.Angular2;
}
else
{
_force = 0.0f;
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body g1 = _ground1;
Body g2 = _ground2;
Body b1 = _body1;
Body b2 = _body2;
float K = 0.0f;
_J.SetZero();
if (_revolute1 != null)
{
_J.Angular1 = -1.0f;
K += b1._invI;
}
else
{
Vector2 ug = g1.GetTransform().TransformDirection(_prismatic1._localXAxis1);
Vector2 r = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
float crug = r.Cross(ug);
_J.Linear1 = -ug;
_J.Angular1 = -crug;
K += b1._invMass + b1._invI * crug * crug;
}
if (_revolute2 != null)
{
_J.Angular2 = -_ratio;
K += _ratio * _ratio * b2._invI;
}
else
{
Vector2 ug = g2.GetTransform().TransformDirection(_prismatic2._localXAxis1);
Vector2 r = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
float crug = r.Cross(ug);
_J.Linear2 = -_ratio * ug;
_J.Angular2 = -_ratio * crug;
K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
}
// Compute effective mass.
Box2DXDebug.Assert(K > 0.0f);
_mass = 1.0f / K;
if (step.WarmStarting)
{
// Warm starting.
b1._linearVelocity += b1._invMass * _impulse * _J.Linear1;
b1._angularVelocity += b1._invI * _impulse * _J.Angular1;
b2._linearVelocity += b2._invMass * _impulse * _J.Linear2;
b2._angularVelocity += b2._invI * _impulse * _J.Angular2;
}
else
{
_impulse = 0.0f;
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body g1 = _ground1;
Body g2 = _ground2;
Body b1 = _bodyA;
Body b2 = _bodyB;
float K = 0.0f;
_J.SetZero();
if (_revolute1 != null)
{
_J.Angular1 = -1.0f;
K += b1._invI;
}
else
{
Vec2 ug = Math.Mul(g1.GetTransform().R, _prismatic1._localXAxis1);
Vec2 r = Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
float crug = Vec2.Cross(r, ug);
_J.Linear1 = -ug;
_J.Angular1 = -crug;
K += b1._invMass + b1._invI * crug * crug;
}
if (_revolute2 != null)
{
_J.Angular2 = -_ratio;
K += _ratio * _ratio * b2._invI;
}
else
{
Vec2 ug = Math.Mul(g2.GetTransform().R, _prismatic2._localXAxis1);
Vec2 r = Math.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());
float crug = Vec2.Cross(r, ug);
_J.Linear2 = -_ratio * ug;
_J.Angular2 = -_ratio * crug;
K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
}
// Compute effective mass.
_mass = K > 0.0f ? 1.0f / K : 0.0f;
if (step.WarmStarting)
{
// Warm starting.
b1._linearVelocity += b1._invMass * _impulse * _J.Linear1;
b1._angularVelocity += b1._invI * _impulse * _J.Angular1;
b2._linearVelocity += b2._invMass * _impulse * _J.Linear2;
b2._angularVelocity += b2._invI * _impulse * _J.Angular2;
}
else
{
_impulse = 0.0f;
}
}
