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); } } } } }
internal void Initialize(ContactConstraint cc) { Box2DXDebug.Assert(cc.PointCount > 0); switch (cc.Type) { case ManifoldType.Circles: { Vec2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint); Vec2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint); if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON_SQUARED) { Normal = pointB - pointA; Normal.Normalize(); } else { Normal.Set(1.0f, 0.0f); } Points[0] = 0.5f * (pointA + pointB); Separations[0] = Vec2.Dot(pointB - pointA, Normal) - cc.Radius; } break; case ManifoldType.FaceA: { Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal); Vec2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint); for (int i = 0; i < cc.PointCount; ++i) { Vec2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint); Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius; Points[i] = clipPoint; } } break; case ManifoldType.FaceB: { Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal); Vec2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint); for (int i = 0; i < cc.PointCount; ++i) { Vec2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint); Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius; Points[i] = clipPoint; } // Ensure normal points from A to B Normal = -Normal; } break; } }
internal void Initialize(ContactConstraint cc) { Box2DXDebug.Assert(cc.PointCount > 0); switch (cc.Type) { case ManifoldType.Circles: { Vector2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint); Vector2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint); if ((pointA - pointB).sqrMagnitude > (Mathf.Epsilon * Mathf.Epsilon)) { Normal = pointB - pointA; Normal.Normalize(); } else { Normal = new Vector2(1.0f, 0.0f); } Points[0] = 0.5f * (pointA + pointB); Separations[0] = Vector2.Dot(pointB - pointA, Normal) - cc.Radius; } break; case ManifoldType.FaceA: { Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal); Vector2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint); for (int i = 0; i < cc.PointCount; ++i) { Vector2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint); Separations[i] = Vector2.Dot(clipPoint - planePoint, Normal) - cc.Radius; Points[i] = clipPoint; } } break; case ManifoldType.FaceB: { Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal); Vector2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint); for (int i = 0; i < cc.PointCount; ++i) { Vector2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint); Separations[i] = Vector2.Dot(clipPoint - planePoint, Normal) - cc.Radius; Points[i] = clipPoint; } // Ensure normal points from A to B Normal = -Normal; } break; } }
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 FinalizeVelocityConstraints() { for (int i = 0; i < this._constraintCount; i++) { ContactConstraint contactConstraint = this._constraints[i]; Manifold manifold = contactConstraint.Manifold; for (int j = 0; j < contactConstraint.PointCount; j++) { manifold.Points[j].NormalImpulse = contactConstraint.Points[j].NormalImpulse; manifold.Points[j].TangentImpulse = contactConstraint.Points[j].TangentImpulse; } } }
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(); } 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); 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; ContactSolverSetup(manifold, worldManifold, cc); } }
public void FinalizeVelocityConstraints() { for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = _constraints[i]; Manifold m = c.Manifold; for (int j = 0; j < c.PointCount; ++j) { m.Points[j].NormalImpulse = c.Points[j].NormalImpulse; m.Points[j].TangentImpulse = c.Points[j].TangentImpulse; } } }
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 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; } } } }
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 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 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 }
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 } }
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 void Initialize(ContactConstraint cc) { Box2DXDebug.Assert(cc.PointCount > 0); switch (cc.Type) { case Manifold.ManifoldType.Circles: { Vec2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint); Vec2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint); if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON * Settings.FLT_EPSILON) { Normal = pointB - pointA; Normal.Normalize(); } else { Normal.Set(1.0f, 0.0f); } Points[0] = 0.5f * (pointA + pointB); Separations[0] = Vec2.Dot(pointB - pointA, Normal) - cc.Radius; } break; case Manifold.ManifoldType.FaceA: { Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal); Vec2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint); for (int i = 0; i < cc.PointCount; ++i) { Vec2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint); Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius; Points[i] = clipPoint; } } break; case Manifold.ManifoldType.FaceB: { Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal); Vec2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint); for (int i = 0; i < cc.PointCount; ++i) { Vec2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint); Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius; Points[i] = clipPoint; } // Ensure normal points from A to B Normal = -Normal; } break; } }
public void Report(ContactConstraint[] constraints) { if (_listener == null) { return; } for (int i = 0; i < _contactCount; ++i) { Contact c = _contacts[i]; ContactConstraint cc = constraints[i]; ContactImpulse impulse = new ContactImpulse(); for (int j = 0; j < cc.PointCount; ++j) { impulse.normalImpulses[j] = cc.Points[j].NormalImpulse; impulse.tangentImpulses[j] = cc.Points[j].TangentImpulse; } _listener.PostSolve(c, impulse); } }
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 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++) { _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; } } } } } }
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 void Report(ContactConstraint[] constraints) { if (_listener == null) { return; } for (int i = 0; i < _contactCount; ++i) { Contact c = _contacts[i]; ContactConstraint cc = constraints[i]; ContactResult cr = new ContactResult(); cr.Shape1 = c.GetShape1(); cr.Shape2 = c.GetShape2(); Body b1 = cr.Shape1.GetBody(); int manifoldCount = c.GetManifoldCount(); Manifold[] manifolds = c.GetManifolds(); for (int j = 0; j < manifoldCount; ++j) { Manifold manifold = manifolds[j]; cr.Normal = manifold.Normal; for (int k = 0; k < manifold.PointCount; ++k) { ManifoldPoint point = manifold.Points[k]; ContactConstraintPoint ccp = cc.Points[k]; cr.Position = b1.GetWorldPoint(point.LocalPoint1); // TOI constraint results are not stored, so get // the result from the constraint. cr.NormalImpulse = ccp.NormalImpulse; cr.TangentImpulse = ccp.TangentImpulse; cr.ID = point.ID; _listener.Result(cr); } } } }