internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vec2 pivotCdot = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2) - b1._linearVelocity -
Vec2.Cross(b1._angularVelocity, r1);
Vec2 pivotForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * Box2DXMath.Mul(_pivotMass, pivotCdot);
#if B2_TOI_JOINTS
if (step.WarmStarting)
{
_pivotForce += pivotForce;
_lastWarmStartingPivotForce = _pivotForce;
}
else
{
_pivotForce = _lastWarmStartingPivotForce;
//Do not update warm starting value!
}
#else
_pivotForce += pivotForce;
#endif
Vec2 P = Settings.FORCE_SCALE(step.Dt) * pivotForce;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * Vec2.Cross(r2, P);
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float motorCdot = b2._angularVelocity - b1._angularVelocity - _motorSpeed;
float motorForce = -step.Inv_Dt * _motorMass * motorCdot;
float oldMotorForce = _motorForce;
_motorForce = Box2DXMath.Clamp(_motorForce + motorForce, -_maxMotorTorque, _maxMotorTorque);
motorForce = _motorForce - oldMotorForce;
float P_ = step.Dt * motorForce;
b1._angularVelocity -= b1._invI * P_;
b2._angularVelocity += b2._invI * P_;
}
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float limitCdot = b2._angularVelocity - b1._angularVelocity;
float limitForce = -step.Inv_Dt * _motorMass * limitCdot;
if (_limitState == LimitState.EqualLimits)
{
_limitForce += limitForce;
}
else if (_limitState == LimitState.AtLowerLimit)
{
float oldLimitForce = _limitForce;
_limitForce = Box2DXMath.Max(_limitForce + limitForce, 0.0f);
limitForce = _limitForce - oldLimitForce;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float oldLimitForce = _limitForce;
_limitForce = Box2DXMath.Min(_limitForce + limitForce, 0.0f);
limitForce = _limitForce - oldLimitForce;
}
float P_ = step.Dt * limitForce;
b1._angularVelocity -= b1._invI * P_;
b2._angularVelocity += b2._invI * P_;
}
}
