// Sequential position solver for position constraints.
bool ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB)
{
float32 minSeparation = 0.0f;
for (int32 i = 0; i < m_count; ++i)
{
ContactPositionConstraint* pc = m_positionConstraints + i;
int32 indexA = pc->indexA;
int32 indexB = pc->indexB;
Vec2 localCenterA = pc->localCenterA;
Vec2 localCenterB = pc->localCenterB;
int32 pointCount = pc->pointCount;
float32 mA = 0.0f;
float32 iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc->invMassA;
iA = pc->invIA;
}
float32 mB = 0.0f;
float32 iB = 0.;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc->invMassB;
iB = pc->invIB;
}
Vec2 cA = m_positions[indexA].c;
float32 aA = m_positions[indexA].a;
Vec2 cB = m_positions[indexB].c;
float32 aB = m_positions[indexB].a;
// Solve normal constraints
for (int32 j = 0; j < pointCount; ++j)
{
Transform xfA, xfB;
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - Mul(xfA.q, localCenterA);
xfB.p = cB - Mul(xfB.q, localCenterB);
PositionSolverManifold psm;
psm.Initialize(pc, xfA, xfB, j);
Vec2 normal = psm.normal;
Vec2 point = psm.point;
float32 separation = psm.separation;
Vec2 rA = point - cA;
Vec2 rB = point - cB;
// Track max constraint error.
minSeparation = Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float32 C = Clamp(toiBaugarte * (separation + linearSlop), -maxLinearCorrection, 0.0f);
// Compute the effective mass.
float32 rnA = Cross(rA, normal);
float32 rnB = Cross(rB, normal);
float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float32 impulse = K > 0.0f ? - C / K : 0.0f;
Vec2 P = impulse * normal;
cA -= mA * P;
aA -= iA * Cross(rA, P);
cB += mB * P;
aB += iB * Cross(rB, P);
}
m_positions[indexA].c = cA;
m_positions[indexA].a = aA;
m_positions[indexB].c = cB;
m_positions[indexB].a = aB;
}
// We can't expect minSpeparation >= -linearSlop because we don't
// push the separation above -linearSlop.
return minSeparation >= -1.5f * linearSlop;
}
// Sequential solver.
bool ContactSolver::SolvePositionConstraints()
{
real32 minSeparation = 0.0f;
for (s32 i = 0; i < m_count; ++i)
{
ContactPositionConstraint* pc = m_positionConstraints + i;
s32 indexA = pc->indexA;
s32 indexB = pc->indexB;
glm::vec2 localCenterA = pc->localCenterA;
real32 mA = pc->invMassA;
real32 iA = pc->invIA;
glm::vec2 localCenterB = pc->localCenterB;
real32 mB = pc->invMassB;
real32 iB = pc->invIB;
s32 pointCount = pc->pointCount;
glm::vec2 cA = m_positions[indexA].c;
real32 aA = m_positions[indexA].a;
glm::vec2 cB = m_positions[indexB].c;
real32 aB = m_positions[indexB].a;
// Solve normal constraints
for (s32 j = 0; j < pointCount; ++j)
{
Transform2D xfA, xfB;
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - Rotation2D::Mul(xfA.q, localCenterA);
xfB.p = cB - Rotation2D::Mul(xfB.q, localCenterB);
PositionSolverManifold psm;
psm.Initialize(pc, xfA, xfB, j);
glm::vec2 normal = psm.normal;
glm::vec2 point = psm.point;
real32 separation = psm.separation;
glm::vec2 rA = point - cA;
glm::vec2 rB = point - cB;
// Track max constraint error.
minSeparation = glm::min(minSeparation, separation);
// Prevent large corrections and allow slop.
real32 C = glm::clamp(baumgarte * (separation + linearSlop), - maxLinearCorrection, 0.0f);
// Compute the effective mass.
real32 rnA = MathUtils::Cross2(rA, normal);
real32 rnB = MathUtils::Cross2(rB, normal);
real32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
real32 impulse = K > 0.0f ? - C / K : 0.0f;
glm::vec2 P = impulse * normal;
cA -= mA * P;
aA -= iA * MathUtils::Cross2(rA, P);
cB += mB * P;
aB += iB * MathUtils::Cross2(rB, P);
}
m_positions[indexA].c = cA;
m_positions[indexA].a = aA;
m_positions[indexB].c = cB;
m_positions[indexB].a = aB;
}
// We can't expect minSpeparation >= -_linearSlop because we don't
// push the separation above -_linearSlop.
return minSeparation >= -3.0f * linearSlop;
}
