// Sequential position solver for position constraints.
public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
{
GGame.Math.Fix64 minSeparation = 0.0f;
for (int i = 0; i < _count; ++i)
{
ContactPositionConstraint pc = _positionConstraints[i];
int indexA = pc.IndexA;
int indexB = pc.IndexB;
Vector2 localCenterA = pc.LocalCenterA;
Vector2 localCenterB = pc.LocalCenterB;
int pointCount = pc.PointCount;
GGame.Math.Fix64 mA = 0.0f;
GGame.Math.Fix64 iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc.InvMassA;
iA = pc.InvIA;
}
GGame.Math.Fix64 mB = 0.0f;
GGame.Math.Fix64 iB = 0.0f;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc.InvMassB;
iB = pc.InvIB;
}
Vector2 cA = _positions[indexA].C;
GGame.Math.Fix64 aA = _positions[indexA].A;
Vector2 cB = _positions[indexB].C;
GGame.Math.Fix64 aB = _positions[indexB].A;
// Solve normal constraints
for (int j = 0; j < pointCount; ++j)
{
Transform xfA = new Transform();
Transform xfB = new Transform();
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
PositionSolverManifold.Initialize(pc, xfA, xfB, j, out Vector2 normal, out Vector2 point, out GGame.Math.Fix64 separation);
Vector2 rA = point - cA;
Vector2 rB = point - cB;
// Track max constraint error.
minSeparation = Math.Min((float)minSeparation, (float)separation);
// Prevent large corrections and allow slop.
GGame.Math.Fix64 C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
// Compute the effective mass.
GGame.Math.Fix64 rnA = MathUtils.Cross(rA, normal);
GGame.Math.Fix64 rnB = MathUtils.Cross(rB, normal);
GGame.Math.Fix64 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
GGame.Math.Fix64 impulse = K > 0.0f ? -C / K : 0.0f;
Vector2 P = impulse * normal;
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
_positions[indexA].C = cA;
_positions[indexA].A = aA;
_positions[indexB].C = cB;
_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.LinearSlop);
}
// Sequential position solver for position constraints.
public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
{
var minSeparation = 0.0f;
for (var i = 0; i < _count; ++i)
{
var pc = _positionConstraints[i];
var indexA = pc.IndexA;
var indexB = pc.IndexB;
var localCenterA = pc.LocalCenterA;
var localCenterB = pc.LocalCenterB;
var pointCount = pc.PointCount;
var mA = 0.0f;
var iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc.InvMassA;
iA = pc.InvIA;
}
var mB = 0.0f;
var iB = 0.0f;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc.InvMassB;
iB = pc.InvIB;
}
var cA = _positions[indexA].C;
var aA = _positions[indexA].A;
var cB = _positions[indexB].C;
var aB = _positions[indexB].A;
// Solve normal constraints
for (var j = 0; j < pointCount; ++j)
{
var xfA = new Transform();
var xfB = new Transform();
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
PositionSolverManifold.Initialize(pc, xfA, xfB, j, out var normal, out var point,
out var separation);
var rA = point - cA;
var rB = point - cB;
// Track max constraint error.
minSeparation = Mathf.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
var C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop),
-Settings.MaxLinearCorrection, 0.0f);
// Compute the effective mass.
var rnA = MathUtils.Cross(rA, normal);
var rnB = MathUtils.Cross(rB, normal);
var K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
var impulse = K > 0.0f ? -C / K : 0.0f;
var P = impulse * normal;
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
_positions[indexA].C = cA;
_positions[indexA].A = aA;
_positions[indexB].C = cB;
_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.LinearSlop);
}
