// 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;
Vector2 localCenterA = pc.localCenterA;
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;
}
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;
}
// 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)
{
FP minSeparation = 0.0f;
for (int i = 0; i < _count; ++i)
{
ContactPositionConstraint pc = _positionConstraints[i];
int indexA = pc.indexA;
int indexB = pc.indexB;
TSVector2 localCenterA = pc.localCenterA;
TSVector2 localCenterB = pc.localCenterB;
int pointCount = pc.pointCount;
FP mA = 0.0f;
FP iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc.invMassA;
iA = pc.invIA;
}
FP mB = 0.0f;
FP iB = 0.0f;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc.invMassB;
iB = pc.invIB;
}
TSVector2 cA = _positions[indexA].c;
FP aA = _positions[indexA].a;
TSVector2 cB = _positions[indexB].c;
FP 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);
TSVector2 normal;
TSVector2 point;
FP separation;
PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation);
TSVector2 rA = point - cA;
TSVector2 rB = point - cB;
// Track max constraint error.
minSeparation = TrueSync.TSMath.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
FP C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
// Compute the effective mass.
FP rnA = MathUtils.Cross(rA, normal);
FP rnB = MathUtils.Cross(rB, normal);
FP K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
FP impulse = K > 0.0f ? -C / K : 0.0f;
TSVector2 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;
}
_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.
_positions[orderedIndexB].Lock = 0;
_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);
}
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;
Vector2 localCenterA = pc.localCenterA;
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;
}
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();
Transform xfB = new Transform();
xfA.q = Matrex.CreateRotation(aA); // Actually about twice as fast to use our own function
xfB.q = Matrex.CreateRotation(aB); // Actually about twice as fast to use our own function
xfA.p = cA - Vector2.Transform(localCenterA, xfA.q); // Common.Math.Mul(xfA.q, localCenterA);
xfB.p = cB - Vector2.Transform(localCenterB, xfB.q); // Common.Math.Mul(xfB.q, localCenterB);
PositionSolverManifold psm = new PositionSolverManifold();
psm.Initialize(pc, xfA, xfB, j);
Vector2 normal = psm.normal;
Vector2 point = psm.point;
float separation = psm.separation;
Vector2 rA = point - cA;
Vector2 rB = point - cB;
// Track max constraint error.
minSeparation = MathF.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = Math.Clamp(Settings.TOIBaumgarte * (separation + Settings.LinearSlop),
-Settings.MaxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = Vectex.Cross(rA, normal);
float rnB = Vectex.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;
Vector2 P = impulse * normal;
cA -= mA * P;
aA -= iA * Vectex.Cross(rA, P);
cB += mB * P;
aB += iB * Vectex.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);
}
