public ContactConstraint()
{
for (int i = 0; i < Settings.MaxManifoldPoints; i++)
{
Points[i] = new ContactConstraintPoint();
}
}
public void Report(ContactConstraint[] constraints)
{
if (this._listener != null)
{
for (int i = 0; i < this._contactCount; i++)
{
Contact contact = this._contacts[i];
ContactConstraint contactConstraint = constraints[i];
ContactResult contactResult = new ContactResult();
contactResult.Shape1 = contact.GetShape1();
contactResult.Shape2 = contact.GetShape2();
Body body = contactResult.Shape1.GetBody();
int manifoldCount = contact.GetManifoldCount();
Manifold[] manifolds = contact.GetManifolds();
for (int j = 0; j < manifoldCount; j++)
{
Manifold manifold = manifolds[j];
contactResult.Normal = manifold.Normal;
for (int k = 0; k < manifold.PointCount; k++)
{
ManifoldPoint manifoldPoint = manifold.Points[k];
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[k];
contactResult.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1);
contactResult.NormalImpulse = contactConstraintPoint.NormalImpulse;
contactResult.TangentImpulse = contactConstraintPoint.TangentImpulse;
contactResult.ID = manifoldPoint.ID;
this._listener.Result(contactResult);
}
}
}
}
}
public bool SolvePositionConstraints(float baumgarte)
{
float minSeparation = 0.0f;
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body b1 = c.Body1;
Body b2 = c.Body2;
float invMass1 = b1._mass * b1._invMass;
float invI1 = b1._mass * b1._invI;
float invMass2 = b2._mass * b2._invMass;
float invI2 = b2._mass * b2._invI;
Vec2 normal = c.Normal;
// Solver normal constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, ccp.LocalAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, ccp.LocalAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 dp = p2 - p1;
// Approximate the current separation.
float separation = Vec2.Dot(dp, normal) + ccp.Separation;
// Track max constraint error.
minSeparation = Common.Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = baumgarte * Common.Math.Clamp(separation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
// Compute normal impulse
float impulse = -ccp.EqualizedMass * C;
Vec2 P = impulse * normal;
b1._sweep.C -= invMass1 * P;
b1._sweep.A -= invI1 * Vec2.Cross(r1, P);
b1.SynchronizeTransform();
b2._sweep.C += invMass2 * P;
b2._sweep.A += invI2 * Vec2.Cross(r2, P);
b2.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -Settings.LinearSlop because we don't
// push the separation above -Settings.LinearSlop.
return(minSeparation >= -1.5f * Settings.LinearSlop);
}
// Sequential solver.
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;
PositionSolverManifold psm = new PositionSolverManifold();
psm.Initialize(c);
Vec2 normal = psm.Normal;
// Solve normal constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
Vec2 point = psm.Points[j];
float separation = psm.Separations[j];
Vec2 rA = point - bodyA._sweep.C;
Vec2 rB = point - bodyB._sweep.C;
// Track max constraint error.
minSeparation = Common.Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = Common.Math.Clamp(baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
// Compute normal impulse
float impulse = -ccp.EqualizedMass * C;
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 >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return(minSeparation >= -1.5f * Settings.LinearSlop);
}
public void InitVelocityConstraints(TimeStep step)
{
// Warm start.
for (int i = 0; i < ConstraintCount; ++i)
{
ContactConstraint c = Constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float invMassA = c.BodyA._invMass;
float invIA = c.BodyA._invI;
float invMassB = c.BodyB._invMass;
float invIB = c.BodyB._invI;
Vec2 normal = c.Normal;
Vec2 tangent = Vec2.Cross(normal, 1.0f);
if (step.WarmStarting)
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[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 = c.Points[j];
ccp.NormalImpulse = 0.0f;
ccp.TangentImpulse = 0.0f;
}
}
}
}
public void InitVelocityConstraints(TimeStep step)
{
// Warm start.
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body b1 = c.Body1;
Body b2 = c.Body2;
float invMass1 = b1._invMass;
float invI1 = b1._invI;
float invMass2 = b2._invMass;
float invI2 = b2._invI;
Vec2 normal = c.Normal;
Vec2 tangent = Vec2.Cross(normal, 1.0f);
if (step.WarmStarting)
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
ccp.NormalImpulse *= step.DtRatio;
ccp.TangentImpulse *= step.DtRatio;
Vec2 P = ccp.NormalImpulse * normal + ccp.TangentImpulse * tangent;
b1._angularVelocity -= invI1 * Vec2.Cross(ccp.R1, P);
b1._linearVelocity -= invMass1 * P;
b2._angularVelocity += invI2 * Vec2.Cross(ccp.R2, P);
b2._linearVelocity += invMass2 * P;
}
}
else
{
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
ccp.NormalImpulse = 0.0f;
ccp.TangentImpulse = 0.0f;
}
}
}
}
public bool SolvePositionConstraints(float baumgarte)
{
float num = 0f;
for (int i = 0; i < this._constraintCount; i++)
{
ContactConstraint contactConstraint = this._constraints[i];
Body body = contactConstraint.Body1;
Body body2 = contactConstraint.Body2;
float a = body._mass * body._invMass;
float num2 = body._mass * body._invI;
float a2 = body2._mass * body2._invMass;
float num3 = body2._mass * body2._invI;
Vec2 normal = contactConstraint.Normal;
for (int j = 0; j < contactConstraint.PointCount; j++)
{
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[j];
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, contactConstraintPoint.LocalAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, contactConstraintPoint.LocalAnchor2 - body2.GetLocalCenter());
Vec2 v = body._sweep.C + vec;
Vec2 v2 = body2._sweep.C + vec2;
Vec2 a3 = v2 - v;
float num4 = Vec2.Dot(a3, normal) + contactConstraintPoint.Separation;
num = Box2DX.Common.Math.Min(num, num4);
float num5 = baumgarte * Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
float a4 = -contactConstraintPoint.EqualizedMass * num5;
Vec2 vec3 = a4 * normal;
Body expr_157_cp_0 = body;
expr_157_cp_0._sweep.C = expr_157_cp_0._sweep.C - a * vec3;
Body expr_176_cp_0 = body;
expr_176_cp_0._sweep.A = expr_176_cp_0._sweep.A - num2 * Vec2.Cross(vec, vec3);
body.SynchronizeTransform();
Body expr_19C_cp_0 = body2;
expr_19C_cp_0._sweep.C = expr_19C_cp_0._sweep.C + a2 * vec3;
Body expr_1BC_cp_0 = body2;
expr_1BC_cp_0._sweep.A = expr_1BC_cp_0._sweep.A + num3 * Vec2.Cross(vec2, vec3);
body2.SynchronizeTransform();
}
}
return(num >= -1.5f * Settings.LinearSlop);
}
public void InitVelocityConstraints(TimeStep step)
{
for (int i = 0; i < this._constraintCount; i++)
{
ContactConstraint contactConstraint = this._constraints[i];
Body body = contactConstraint.Body1;
Body body2 = contactConstraint.Body2;
float invMass = body._invMass;
float invI = body._invI;
float invMass2 = body2._invMass;
float invI2 = body2._invI;
Vec2 normal = contactConstraint.Normal;
Vec2 v = Vec2.Cross(normal, 1f);
if (step.WarmStarting)
{
for (int j = 0; j < contactConstraint.PointCount; j++)
{
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[j];
contactConstraintPoint.NormalImpulse *= step.DtRatio;
contactConstraintPoint.TangentImpulse *= step.DtRatio;
Vec2 vec = contactConstraintPoint.NormalImpulse * normal + contactConstraintPoint.TangentImpulse * v;
body._angularVelocity -= invI * Vec2.Cross(contactConstraintPoint.R1, vec);
Body expr_EA = body;
expr_EA._linearVelocity -= invMass * vec;
body2._angularVelocity += invI2 * Vec2.Cross(contactConstraintPoint.R2, vec);
Body expr_122 = body2;
expr_122._linearVelocity += invMass2 * vec;
}
}
else
{
for (int j = 0; j < contactConstraint.PointCount; j++)
{
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[j];
contactConstraintPoint.NormalImpulse = 0f;
contactConstraintPoint.TangentImpulse = 0f;
}
}
}
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
_step = step;
_constraintCount = 0;
for (int i = 0; i < contactCount; ++i)
{
Box2DXDebug.Assert(contacts[i].IsSolid());
_constraintCount += contacts[i].GetManifoldCount();
}
_constraints = new ContactConstraint[_constraintCount];
for (int i = 0; i < _constraintCount; i++)
{
_constraints[i] = new ContactConstraint();
}
int count = 0;
for (int i = 0; i < contactCount; ++i)
{
Contact contact = contacts[i];
Shape shape1 = contact._shape1;
Shape shape2 = contact._shape2;
Body b1 = shape1.GetBody();
Body b2 = shape2.GetBody();
int manifoldCount = contact.GetManifoldCount();
Manifold[] manifolds = contact.GetManifolds();
float friction = Settings.MixFriction(shape1.Friction, shape2.Friction);
float restitution = Settings.MixRestitution(shape1.Restitution, shape2.Restitution);
Vec2 v1 = b1._linearVelocity;
Vec2 v2 = b2._linearVelocity;
float w1 = b1._angularVelocity;
float w2 = b2._angularVelocity;
for (int j = 0; j < manifoldCount; ++j)
{
Manifold manifold = manifolds[j];
Box2DXDebug.Assert(manifold.PointCount > 0);
Vec2 normal = manifold.Normal;
Box2DXDebug.Assert(count < _constraintCount);
ContactConstraint cc = _constraints[count];
cc.Body1 = b1;
cc.Body2 = b2;
cc.Manifold = manifold;
cc.Normal = normal;
cc.PointCount = manifold.PointCount;
cc.Friction = friction;
cc.Restitution = restitution;
for (int k = 0; k < cc.PointCount; ++k)
{
ManifoldPoint cp = manifold.Points[k];
ContactConstraintPoint ccp = cc.Points[k];
ccp.NormalImpulse = cp.NormalImpulse;
ccp.TangentImpulse = cp.TangentImpulse;
ccp.Separation = cp.Separation;
ccp.LocalAnchor1 = cp.LocalPoint1;
ccp.LocalAnchor2 = cp.LocalPoint2;
ccp.R1 = Common.Math.Mul(b1.GetXForm().R, cp.LocalPoint1 - b1.GetLocalCenter());
ccp.R2 = Common.Math.Mul(b2.GetXForm().R, cp.LocalPoint2 - b2.GetLocalCenter());
float rn1 = Vec2.Cross(ccp.R1, normal);
float rn2 = Vec2.Cross(ccp.R2, normal);
rn1 *= rn1;
rn2 *= rn2;
float kNormal = b1._invMass + b2._invMass + b1._invI * rn1 + b2._invI * rn2;
Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
ccp.NormalMass = 1.0f / kNormal;
float kEqualized = b1._mass * b1._invMass + b2._mass * b2._invMass;
kEqualized += b1._mass * b1._invI * rn1 + b2._mass * b2._invI * rn2;
Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
ccp.EqualizedMass = 1.0f / kEqualized;
Vec2 tangent = Vec2.Cross(normal, 1.0f);
float rt1 = Vec2.Cross(ccp.R1, tangent);
float rt2 = Vec2.Cross(ccp.R2, tangent);
rt1 *= rt1;
rt2 *= rt2;
float kTangent = b1._invMass + b2._invMass + b1._invI * rt1 + b2._invI * rt2;
Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
if (ccp.Separation > 0.0f)
{
ccp.VelocityBias = -step.Inv_Dt * ccp.Separation; // TODO_ERIN b2TimeStep
}
else
{
float vRel = Vec2.Dot(cc.Normal, v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1));
if (vRel < -Settings.VelocityThreshold)
{
ccp.VelocityBias = -cc.Restitution * vRel;
}
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint ccp1 = cc.Points[0];
ContactConstraintPoint ccp2 = cc.Points[1];
float invMass1 = b1._invMass;
float invI1 = b1._invI;
float invMass2 = b2._invMass;
float invI2 = b2._invI;
float rn11 = Vec2.Cross(ccp1.R1, normal);
float rn12 = Vec2.Cross(ccp1.R2, normal);
float rn21 = Vec2.Cross(ccp2.R1, normal);
float rn22 = Vec2.Cross(ccp2.R2, normal);
float k11 = invMass1 + invMass2 + invI1 * rn11 * rn11 + invI2 * rn12 * rn12;
float k22 = invMass1 + invMass2 + invI1 * rn21 * rn21 + invI2 * rn22 * rn22;
float k12 = invMass1 + invMass2 + invI1 * rn11 * rn21 + invI2 * rn12 * rn22;
// 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.Invert();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
++count;
}
}
Box2DXDebug.Assert(count == _constraintCount);
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body b1 = c.Body1;
Body b2 = c.Body2;
float w1 = b1._angularVelocity;
float w2 = b2._angularVelocity;
Vec2 v1 = b1._linearVelocity;
Vec2 v2 = b2._linearVelocity;
float invMass1 = b1._invMass;
float invI1 = b1._invI;
float invMass2 = b2._invMass;
float invI2 = b2._invI;
Vec2 normal = c.Normal;
Vec2 tangent = Vec2.Cross(normal, 1.0f);
float friction = c.Friction;
Box2DXDebug.Assert(c.PointCount == 1 || c.PointCount == 2);
// Solve normal constraints
if (c.PointCount == 1)
{
ContactConstraintPoint ccp = c.Points[0];
// Relative velocity at contact
Vec2 dv = v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1);
// Compute normal impulse
float vn = Vec2.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
Vec2 P = lambda * normal;
v1 -= invMass1 * P;
w1 -= invI1 * Vec2.Cross(ccp.R1, P);
v2 += invMass2 * P;
w2 += invI2 * Vec2.Cross(ccp.R2, 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 = c.Points[0];
ContactConstraintPoint cp2 = c.Points[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 = v2 + Vec2.Cross(w2, cp1.R2) - v1 - Vec2.Cross(w1, cp1.R1);
Vec2 dv2 = v2 + Vec2.Cross(w2, cp2.R2) - v1 - Vec2.Cross(w1, cp2.R1);
// 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 -= Common.Math.Mul(c.K, a);
const float k_errorTol = 1e-3f;
for (; ;)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
Vec2 x = -Common.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;
v1 -= invMass1 * (P1 + P2);
w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));
v2 += invMass2 * (P1 + P2);
w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv1 = v2 + Vector2.Cross(w2, cp1.R2) - v1 - Vector2.Cross(w1, cp1.R1);
dv2 = v2 + Vector2.Cross(w2, cp2.R2) - v1 - Vector2.Cross(w1, cp2.R1);
// Compute normal velocity
vn1 = Vector2.Dot(dv1, normal);
vn2 = Vector2.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;
v1 -= invMass1 * (P1 + P2);
w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));
v2 += invMass2 * (P1 + P2);
w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv1 = v2 + Vector2.Cross(w2, cp1.R2) - v1 - Vector2.Cross(w1, cp1.R1);
// Compute normal velocity
vn1 = Vector2.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;
v1 -= invMass1 * (P1 + P2);
w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));
v2 += invMass2 * (P1 + P2);
w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv2 = v2 + Vector2.Cross(w2, cp2.R2) - v1 - Vector2.Cross(w1, cp2.R1);
// Compute normal velocity
vn2 = Vector2.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;
v1 -= invMass1 * (P1 + P2);
w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));
v2 += invMass2 * (P1 + P2);
w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, 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;
}
}
// Solve tangent constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
// Relative velocity at contact
Vec2 dv = v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1);
// Compute tangent force
float vt = Vec2.Dot(dv, tangent);
float lambda = ccp.TangentMass * (-vt);
// Clamp the accumulated force
float maxFriction = friction * ccp.NormalImpulse;
float newImpulse = Common.Math.Clamp(ccp.TangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - ccp.TangentImpulse;
// Apply contact impulse
Vec2 P = lambda * tangent;
v1 -= invMass1 * P;
w1 -= invI1 * Vec2.Cross(ccp.R1, P);
v2 += invMass2 * P;
w2 += invI2 * Vec2.Cross(ccp.R2, P);
ccp.TangentImpulse = newImpulse;
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
}
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;
Box2DXDebug.Assert(c.PointCount == 1 || c.PointCount == 2);
#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);
Box2DXDebug.Assert(a.x >= 0.0f && a.y >= 0.0f);
// 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);
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
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);
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
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);
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
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 = 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);
Box2DXDebug.Assert(a.x >= 0.0f && a.y >= 0.0f);
// 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);
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
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);
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
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);
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
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
}
}
internal 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;
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;
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;
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;
Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
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 void SolveVelocityConstraints()
{
for (int i = 0; i < this._constraintCount; i++)
{
ContactConstraint contactConstraint = this._constraints[i];
Body body = contactConstraint.Body1;
Body body2 = contactConstraint.Body2;
float num = body._angularVelocity;
float num2 = body2._angularVelocity;
Vec2 vec = body._linearVelocity;
Vec2 vec2 = body2._linearVelocity;
float invMass = body._invMass;
float invI = body._invI;
float invMass2 = body2._invMass;
float invI2 = body2._invI;
Vec2 normal = contactConstraint.Normal;
Vec2 vec3 = Vec2.Cross(normal, 1f);
float friction = contactConstraint.Friction;
Box2DXDebug.Assert(contactConstraint.PointCount == 1 || contactConstraint.PointCount == 2);
if (contactConstraint.PointCount == 1)
{
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[0];
Vec2 a = vec2 + Vec2.Cross(num2, contactConstraintPoint.R2) - vec - Vec2.Cross(num, contactConstraintPoint.R1);
float num3 = Vec2.Dot(a, normal);
float num4 = -contactConstraintPoint.NormalMass * (num3 - contactConstraintPoint.VelocityBias);
float num5 = Box2DX.Common.Math.Max(contactConstraintPoint.NormalImpulse + num4, 0f);
num4 = num5 - contactConstraintPoint.NormalImpulse;
Vec2 vec4 = num4 * normal;
vec -= invMass * vec4;
num -= invI * Vec2.Cross(contactConstraintPoint.R1, vec4);
vec2 += invMass2 * vec4;
num2 += invI2 * Vec2.Cross(contactConstraintPoint.R2, vec4);
contactConstraintPoint.NormalImpulse = num5;
}
else
{
ContactConstraintPoint contactConstraintPoint2 = contactConstraint.Points[0];
ContactConstraintPoint contactConstraintPoint3 = contactConstraint.Points[1];
Vec2 vec5 = new Vec2(contactConstraintPoint2.NormalImpulse, contactConstraintPoint3.NormalImpulse);
Box2DXDebug.Assert(vec5.X >= 0f && vec5.Y >= 0f);
Vec2 a2 = vec2 + Vec2.Cross(num2, contactConstraintPoint2.R2) - vec - Vec2.Cross(num, contactConstraintPoint2.R1);
Vec2 a3 = vec2 + Vec2.Cross(num2, contactConstraintPoint3.R2) - vec - Vec2.Cross(num, contactConstraintPoint3.R1);
float num6 = Vec2.Dot(a2, normal);
float num7 = Vec2.Dot(a3, normal);
Vec2 vec6;
vec6.X = num6 - contactConstraintPoint2.VelocityBias;
vec6.Y = num7 - contactConstraintPoint3.VelocityBias;
vec6 -= Box2DX.Common.Math.Mul(contactConstraint.K, vec5);
Vec2 v = -Box2DX.Common.Math.Mul(contactConstraint.NormalMass, vec6);
if (v.X >= 0f && v.Y >= 0f)
{
Vec2 vec7 = v - vec5;
Vec2 vec8 = vec7.X * normal;
Vec2 vec9 = vec7.Y * normal;
vec -= invMass * (vec8 + vec9);
num -= invI * (Vec2.Cross(contactConstraintPoint2.R1, vec8) + Vec2.Cross(contactConstraintPoint3.R1, vec9));
vec2 += invMass2 * (vec8 + vec9);
num2 += invI2 * (Vec2.Cross(contactConstraintPoint2.R2, vec8) + Vec2.Cross(contactConstraintPoint3.R2, vec9));
contactConstraintPoint2.NormalImpulse = v.X;
contactConstraintPoint3.NormalImpulse = v.Y;
}
else
{
v.X = -contactConstraintPoint2.NormalMass * vec6.X;
v.Y = 0f;
num7 = contactConstraint.K.Col1.Y * v.X + vec6.Y;
if (v.X >= 0f && num7 >= 0f)
{
Vec2 vec7 = v - vec5;
Vec2 vec8 = vec7.X * normal;
Vec2 vec9 = vec7.Y * normal;
vec -= invMass * (vec8 + vec9);
num -= invI * (Vec2.Cross(contactConstraintPoint2.R1, vec8) + Vec2.Cross(contactConstraintPoint3.R1, vec9));
vec2 += invMass2 * (vec8 + vec9);
num2 += invI2 * (Vec2.Cross(contactConstraintPoint2.R2, vec8) + Vec2.Cross(contactConstraintPoint3.R2, vec9));
contactConstraintPoint2.NormalImpulse = v.X;
contactConstraintPoint3.NormalImpulse = v.Y;
}
else
{
v.X = 0f;
v.Y = -contactConstraintPoint3.NormalMass * vec6.Y;
num6 = contactConstraint.K.Col2.X * v.Y + vec6.X;
if (v.Y >= 0f && num6 >= 0f)
{
Vec2 vec7 = v - vec5;
Vec2 vec8 = vec7.X * normal;
Vec2 vec9 = vec7.Y * normal;
vec -= invMass * (vec8 + vec9);
num -= invI * (Vec2.Cross(contactConstraintPoint2.R1, vec8) + Vec2.Cross(contactConstraintPoint3.R1, vec9));
vec2 += invMass2 * (vec8 + vec9);
num2 += invI2 * (Vec2.Cross(contactConstraintPoint2.R2, vec8) + Vec2.Cross(contactConstraintPoint3.R2, vec9));
contactConstraintPoint2.NormalImpulse = v.X;
contactConstraintPoint3.NormalImpulse = v.Y;
}
else
{
v.X = 0f;
v.Y = 0f;
num6 = vec6.X;
num7 = vec6.Y;
if (num6 >= 0f && num7 >= 0f)
{
Vec2 vec7 = v - vec5;
Vec2 vec8 = vec7.X * normal;
Vec2 vec9 = vec7.Y * normal;
vec -= invMass * (vec8 + vec9);
num -= invI * (Vec2.Cross(contactConstraintPoint2.R1, vec8) + Vec2.Cross(contactConstraintPoint3.R1, vec9));
vec2 += invMass2 * (vec8 + vec9);
num2 += invI2 * (Vec2.Cross(contactConstraintPoint2.R2, vec8) + Vec2.Cross(contactConstraintPoint3.R2, vec9));
contactConstraintPoint2.NormalImpulse = v.X;
contactConstraintPoint3.NormalImpulse = v.Y;
}
}
}
}
}
for (int j = 0; j < contactConstraint.PointCount; j++)
{
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[j];
Vec2 a = vec2 + Vec2.Cross(num2, contactConstraintPoint.R2) - vec - Vec2.Cross(num, contactConstraintPoint.R1);
float num8 = Vec2.Dot(a, vec3);
float num4 = contactConstraintPoint.TangentMass * -num8;
float num9 = friction * contactConstraintPoint.NormalImpulse;
float num5 = Box2DX.Common.Math.Clamp(contactConstraintPoint.TangentImpulse + num4, -num9, num9);
num4 = num5 - contactConstraintPoint.TangentImpulse;
Vec2 vec4 = num4 * vec3;
vec -= invMass * vec4;
num -= invI * Vec2.Cross(contactConstraintPoint.R1, vec4);
vec2 += invMass2 * vec4;
num2 += invI2 * Vec2.Cross(contactConstraintPoint.R2, vec4);
contactConstraintPoint.TangentImpulse = num5;
}
body._linearVelocity = vec;
body._angularVelocity = num;
body2._linearVelocity = vec2;
body2._angularVelocity = num2;
}
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
this._step = step;
this._constraintCount = 0;
for (int i = 0; i < contactCount; i++)
{
Box2DXDebug.Assert(contacts[i].IsSolid());
this._constraintCount += contacts[i].GetManifoldCount();
}
this._constraints = new ContactConstraint[this._constraintCount];
for (int i = 0; i < this._constraintCount; i++)
{
this._constraints[i] = new ContactConstraint();
}
int num = 0;
for (int i = 0; i < contactCount; i++)
{
Contact contact = contacts[i];
Shape shape = contact._shape1;
Shape shape2 = contact._shape2;
Body body = shape.GetBody();
Body body2 = shape2.GetBody();
int manifoldCount = contact.GetManifoldCount();
Manifold[] manifolds = contact.GetManifolds();
float friction = Settings.MixFriction(shape.Friction, shape2.Friction);
float restitution = Settings.MixRestitution(shape.Restitution, shape2.Restitution);
Vec2 linearVelocity = body._linearVelocity;
Vec2 linearVelocity2 = body2._linearVelocity;
float angularVelocity = body._angularVelocity;
float angularVelocity2 = body2._angularVelocity;
for (int j = 0; j < manifoldCount; j++)
{
Manifold manifold = manifolds[j];
Box2DXDebug.Assert(manifold.PointCount > 0);
Vec2 normal = manifold.Normal;
Box2DXDebug.Assert(num < this._constraintCount);
ContactConstraint contactConstraint = this._constraints[num];
contactConstraint.Body1 = body;
contactConstraint.Body2 = body2;
contactConstraint.Manifold = manifold;
contactConstraint.Normal = normal;
contactConstraint.PointCount = manifold.PointCount;
contactConstraint.Friction = friction;
contactConstraint.Restitution = restitution;
for (int k = 0; k < contactConstraint.PointCount; k++)
{
ManifoldPoint manifoldPoint = manifold.Points[k];
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[k];
contactConstraintPoint.NormalImpulse = manifoldPoint.NormalImpulse;
contactConstraintPoint.TangentImpulse = manifoldPoint.TangentImpulse;
contactConstraintPoint.Separation = manifoldPoint.Separation;
contactConstraintPoint.LocalAnchor1 = manifoldPoint.LocalPoint1;
contactConstraintPoint.LocalAnchor2 = manifoldPoint.LocalPoint2;
contactConstraintPoint.R1 = Box2DX.Common.Math.Mul(body.GetXForm().R, manifoldPoint.LocalPoint1 - body.GetLocalCenter());
contactConstraintPoint.R2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, manifoldPoint.LocalPoint2 - body2.GetLocalCenter());
float num2 = Vec2.Cross(contactConstraintPoint.R1, normal);
float num3 = Vec2.Cross(contactConstraintPoint.R2, normal);
num2 *= num2;
num3 *= num3;
float num4 = body._invMass + body2._invMass + body._invI * num2 + body2._invI * num3;
Box2DXDebug.Assert(num4 > Settings.FLT_EPSILON);
contactConstraintPoint.NormalMass = 1f / num4;
float num5 = body._mass * body._invMass + body2._mass * body2._invMass;
num5 += body._mass * body._invI * num2 + body2._mass * body2._invI * num3;
Box2DXDebug.Assert(num5 > Settings.FLT_EPSILON);
contactConstraintPoint.EqualizedMass = 1f / num5;
Vec2 b = Vec2.Cross(normal, 1f);
float num6 = Vec2.Cross(contactConstraintPoint.R1, b);
float num7 = Vec2.Cross(contactConstraintPoint.R2, b);
num6 *= num6;
num7 *= num7;
float num8 = body._invMass + body2._invMass + body._invI * num6 + body2._invI * num7;
Box2DXDebug.Assert(num8 > Settings.FLT_EPSILON);
contactConstraintPoint.TangentMass = 1f / num8;
contactConstraintPoint.VelocityBias = 0f;
if (contactConstraintPoint.Separation > 0f)
{
contactConstraintPoint.VelocityBias = -step.Inv_Dt * contactConstraintPoint.Separation;
}
else
{
float num9 = Vec2.Dot(contactConstraint.Normal, linearVelocity2 + Vec2.Cross(angularVelocity2, contactConstraintPoint.R2) - linearVelocity - Vec2.Cross(angularVelocity, contactConstraintPoint.R1));
if (num9 < -Settings.VelocityThreshold)
{
contactConstraintPoint.VelocityBias = -contactConstraint.Restitution * num9;
}
}
}
if (contactConstraint.PointCount == 2)
{
ContactConstraintPoint contactConstraintPoint2 = contactConstraint.Points[0];
ContactConstraintPoint contactConstraintPoint3 = contactConstraint.Points[1];
float invMass = body._invMass;
float invI = body._invI;
float invMass2 = body2._invMass;
float invI2 = body2._invI;
float num10 = Vec2.Cross(contactConstraintPoint2.R1, normal);
float num11 = Vec2.Cross(contactConstraintPoint2.R2, normal);
float num12 = Vec2.Cross(contactConstraintPoint3.R1, normal);
float num13 = Vec2.Cross(contactConstraintPoint3.R2, normal);
float num14 = invMass + invMass2 + invI * num10 * num10 + invI2 * num11 * num11;
float num15 = invMass + invMass2 + invI * num12 * num12 + invI2 * num13 * num13;
float num16 = invMass + invMass2 + invI * num10 * num12 + invI2 * num11 * num13;
if (num14 * num14 < 100f * (num14 * num15 - num16 * num16))
{
contactConstraint.K.Col1.Set(num14, num16);
contactConstraint.K.Col2.Set(num16, num15);
contactConstraint.NormalMass = contactConstraint.K.Invert();
}
else
{
contactConstraint.PointCount = 1;
}
}
num++;
}
}
Box2DXDebug.Assert(num == this._constraintCount);
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
Step = step;
ConstraintCount = contactCount;
Constraints = new ContactConstraint[ConstraintCount];
for (int i = 0; i < ConstraintCount; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact.GetFixtureA();
Fixture fixtureB = contact.GetFixtureB();
Shape shapeA = fixtureA.GetShape();
Shape shapeB = fixtureB.GetShape();
float radiusA = shapeA._radius;
float radiusB = shapeB._radius;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
Manifold manifold = contact.GetManifold();
float friction = Settings.MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
float restitution = Settings.MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());
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.GetTransform(), radiusA, bodyB.GetTransform(), radiusB);
ContactConstraint cc = new ContactConstraint();
Constraints[i] = cc;
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;
for (int j = 0; j < cc.PointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
ContactConstraintPoint ccp = cc.Points[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 > 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 > 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 > 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 < -Settings.VelocityThreshold)
{
ccp.VelocityBias = -cc.Restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint ccp1 = cc.Points[0];
ContactConstraintPoint ccp2 = cc.Points[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.Invert();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
}
}
public ContactConstraint()
{
for (int i = 0; i < Settings.MaxManifoldPoints; i++)
Points[i] = new ContactConstraintPoint();
}
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
}
