// Sequential position solver for position constraints.
public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
{
float minSeparation = 0.0f;
for (int i = 0; i < m_count; ++i)
{
b2ContactPositionConstraint pc = m_positionConstraints[i];
int indexA = pc.indexA;
int indexB = pc.indexB;
b2Vec2 localCenterA = pc.localCenterA;
b2Vec2 localCenterB = pc.localCenterB;
int pointCount = pc.pointCount;
float mA = 0.0f;
float iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc.invMassA;
iA = pc.invIA;
}
float mB = pc.invMassB;
float iB = pc.invIB;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc.invMassB;
iB = pc.invIB;
}
b2Vec2 cA = m_positions[indexA].c;
float aA = m_positions[indexA].a;
b2Vec2 cB = m_positions[indexB].c;
float aB = m_positions[indexB].a;
// Solve normal constraints
for (int j = 0; j < pointCount; ++j)
{
b2Transform xfA = b2Transform.Create(), xfB = b2Transform.Create();
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - b2Math.b2Mul(xfA.q, localCenterA);
xfB.p = cB - b2Math.b2Mul(xfB.q, localCenterB);
b2PositionSolverManifold psm = new b2PositionSolverManifold(ref pc, ref xfA, ref xfB, j);
b2Vec2 normal = psm.normal;
b2Vec2 point = psm.point;
float separation = psm.separation;
b2Vec2 rA = point - cA;
b2Vec2 rB = point - cB;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = b2Math.b2Clamp(b2Settings.b2_toiBaugarte * (separation + b2Settings.b2_linearSlop), -b2Settings.b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = b2Math.b2Cross(rA, normal);
float rnB = b2Math.b2Cross(rB, normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
b2Vec2 P = impulse * normal;
cA -= mA * P;
aA -= iA * b2Math.b2Cross(rA, P);
cB += mB * P;
aB += iB * b2Math.b2Cross(rB, P);
}
m_positions[indexA].c = cA;
m_positions[indexA].a = aA;
m_positions[indexB].c = cB;
m_positions[indexB].a = aB;
m_positionConstraints[i] = pc;
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return minSeparation >= -1.5f * b2Settings.b2_linearSlop;
}
// Sequential position solver for position constraints.
public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
{
float minSeparation = 0.0f;
for (int i = 0; i < m_count; ++i)
{
b2ContactPositionConstraint pc = m_positionConstraints[i];
int indexA = pc.indexA;
int indexB = pc.indexB;
b2Vec2 localCenterA = new b2Vec2(pc.localCenterA);
b2Vec2 localCenterB = new b2Vec2(pc.localCenterB);
int pointCount = pc.pointCount;
float mA = 0.0f;
float iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc.invMassA;
iA = pc.invIA;
}
float mB = 0.0f;
float iB = 0.0F;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc.invMassB;
iB = pc.invIB;
}
b2Vec2 cA = m_positions[indexA].c;
float aA = m_positions[indexA].a;
b2Vec2 cB = m_positions[indexB].c;
float aB = m_positions[indexB].a;
// Solve normal constraints
for (int j = 0; j < pointCount; ++j)
{
b2Transform xfA = new b2Transform();
b2Transform xfB = new b2Transform();
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - Utils.b2Mul(xfA.q, localCenterA);
xfB.p = cB - Utils.b2Mul(xfB.q, localCenterB);
b2PositionSolverManifold psm = new b2PositionSolverManifold();
psm.Initialize(pc, xfA, xfB, j);
b2Vec2 normal = new b2Vec2(psm.normal);
b2Vec2 point = new b2Vec2(psm.point);
float separation = psm.separation;
b2Vec2 rA = point - cA;
b2Vec2 rB = point - cB;
// Track max constraint error.
minSeparation = Utils.b2Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = Utils.b2Clamp(Settings.b2_toiBaugarte * (separation + Settings.b2_linearSlop), -Settings.b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = Utils.b2Cross(rA, normal);
float rnB = Utils.b2Cross(rB, normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
b2Vec2 P = impulse * normal;
cA -= mA * P;
aA -= iA * Utils.b2Cross(rA, P);
cB += mB * P;
aB += iB * Utils.b2Cross(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 >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return(minSeparation >= -1.5f * Settings.b2_linearSlop);
}
