internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vec2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.Dt * _maxMotorForce;
_motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vec2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
float Cdot1 = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vec2 Cdot = new Vec2(Cdot1, Cdot2);
Vec2 f1 = _impulse;
Vec2 df = _K.Solve(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLowerLimit)
{
_impulse.Y = Box2DX.Common.Math.Max(_impulse.Y, 0.0f);
}
else if (_limitState == LimitState.AtUpperLimit)
{
_impulse.Y = Box2DX.Common.Math.Min(_impulse.Y, 0.0f);
}
// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
float b = -Cdot1 - (_impulse.Y - f1.Y) * _K.Col2.X;
float f2r = b / _K.Col1.X + f1.X;
_impulse.X = f2r;
df = _impulse - f1;
Vec2 P = df.X * _perp + df.Y * _axis;
float L1 = df.X * _s1 + df.Y * _a1;
float L2 = df.X * _s2 + df.Y * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
float df = (-Cdot1) / _K.Col1.X;
_impulse.X += df;
Vec2 P = df * _perp;
float L1 = df * _s1;
float L2 = df * _s2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}