public void InitVelocityConstraints(TimeStep step)
{
unsafe
{
// Warm start.
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
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);
fixed(ContactConstraintPoint *pointsPtr = c.Points)
{
if (step.WarmStarting)
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint *ccp = &pointsPtr[j];
ccp->NormalImpulse *= step.DtRatio;
ccp->TangentImpulse *= step.DtRatio;
Vec2 P = ccp->NormalImpulse * normal + ccp->TangentImpulse * tangent;
bodyA._angularVelocity -= invIA * Vec2.Cross(ccp->RA, P);
bodyA._linearVelocity -= invMassA * P;
bodyB._angularVelocity += invIB * Vec2.Cross(ccp->RB, P);
bodyB._linearVelocity += invMassB * P;
}
}
else
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint *ccp = &pointsPtr[j];
ccp->NormalImpulse = 0.0f;
ccp->TangentImpulse = 0.0f;
}
}
}
}
}
}
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;
Vector2 vA = bodyA._linearVelocity;
Vector2 vB = bodyB._linearVelocity;
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
Vector2 normal = c.Normal;
Vector2 tangent = normal.CrossScalarPostMultiply(1.0f);
float friction = c.Friction;
#if ALLOWUNSAFE
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
Vector2 dv = vB + ccp->RB.CrossScalarPreMultiply(wB) - vA - ccp->RA.CrossScalarPreMultiply(wA);
// Compute tangent force
float vt = Vector2.Dot(dv, tangent);
float lambda = ccp->TangentMass * (-vt);
// b2Clamp the accumulated force
float maxFriction = friction * ccp->NormalImpulse;
float newImpulse = Mathf.Clamp(ccp->TangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - ccp->TangentImpulse;
// Apply contact impulse
Vector2 P = lambda * tangent;
vA -= invMassA * P;
wA -= invIA * ccp->RA.Cross(P);
vB += invMassB * P;
wB += invIB * ccp->RB.Cross(P);
ccp->TangentImpulse = newImpulse;
}
// Solve normal constraints
if (c.PointCount == 1)
{
ContactConstraintPoint ccp = c.Points[0];
// Relative velocity at contact
Vector2 dv = vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA);
// Compute normal impulse
float vn = Vector2.Dot(dv, normal);
float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias);
// Clamp the accumulated impulse
float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f);
lambda = newImpulse - ccp.NormalImpulse;
// Apply contact impulse
Vector2 P = lambda * normal;
vA -= invMassA * P;
wA -= invIA * ccp.RA.Cross(P);
vB += invMassB * P;
wB += invIB * ccp.RB.Cross(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];
Vector2 a = new Vector2(cp1->NormalImpulse, cp2->NormalImpulse);
// Relative velocity at contact
Vector2 dv1 = vB + cp1->RB.CrossScalarPreMultiply(wB) - vA - cp1->RA.CrossScalarPreMultiply(wA);
Vector2 dv2 = vB + cp2->RB.CrossScalarPreMultiply(wB) - vA - cp2->RA.CrossScalarPreMultiply(wA);
// Compute normal velocity
float vn1 = Vector2.Dot(dv1, normal);
float vn2 = Vector2.Dot(dv2, normal);
Vector2 b = new Vector2(vn1 - cp1->VelocityBias, vn2 - cp2->VelocityBias);
b -= c.K.Multiply(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'
//
Vector2 x = -c.NormalMass.Multiply(b);
if (x.x >= 0.0f && x.y >= 0.0f)
{
// Resubstitute for the incremental impulse
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.x * normal;
Vector2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(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);
#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
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.x * normal;
Vector2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(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);
#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
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.x * normal;
Vector2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(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);
#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
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.x * normal;
Vector2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(P2));
// Accumulate
cp1->NormalImpulse = x.x;
cp2->NormalImpulse = x.y;
break;
}
// 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;
}
}
#else
ContactConstraintPoint[] pointsPtr = c.Points;
// Solve tangent constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = pointsPtr[j];
// Relative velocity at contact
Vector2 dv = vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA);
// Compute tangent force
float vt = Vector2.Dot(dv, tangent);
float lambda = ccp.TangentMass * (-vt);
// b2Clamp the accumulated force
float maxFriction = friction * ccp.NormalImpulse;
float newImpulse = Box2DNet.Common.Math.Clamp(ccp.TangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - ccp.TangentImpulse;
// Apply contact impulse
Vector2 P = lambda * tangent;
vA -= invMassA * P;
wA -= invIA * ccp.RA.Cross(P);
vB += invMassB * P;
wB += invIB * ccp.RB.Cross(P);
ccp.TangentImpulse = newImpulse;
}
// Solve normal constraints
if (c.PointCount == 1)
{
ContactConstraintPoint ccp = c.Points[0];
// Relative velocity at contact
Vector2 dv = vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA);
// Compute normal impulse
float vn = Vector2.Dot(dv, normal);
float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias);
// Clamp the accumulated impulse
float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f);
lambda = newImpulse - ccp.NormalImpulse;
// Apply contact impulse
Vector2 P = lambda * normal;
vA -= invMassA * P;
wA -= invIA * ccp.RA.Cross(P);
vB += invMassB * P;
wB += invIB * ccp.RB.Cross(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];
Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse);
// Relative velocity at contact
Vector2 dv1 = vB + cp1.RB.CrossScalarPreMultiply(wB) - vA - cp1.RA.CrossScalarPreMultiply(wA);
Vector2 dv2 = vB + cp2.RB.CrossScalarPreMultiply(wB) - vA - cp2.RA.CrossScalarPreMultiply(wA);
// Compute normal velocity
float vn1 = Vector2.Dot(dv1, normal);
float vn2 = Vector2.Dot(dv2, normal);
Vector2 b = new Vector2(vn1 - cp1.VelocityBias, vn2 - cp2.VelocityBias);
b -= c.K.Multiply(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'
//
Vector2 x = -c.NormalMass.Multiply(b);
if (x.X >= 0.0f && x.Y >= 0.0f)
{
// Resubstitute for the incremental impulse
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(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);
#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
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(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);
#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
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(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);
#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
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(P2));
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
break;
}
// 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;
#endif // ALLOWUNSAFE
}
}
public void InitVelocityConstraints(TimeStep step)
{
#if ALLOWUNSAFE
unsafe
{
// Warm start.
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
Vector2 normal = c.Normal;
Vector2 tangent = normal.CrossScalarPostMultiply(1.0f);
fixed(ContactConstraintPoint *pointsPtr = c.Points)
{
if (step.WarmStarting)
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint *ccp = &pointsPtr[j];
ccp->NormalImpulse *= step.DtRatio;
ccp->TangentImpulse *= step.DtRatio;
Vector2 P = ccp->NormalImpulse * normal + ccp->TangentImpulse * tangent;
bodyA._angularVelocity -= invIA * ccp->RA.Cross(P);
bodyA._linearVelocity -= invMassA * P;
bodyB._angularVelocity += invIB * ccp->RB.Cross(P);
bodyB._linearVelocity += invMassB * P;
}
}
else
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint *ccp = &pointsPtr[j];
ccp->NormalImpulse = 0.0f;
ccp->TangentImpulse = 0.0f;
}
}
}
}
}
#else
// Warm start.
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
Vector2 normal = c.Normal;
Vector2 tangent = normal.CrossScalarPostMultiply(1.0f);
ContactConstraintPoint[] points = c.Points;
if (step.WarmStarting)
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = points[j];
ccp.NormalImpulse *= step.DtRatio;
ccp.TangentImpulse *= step.DtRatio;
Vector2 P = ccp.NormalImpulse * normal + ccp.TangentImpulse * tangent;
bodyA._angularVelocity -= invIA * ccp.RA.Cross(P);
bodyA._linearVelocity -= invMassA * P;
bodyB._angularVelocity += invIB * ccp.RB.Cross(P);
bodyB._linearVelocity += invMassB * P;
}
}
else
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = points[j];
ccp.NormalImpulse = 0.0f;
ccp.TangentImpulse = 0.0f;
}
}
}
#endif
}
internal unsafe void ContactSolverSetup(Manifold manifold, WorldManifold worldManifold, ContactConstraint cc)
{
// this is kind of yucky but we do know these were setup before entry to this method
var bodyA = cc.BodyA;
var bodyB = cc.BodyB;
Vector2 vA = bodyA._linearVelocity;
Vector2 vB = bodyB._linearVelocity;
float wA = bodyA._angularVelocity;
float wB = bodyB._angularVelocity;
fixed(ContactConstraintPoint *ccPointsPtr = cc.Points)
{
for (int j = 0; j < cc.PointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
ContactConstraintPoint *ccp = &ccPointsPtr[j];
ccp->NormalImpulse = cp.NormalImpulse;
ccp->TangentImpulse = cp.TangentImpulse;
ccp->LocalPoint = cp.LocalPoint;
ccp->RA = worldManifold.Points[j] - bodyA._sweep.C;
ccp->RB = worldManifold.Points[j] - bodyB._sweep.C;
float rnA = ccp->RA.Cross(cc.Normal);
float rnB = ccp->RB.Cross(cc.Normal);
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;
ccp->NormalMass = 1.0f / kNormal;
float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;
ccp->EqualizedMass = 1.0f / kEqualized;
Vector2 tangent = cc.Normal.CrossScalarPostMultiply(1.0f);
float rtA = ccp->RA.Cross(tangent);
float rtB = ccp->RB.Cross(tangent);
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;
ccp->TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp->VelocityBias = 0.0f;
float vRel = Vector2.Dot(cc.Normal, vB + ccp->RB.CrossScalarPreMultiply(wB) - vA - ccp->RA.CrossScalarPreMultiply(wA));
if (vRel < -Common.Settings.VelocityThreshold)
{
ccp->VelocityBias = -cc.Restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint *ccp1 = &ccPointsPtr[0];
ContactConstraintPoint *ccp2 = &ccPointsPtr[1];
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
float rn1A = ccp1->RA.Cross(cc.Normal);
float rn1B = ccp1->RB.Cross(cc.Normal);
float rn2A = ccp2->RA.Cross(cc.Normal);
float rn2B = ccp2->RB.Cross(cc.Normal);
float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K.Col1 = new Vector2(k11, k12);
cc.K.Col2 = new Vector2(k12, k22);
cc.NormalMass = cc.K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
}
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
_step = step;
_constraintCount = contactCount;
_constraints = new ContactConstraint[_constraintCount];
for (int i = 0; i < _constraintCount; i++)
{
_constraints[i] = new ContactConstraint();
}
int count = 0;
for (int i = 0; i < _constraintCount; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
Shape shapeA = fixtureA.Shape;
Shape shapeB = fixtureB.Shape;
float radiusA = shapeA._radius;
float radiusB = shapeB._radius;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
Manifold manifold = contact.Manifold;
float friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction);
float restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution);
Vec2 vA = bodyA._linearVelocity;
Vec2 vB = bodyB._linearVelocity;
float wA = bodyA._angularVelocity;
float wB = bodyB._angularVelocity;
Box2DXDebug.Assert(manifold.PointCount > 0);
WorldManifold worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, bodyA._xf, radiusA, bodyB._xf, radiusB);
ContactConstraint cc = _constraints[i];
cc.BodyA = bodyA;
cc.BodyB = bodyB;
cc.Manifold = manifold;
cc.Normal = worldManifold.Normal;
cc.PointCount = manifold.PointCount;
cc.Friction = friction;
cc.Restitution = restitution;
cc.LocalPlaneNormal = manifold.LocalPlaneNormal;
cc.LocalPoint = manifold.LocalPoint;
cc.Radius = radiusA + radiusB;
cc.Type = manifold.Type;
unsafe
{
fixed(ContactConstraintPoint *ccPointsPtr = cc.Points)
{
for (int j = 0; j < cc.PointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
ContactConstraintPoint *ccp = &ccPointsPtr[j];
ccp->NormalImpulse = cp.NormalImpulse;
ccp->TangentImpulse = cp.TangentImpulse;
ccp->LocalPoint = cp.LocalPoint;
ccp->RA = worldManifold.Points[j] - bodyA._sweep.C;
ccp->RB = worldManifold.Points[j] - bodyB._sweep.C;
float rnA = Vec2.Cross(ccp->RA, cc.Normal);
float rnB = Vec2.Cross(ccp->RB, cc.Normal);
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;
Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
ccp->NormalMass = 1.0f / kNormal;
float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;
Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
ccp->EqualizedMass = 1.0f / kEqualized;
Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f);
float rtA = Vec2.Cross(ccp->RA, tangent);
float rtB = Vec2.Cross(ccp->RB, tangent);
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;
Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
ccp->TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp->VelocityBias = 0.0f;
float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp->RB) - vA - Vec2.Cross(wA, ccp->RA));
if (vRel < -Common.Settings.VelocityThreshold)
{
ccp->VelocityBias = -cc.Restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint *ccp1 = &ccPointsPtr[0];
ContactConstraintPoint *ccp2 = &ccPointsPtr[1];
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
float rn1A = Vec2.Cross(ccp1->RA, cc.Normal);
float rn1B = Vec2.Cross(ccp1->RB, cc.Normal);
float rn2A = Vec2.Cross(ccp2->RA, cc.Normal);
float rn2B = Vec2.Cross(ccp2->RB, cc.Normal);
float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K.Col1.Set(k11, k12);
cc.K.Col2.Set(k12, k22);
cc.NormalMass = cc.K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
}
}
}
}
