/// <summary>
/// Describes whether this instance solve position constraints
/// </summary>
/// <param name="baumgarte">The baumgarte</param>
/// <returns>The bool</returns>
internal override bool SolvePositionConstraints(float baumgarte)
{
Body body1 = Body1;
Body body2 = Body2;
Vec2 groundAnchor1 = Ground.GetXForm().Position + GroundAnchor1;
Vec2 groundAnchor2 = Ground.GetXForm().Position + GroundAnchor2;
float linearError = 0.0f;
if (State == LimitState.AtUpperLimit)
{
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
Vec2 body1SweepC = body1.Sweep.C + mulR1;
Vec2 body2SweepC = body2.Sweep.C + mulR2;
// Get the pulley axes.
U1 = body1SweepC - groundAnchor1;
U2 = body2SweepC - groundAnchor2;
float length1 = U1.Length();
float length2 = U2.Length();
if (length1 > Settings.LinearSlop)
{
U1 *= 1.0f / length1;
}
else
{
U1.SetZero();
}
if (length2 > Settings.LinearSlop)
{
U2 *= 1.0f / length2;
}
else
{
U2.SetZero();
}
float c = Constant - length1 - Ratio * length2;
linearError = Box2DXMath.Max(linearError, -c);
c = Box2DXMath.Clamp(c + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -PulleyMass * c;
Vec2 p1 = -impulse * U1;
Vec2 p2 = -Ratio * impulse * U2;
body1.Sweep.C += body1.InvMass * p1;
body1.Sweep.A += body1.InvI * Vec2.Cross(mulR1, p1);
body2.Sweep.C += body2.InvMass * p2;
body2.Sweep.A += body2.InvI * Vec2.Cross(mulR2, p2);
body1.SynchronizeTransform();
body2.SynchronizeTransform();
}
if (LimitState1 == LimitState.AtUpperLimit)
{
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 body1SweepC = body1.Sweep.C + mulR1;
U1 = body1SweepC - groundAnchor1;
float length1 = U1.Length();
if (length1 > Settings.LinearSlop)
{
U1 *= 1.0f / length1;
}
else
{
U1.SetZero();
}
float c = MaxLength1 - length1;
linearError = Box2DXMath.Max(linearError, -c);
c = Box2DXMath.Clamp(c + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -LimitMass1 * c;
Vec2 p1 = -impulse * U1;
body1.Sweep.C += body1.InvMass * p1;
body1.Sweep.A += body1.InvI * Vec2.Cross(mulR1, p1);
body1.SynchronizeTransform();
}
if (LimitState2 == LimitState.AtUpperLimit)
{
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
Vec2 body2SweepC = body2.Sweep.C + mulR2;
U2 = body2SweepC - groundAnchor2;
float length2 = U2.Length();
if (length2 > Settings.LinearSlop)
{
U2 *= 1.0f / length2;
}
else
{
U2.SetZero();
}
float c = MaxLength2 - length2;
linearError = Box2DXMath.Max(linearError, -c);
c = Box2DXMath.Clamp(c + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -LimitMass2 * c;
Vec2 p2 = -impulse * U2;
body2.Sweep.C += body2.InvMass * p2;
body2.Sweep.A += body2.InvI * Vec2.Cross(mulR2, p2);
body2.SynchronizeTransform();
}
return(linearError < Settings.LinearSlop);
}
/// <summary>
/// Inits the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void InitVelocityConstraints(TimeStep step)
{
Body body1 = Body1;
Body body2 = Body2;
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
Vec2 body1SweepC = body1.Sweep.C + mulR1;
Vec2 body2SweepC = body2.Sweep.C + mulR2;
Vec2 groundAnchor1 = Ground.GetXForm().Position + GroundAnchor1;
Vec2 groundAnchor2 = Ground.GetXForm().Position + GroundAnchor2;
// Get the pulley axes.
U1 = body1SweepC - groundAnchor1;
U2 = body2SweepC - groundAnchor2;
float length1 = U1.Length();
float length2 = U2.Length();
if (length1 > Settings.LinearSlop)
{
U1 *= 1.0f / length1;
}
else
{
U1.SetZero();
}
if (length2 > Settings.LinearSlop)
{
U2 *= 1.0f / length2;
}
else
{
U2.SetZero();
}
float c = Constant - length1 - Ratio * length2;
if (c > 0.0f)
{
State = LimitState.InactiveLimit;
Impulse = 0.0f;
}
else
{
State = LimitState.AtUpperLimit;
}
if (length1 < MaxLength1)
{
LimitState1 = LimitState.InactiveLimit;
LimitImpulse1 = 0.0f;
}
else
{
LimitState1 = LimitState.AtUpperLimit;
}
if (length2 < MaxLength2)
{
LimitState2 = LimitState.InactiveLimit;
LimitImpulse2 = 0.0f;
}
else
{
LimitState2 = LimitState.AtUpperLimit;
}
// Compute effective mass.
float cr1U1 = Vec2.Cross(mulR1, U1);
float cr2U2 = Vec2.Cross(mulR2, U2);
LimitMass1 = body1.InvMass + body1.InvI * cr1U1 * cr1U1;
LimitMass2 = body2.InvMass + body2.InvI * cr2U2 * cr2U2;
PulleyMass = LimitMass1 + Ratio * Ratio * LimitMass2;
Box2DxDebug.Assert(LimitMass1 > Settings.FltEpsilon);
Box2DxDebug.Assert(LimitMass2 > Settings.FltEpsilon);
Box2DxDebug.Assert(PulleyMass > Settings.FltEpsilon);
LimitMass1 = 1.0f / LimitMass1;
LimitMass2 = 1.0f / LimitMass2;
PulleyMass = 1.0f / PulleyMass;
if (step.WarmStarting)
{
// Scale impulses to support variable time steps.
Impulse *= step.DtRatio;
LimitImpulse1 *= step.DtRatio;
LimitImpulse2 *= step.DtRatio;
// Warm starting.
Vec2 p1 = -(Impulse + LimitImpulse1) * U1;
Vec2 p2 = (-Ratio * Impulse - LimitImpulse2) * U2;
body1.LinearVelocity += body1.InvMass * p1;
body1.AngularVelocity += body1.InvI * Vec2.Cross(mulR1, p1);
body2.LinearVelocity += body2.InvMass * p2;
body2.AngularVelocity += body2.InvI * Vec2.Cross(mulR2, p2);
}
else
{
Impulse = 0.0f;
LimitImpulse1 = 0.0f;
LimitImpulse2 = 0.0f;
}
}