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();
}
}
