/// <summary>
/// Describes whether this instance solve position constraints
/// </summary>
/// <param name="baumgarte">The baumgarte</param>
/// <returns>The bool</returns>
public bool SolvePositionConstraints(float baumgarte)
{
float minSeparation = 0.0f;
for (int i = 0; i < ConstraintCount; ++i)
{
ContactConstraint c = Constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float invMassA = bodyA.Mass * bodyA.InvMass;
float invIa = bodyA.Mass * bodyA.InvI;
float invMassB = bodyB.Mass * bodyB.InvMass;
float invIb = bodyB.Mass * bodyB.InvI;
SPositionSolverManifold.Initialize(c);
Vec2 normal = SPositionSolverManifold.Normal;
// Solver normal constraints
for (int j = 0; j < c.PointCount; ++j)
{
Vec2 point = SPositionSolverManifold.Points[j];
float separation = SPositionSolverManifold.Separations[j];
Vec2 rA = point - bodyA.Sweep.C;
Vec2 rB = point - bodyB.Sweep.C;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float clamp = baumgarte * Math.Clamp(separation + Settings.LinearSlop,
-Settings.MaxLinearCorrection,
0.0f);
// Compute normal impulse
float impulse = -c.Points[j].EqualizedMass * clamp;
Vec2 p = impulse * normal;
bodyA.Sweep.C -= invMassA * p;
bodyA.Sweep.A -= invIa * Vec2.Cross(rA, p);
bodyA.SynchronizeTransform();
bodyB.Sweep.C += invMassB * p;
bodyB.Sweep.A += invIb * Vec2.Cross(rB, p);
bodyB.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -Settings.LinearSlop because we don't
// push the separation above -Settings.LinearSlop.
return(minSeparation >= -1.5f * Settings.LinearSlop);
}
/// <summary>
/// Solves the step
/// </summary>
/// <param name="step">The step</param>
/// <param name="gravity">The gravity</param>
/// <param name="allowSleep">The allow sleep</param>
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.IsStatic())
{
continue;
}
// Integrate velocities.
b.LinearVelocity += step.Dt * (gravity + b.InvMass * b.Force);
b.AngularVelocity += step.Dt * b.InvI * b.Torque;
// Reset forces.
b.Force.Set(0.0f, 0.0f);
b.Torque = 0.0f;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b.LinearVelocity *= Math.Clamp(1.0f - step.Dt * b.LinearDamping, 0.0f, 1.0f);
b.AngularVelocity *= Math.Clamp(1.0f - step.Dt * b.AngularDamping, 0.0f, 1.0f);
}
ContactSolver contactSolver = new ContactSolver(step, Contacts, ContactCount);
// Initialize velocity constraints.
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < JointCount; ++i)
{
Joints[i].InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int i = 0; i < step.VelocityIterations; ++i)
{
for (int j = 0; j < JointCount; ++j)
{
Joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.FinalizeVelocityConstraints();
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.IsStatic())
{
continue;
}
// Check for large velocities.
Vec2 translation = step.Dt * b.LinearVelocity;
if (Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
translation.Normalize();
b.LinearVelocity = Settings.MaxTranslation * step.InvDt * translation;
}
float rotation = step.Dt * b.AngularVelocity;
if (rotation * rotation > Settings.MaxRotationSquared)
{
if (rotation < 0.0)
{
b.AngularVelocity = -step.InvDt * Settings.MaxRotation;
}
else
{
b.AngularVelocity = step.InvDt * Settings.MaxRotation;
}
}
// Store positions for continuous collision.
b.Sweep.C0 = b.Sweep.C;
b.Sweep.A0 = b.Sweep.A;
// Integrate
b.Sweep.C += step.Dt * b.LinearVelocity;
b.Sweep.A += step.Dt * b.AngularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < step.PositionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < JointCount; ++j)
{
bool jointOkay = Joints[j].SolvePositionConstraints(Settings.ContactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver.Constraints);
if (allowSleep)
{
float minSleepTime = Settings.FltMax;
#if !TARGET_FLOAT32_IS_FIXED
float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.InvMass == 0.0f)
{
continue;
}
if ((b.Flags & BodyFlags.AllowSleep) == 0)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b.Flags & BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
b.AngularVelocity *b.AngularVelocity > angTolSqr ||
Vec2.Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr)
#endif
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += step.Dt;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
/// <summary>
/// Solves the velocity constraints
/// </summary>
public void SolveVelocityConstraints()
{
for (int i = 0; i < ConstraintCount; ++i)
{
ContactConstraint c = Constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float wA = bodyA.AngularVelocity;
float wB = bodyB.AngularVelocity;
Vec2 vA = bodyA.LinearVelocity;
Vec2 vB = bodyB.LinearVelocity;
float invMassA = bodyA.InvMass;
float invIa = bodyA.InvI;
float invMassB = bodyB.InvMass;
float invIb = bodyB.InvI;
Vec2 normal = c.Normal;
Vec2 tangent = Vec2.Cross(normal, 1.0f);
float friction = c.Friction;
Box2DxDebug.Assert(c.PointCount == 1 || c.PointCount == 2);
unsafe
{
fixed(ContactConstraintPoint *pointsPtr = c.Points)
{
// Solve tangent constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint *ccp = &pointsPtr[j];
// Relative velocity at contact
Vec2 dv = vB + Vec2.Cross(wB, ccp->Rb) - vA - Vec2.Cross(wA, ccp->Ra);
// Compute tangent force
float vt = Vec2.Dot(dv, tangent);
float lambda = ccp->TangentMass * -vt;
// b2Clamp the accumulated force
float maxFriction = friction * ccp->NormalImpulse;
float newImpulse = Math.Clamp(ccp->TangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - ccp->TangentImpulse;
// Apply contact impulse
Vec2 p = lambda * tangent;
vA -= invMassA * p;
wA -= invIa * Vec2.Cross(ccp->Ra, p);
vB += invMassB * p;
wB += invIb * Vec2.Cross(ccp->Rb, p);
ccp->TangentImpulse = newImpulse;
}
// Solve normal constraints
if (c.PointCount == 1)
{
ContactConstraintPoint ccp = c.Points[0];
// Relative velocity at contact
Vec2 dv = vB + Vec2.Cross(wB, ccp.Rb) - vA - Vec2.Cross(wA, ccp.Ra);
// Compute normal impulse
float vn = Vec2.Dot(dv, normal);
float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias);
// Clamp the accumulated impulse
float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f);
lambda = newImpulse - ccp.NormalImpulse;
// Apply contact impulse
Vec2 p = lambda * normal;
vA -= invMassA * p;
wA -= invIa * Vec2.Cross(ccp.Ra, p);
vB += invMassB * p;
wB += invIb * Vec2.Cross(ccp.Rb, p);
ccp.NormalImpulse = newImpulse;
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn_0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = x' - a
//
// Plug into above equation:
//
// vn = A * x + b
// = A * (x' - a) + b
// = A * x' + b - A * a
// = A * x' + b'
// b' = b - A * a;
ContactConstraintPoint *cp1 = &pointsPtr[0];
ContactConstraintPoint *cp2 = &pointsPtr[1];
Vec2 a = new Vec2(cp1->NormalImpulse, cp2->NormalImpulse);
Box2DxDebug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
// Relative velocity at contact
Vec2 dv1 = vB + Vec2.Cross(wB, cp1->Rb) - vA - Vec2.Cross(wA, cp1->Ra);
Vec2 dv2 = vB + Vec2.Cross(wB, cp2->Rb) - vA - Vec2.Cross(wA, cp2->Ra);
// Compute normal velocity
float vn1 = Vec2.Dot(dv1, normal);
float vn2 = Vec2.Dot(dv2, normal);
Vec2 b;
b.X = vn1 - cp1->VelocityBias;
b.Y = vn2 - cp2->VelocityBias;
b -= Math.Mul(c.K, a);
//const float k_errorTol = 1e-3f;
//B2_NOT_USED(k_errorTol);
for (;;)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
Vec2 x = -Math.Mul(c.NormalMass, b);
if (x.X >= 0.0f && x.Y >= 0.0f)
{
// Resubstitute for the incremental impulse
Vec2 d = x - a;
// Apply incremental impulse
Vec2 p1 = d.X * normal;
Vec2 p2 = d.Y * normal;
vA -= invMassA * (p1 + p2);
wA -= invIa * (Vec2.Cross(cp1->Ra, p1) + Vec2.Cross(cp2->Ra, p2));
vB += invMassB * (p1 + p2);
wB += invIb * (Vec2.Cross(cp1->Rb, p1) + Vec2.Cross(cp2->Rb, p2));
// Accumulate
cp1->NormalImpulse = x.X;
cp2->NormalImpulse = x.Y;
#if DEBUG_SOLVER
// Postconditions
dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA);
dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA);
// Compute normal velocity
vn1 = Vec2.Dot(dv1, normal);
vn2 = Vec2.Dot(dv2, normal);
Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1' + a12 * 0 + b1'
// vn2 = a21 * x1' + a22 * 0 + b2'
//
x.X = -cp1->NormalMass * b.X;
x.Y = 0.0f;
/*
* vn1 = 0.0f;
*/
vn2 = c.K.Col1.Y * x.X + b.Y;
if (x.X >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
Vec2 d = x - a;
// Apply incremental impulse
Vec2 p1 = d.X * normal;
Vec2 p2 = d.Y * normal;
vA -= invMassA * (p1 + p2);
wA -= invIa * (Vec2.Cross(cp1->Ra, p1) + Vec2.Cross(cp2->Ra, p2));
vB += invMassB * (p1 + p2);
wB += invIb * (Vec2.Cross(cp1->Rb, p1) + Vec2.Cross(cp2->Rb, p2));
// Accumulate
cp1->NormalImpulse = x.X;
cp2->NormalImpulse = x.Y;
#if DEBUG_SOLVER
// Postconditions
dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA);
// Compute normal velocity
vn1 = Vec2.Dot(dv1, normal);
Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: w2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2' + b1'
// 0 = a21 * 0 + a22 * x2' + b2'
//
x.X = 0.0f;
x.Y = -cp2->NormalMass * b.Y;
vn1 = c.K.Col2.X * x.Y + b.X;
/*
* vn2 = 0.0f;
*/
if (x.Y >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
Vec2 d = x - a;
// Apply incremental impulse
Vec2 p1 = d.X * normal;
Vec2 p2 = d.Y * normal;
vA -= invMassA * (p1 + p2);
wA -= invIa * (Vec2.Cross(cp1->Ra, p1) + Vec2.Cross(cp2->Ra, p2));
vB += invMassB * (p1 + p2);
wB += invIb * (Vec2.Cross(cp1->Rb, p1) + Vec2.Cross(cp2->Rb, p2));
// Accumulate
cp1->NormalImpulse = x.X;
cp2->NormalImpulse = x.Y;
#if DEBUG_SOLVER
// Postconditions
dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA);
// Compute normal velocity
vn2 = Vec2.Dot(dv2, normal);
Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
x.X = 0.0f;
x.Y = 0.0f;
vn1 = b.X;
vn2 = b.Y;
if (vn1 >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
Vec2 d = x - a;
// Apply incremental impulse
Vec2 p1 = d.X * normal;
Vec2 p2 = d.Y * normal;
vA -= invMassA * (p1 + p2);
wA -= invIa * (Vec2.Cross(cp1->Ra, p1) + Vec2.Cross(cp2->Ra, p2));
vB += invMassB * (p1 + p2);
wB += invIb * (Vec2.Cross(cp1->Rb, p1) + Vec2.Cross(cp2->Rb, p2));
// Accumulate
cp1->NormalImpulse = x.X;
cp2->NormalImpulse = x.Y;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
bodyA.LinearVelocity = vA;
bodyA.AngularVelocity = wA;
bodyB.LinearVelocity = vB;
bodyB.AngularVelocity = wB;
}
}
}
}