public void InitVelocityConstraints(TimeStep step)
{
#if ALLOWUNSAFE
unsafe
{
// Warm start.
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
Vector2 normal = c.Normal;
Vector2 tangent = normal.CrossScalarPostMultiply(1.0f);
fixed (ContactConstraintPoint* pointsPtr = c.Points)
{
if (step.WarmStarting)
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint* ccp = &pointsPtr[j];
ccp->NormalImpulse *= step.DtRatio;
ccp->TangentImpulse *= step.DtRatio;
Vector2 P = ccp->NormalImpulse * normal + ccp->TangentImpulse * tangent;
bodyA._angularVelocity -= invIA * ccp->RA.Cross(P);
bodyA._linearVelocity -= invMassA * P;
bodyB._angularVelocity += invIB * ccp->RB.Cross(P);
bodyB._linearVelocity += invMassB * P;
}
}
else
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint* ccp = &pointsPtr[j];
ccp->NormalImpulse = 0.0f;
ccp->TangentImpulse = 0.0f;
}
}
}
}
}
#else
// Warm start.
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
Vector2 normal = c.Normal;
Vector2 tangent = normal.CrossScalarPostMultiply(1.0f);
ContactConstraintPoint[] points = c.Points;
if (step.WarmStarting)
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = points[j];
ccp.NormalImpulse *= step.DtRatio;
ccp.TangentImpulse *= step.DtRatio;
Vector2 P = ccp.NormalImpulse * normal + ccp.TangentImpulse * tangent;
bodyA._angularVelocity -= invIA * ccp.RA.Cross(P);
bodyA._linearVelocity -= invMassA * P;
bodyB._angularVelocity += invIB * ccp.RB.Cross(P);
bodyB._linearVelocity += invMassB * P;
}
}
else
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = points[j];
ccp.NormalImpulse = 0.0f;
ccp.TangentImpulse = 0.0f;
}
}
}
#endif
}