private void PostSolve(Contact contact, ContactConstraint impulse) { if (!Broken) { if (Parts.Contains(contact.FixtureA) || Parts.Contains(contact.FixtureB)) { float maxImpulse = 0.0f; int count = contact.Manifold.PointCount; for (int i = 0; i < count; ++i) { maxImpulse = Math.Max(maxImpulse, impulse.Points[i].NormalImpulse); } if (maxImpulse > Strength) { // Flag the body for breaking. _break = true; } } } }
private static void Solve(ContactConstraint cc, int index, out Vector2 normal, out Vector2 point, out float separation) { Debug.Assert(cc.PointCount > 0); normal = Vector2.Zero; switch (cc.Type) { case ManifoldType.Circles: { Vector2 pointA = cc.BodyA.GetWorldPoint(ref cc.LocalPoint); Vector2 pointB = cc.BodyB.GetWorldPoint(ref cc.Points[0].LocalPoint); float a = (pointA.X - pointB.X) * (pointA.X - pointB.X) + (pointA.Y - pointB.Y) * (pointA.Y - pointB.Y); if (a > Settings.Epsilon * Settings.Epsilon) { Vector2 normalTmp = pointB - pointA; float factor = 1f / (float)Math.Sqrt(normalTmp.X * normalTmp.X + normalTmp.Y * normalTmp.Y); normal.X = normalTmp.X * factor; normal.Y = normalTmp.Y * factor; } else { normal.X = 1; normal.Y = 0; } point = 0.5f * (pointA + pointB); separation = (pointB.X - pointA.X) * normal.X + (pointB.Y - pointA.Y) * normal.Y - cc.RadiusA - cc.RadiusB; } break; case ManifoldType.FaceA: { normal = cc.BodyA.GetWorldVector(ref cc.LocalNormal); Vector2 planePoint = cc.BodyA.GetWorldPoint(ref cc.LocalPoint); Vector2 clipPoint = cc.BodyB.GetWorldPoint(ref cc.Points[index].LocalPoint); separation = (clipPoint.X - planePoint.X) * normal.X + (clipPoint.Y - planePoint.Y) * normal.Y - cc.RadiusA - cc.RadiusB; point = clipPoint; } break; case ManifoldType.FaceB: { normal = cc.BodyB.GetWorldVector(ref cc.LocalNormal); Vector2 planePoint = cc.BodyB.GetWorldPoint(ref cc.LocalPoint); Vector2 clipPoint = cc.BodyA.GetWorldPoint(ref cc.Points[index].LocalPoint); separation = (clipPoint.X - planePoint.X) * normal.X + (clipPoint.Y - planePoint.Y) * normal.Y - cc.RadiusA - cc.RadiusB; point = clipPoint; // Ensure normal points from A to B normal = -normal; } break; default: point = Vector2.Zero; separation = 0.0f; break; } }
private void Report(ContactConstraint[] constraints) { if (_contactManager == null) return; for (int i = 0; i < ContactCount; ++i) { Contact c = _contacts[i]; if (c.FixtureA.AfterCollision != null) c.FixtureA.AfterCollision(c.FixtureA, c.FixtureB, c); if (c.FixtureB.AfterCollision != null) c.FixtureB.AfterCollision(c.FixtureB, c.FixtureA, c); if (_contactManager.PostSolve != null) { ContactConstraint cc = constraints[i]; _contactManager.PostSolve(c, cc); } } }
public void Reset(Contact[] contacts, int contactCount, float impulseRatio, bool warmstarting) { _contacts = contacts; _constraintCount = contactCount; // grow the array if (Constraints == null || Constraints.Length < _constraintCount) { Constraints = new ContactConstraint[_constraintCount * 2]; for (int i = 0; i < Constraints.Length; i++) { Constraints[i] = new ContactConstraint(); } } // Initialize position independent portions of the constraints. 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; PhysicsBody bodyA = fixtureA.Body; PhysicsBody bodyB = fixtureB.Body; Manifold manifold = contact.Manifold; Debug.Assert(manifold.PointCount > 0); ContactConstraint cc = Constraints[i]; cc.Friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction); cc.Restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution); cc.BodyA = bodyA; cc.BodyB = bodyB; cc.Manifold = manifold; cc.Normal = Vector2.Zero; cc.PointCount = manifold.PointCount; cc.LocalNormal = manifold.LocalNormal; cc.LocalPoint = manifold.LocalPoint; cc.RadiusA = radiusA; cc.RadiusB = radiusB; cc.Type = manifold.Type; for (int j = 0; j < cc.PointCount; ++j) { ManifoldPoint cp = manifold.Points[j]; ContactConstraintPoint ccp = cc.Points[j]; if (warmstarting) { ccp.NormalImpulse = impulseRatio * cp.NormalImpulse; ccp.TangentImpulse = impulseRatio * cp.TangentImpulse; } else { ccp.NormalImpulse = 0.0f; ccp.TangentImpulse = 0.0f; } ccp.LocalPoint = cp.LocalPoint; ccp.rA = Vector2.Zero; ccp.rB = Vector2.Zero; ccp.NormalMass = 0.0f; ccp.TangentMass = 0.0f; ccp.VelocityBias = 0.0f; } cc.K.SetZero(); cc.NormalMass.SetZero(); } }