// Sequential position solver for position constraints.
public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
{
float minSeparation = 0.0f;
for (int i = 0; i < _count; ++i)
{
ContactPositionConstraint pc = _positionConstraints[i];
int indexA = pc.IndexA;
int indexB = pc.IndexB;
System.Numerics.Vector2 localCenterA = pc.LocalCenterA;
System.Numerics.Vector2 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 = 0.0f;
float iB = 0.0f;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc.InvMassB;
iB = pc.InvIB;
}
System.Numerics.Vector2 cA = _positions[indexA].C;
float aA = _positions[indexA].A;
System.Numerics.Vector2 cB = _positions[indexB].C;
float 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);
System.Numerics.Vector2 normal;
System.Numerics.Vector2 point;
float separation;
PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation);
System.Numerics.Vector2 rA = point - cA;
System.Numerics.Vector2 rB = point - cB;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop),
-Settings.MaxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = MathUtils.Cross(rA, normal);
float rnB = MathUtils.Cross(rB, normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
System.Numerics.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);
}
private bool SolvePositionConstraints(int start, int end)
{
float minSeparation = 0.0f;
for (int i = start; i < end; ++i)
{
ContactPositionConstraint pc = _positionConstraints[i];
#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
// Find lower order item.
int orderedIndexA = pc.indexA;
int orderedIndexB = pc.indexB;
if (orderedIndexB < orderedIndexA)
{
orderedIndexA = pc.indexB;
orderedIndexB = pc.indexA;
}
// Lock bodies.
for (; ;)
{
if (Interlocked.CompareExchange(ref _positions[orderedIndexA].Lock, 1, 0) == 0)
{
if (Interlocked.CompareExchange(ref _positions[orderedIndexB].Lock, 1, 0) == 0)
{
break;
}
System.Threading.Interlocked.Exchange(ref _positions[orderedIndexA].Lock, 0);
}
#if NET40 || NET45
Thread.Sleep(0);
#endif
}
#endif
int indexA = pc.indexA;
int indexB = pc.indexB;
Vector2 localCenterA = pc.localCenterA;
float mA = pc.invMassA;
float iA = pc.invIA;
Vector2 localCenterB = pc.localCenterB;
float mB = pc.invMassB;
float iB = pc.invIB;
int pointCount = pc.pointCount;
Vector2 cA = _positions[indexA].c;
float aA = _positions[indexA].a;
Vector2 cB = _positions[indexB].c;
float aB = _positions[indexB].a;
// Solve normal constraints
for (int j = 0; j < pointCount; ++j)
{
Transform xfA = new Transform(Vector2.Zero, aA);
Transform xfB = new Transform(Vector2.Zero, aB);
xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q);
xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q);
Vector2 normal;
Vector2 point;
float separation;
PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out normal, out point, out separation);
Vector2 rA = point - cA;
Vector2 rB = point - cB;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = MathUtils.Cross(ref rA, ref normal);
float rnB = MathUtils.Cross(ref rB, ref normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
Vector2 P = impulse * normal;
cA -= mA * P;
aA -= iA * MathUtils.Cross(ref rA, ref P);
cB += mB * P;
aB += iB * MathUtils.Cross(ref rB, ref P);
}
_positions[indexA].c = cA;
_positions[indexA].a = aA;
_positions[indexB].c = cB;
_positions[indexB].a = aB;
#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
// Unlock bodies.
System.Threading.Interlocked.Exchange(ref _positions[orderedIndexB].Lock, 0);
System.Threading.Interlocked.Exchange(ref _positions[orderedIndexA].Lock, 0);
#endif
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return(minSeparation >= -3.0f * Settings.LinearSlop);
}
