internal void Update(IContactListener listener)
{
Manifold oldManifold = _manifold;
Evaluate();
Body bodyA = _fixtureA.GetBody();
Body bodyB = _fixtureB.GetBody();
int oldCount = oldManifold._pointCount;
int newCount = _manifold._pointCount;
if (newCount == 0 && oldCount > 0)
{
bodyA.WakeUp();
bodyB.WakeUp();
}
// Slow contacts don't generate TOI events.
if (bodyA.IsStatic || bodyA.IsBullet || bodyB.IsStatic || bodyB.IsBullet)
{
_flags &= ~ContactFlags.Slow;
}
else
{
_flags |= ContactFlags.Slow;
}
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < _manifold._pointCount; ++i)
{
ManifoldPoint mp2 = _manifold._points[i];
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
ContactID id2 = mp2.Id;
for (int j = 0; j < oldManifold._pointCount; ++j)
{
ManifoldPoint mp1 = oldManifold._points[j];
if (mp1.Id.Key == id2.Key)
{
mp2.NormalImpulse = mp1.NormalImpulse;
mp2.TangentImpulse = mp1.TangentImpulse;
break;
}
}
_manifold._points[i] = mp2;
}
if (oldCount == 0 && newCount > 0)
{
_flags |= ContactFlags.Touch;
listener.BeginContact(this);
}
if (oldCount > 0 && newCount == 0)
{
_flags &= ~ContactFlags.Touch;
listener.EndContact(this);
}
if ((_flags & ContactFlags.NonSolid) == 0)
{
listener.PreSolve(this, ref oldManifold);
// The user may have disabled contact.
if (_manifold._pointCount == 0)
{
_flags &= ~ContactFlags.Touch;
}
}
}
public void Reset(ref TimeStep step, List <Contact> contacts)
{
_step = step;
_contacts = contacts;
int contactCount = contacts.Count;
int constraintCount = contactCount;
_constraints.Clear();
for (int i = 0; i < constraintCount; i++)
{
_constraints.Add(new ContactConstraint());
}
for (int i = 0; i < constraintCount; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
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(out manifold);
float friction = Settings.b2MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
float restitution = Settings.b2MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());
Vector2 vA = bodyA._linearVelocity;
Vector2 vB = bodyB._linearVelocity;
float wA = bodyA._angularVelocity;
float wB = bodyB._angularVelocity;
Debug.Assert(manifold._pointCount > 0);
WorldManifold worldManifold = new WorldManifold(ref manifold, ref bodyA._xf, radiusA, ref 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;
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 = MathUtils.Cross(ccp.rA, cc.normal);
float rnB = MathUtils.Cross(ccp.rB, cc.normal);
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;
Debug.Assert(kNormal > Settings.b2_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;
Debug.Assert(kEqualized > Settings.b2_FLT_EPSILON);
ccp.equalizedMass = 1.0f / kEqualized;
Vector2 tangent = MathUtils.Cross(cc.normal, 1.0f);
float rtA = MathUtils.Cross(ccp.rA, tangent);
float rtB = MathUtils.Cross(ccp.rB, tangent);
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;
Debug.Assert(kTangent > Settings.b2_FLT_EPSILON);
ccp.tangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.velocityBias = 0.0f;
float vRel = Vector2.Dot(cc.normal, vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA));
if (vRel < -Settings.b2_velocityThreshold)
{
ccp.velocityBias = -cc.restitution * vRel;
}
cc.points[j] = ccp;
}
// 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 = MathUtils.Cross(ccp1.rA, cc.normal);
float rn1B = MathUtils.Cross(ccp1.rB, cc.normal);
float rn2A = MathUtils.Cross(ccp2.rA, cc.normal);
float rn2B = MathUtils.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.
float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K = new Mat22(new Vector2(k11, k12), new Vector2(k12, k22));
cc.normalMass = cc.K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.pointCount = 1;
}
}
_constraints[i] = cc;
}
}
/// Compute the collision manifold between two polygons.
public static void CollidePolygons(ref Manifold manifold,
PolygonShape polyA, ref XForm xfA,
PolygonShape polyB, ref XForm xfB)
{
manifold._pointCount = 0;
float totalRadius = polyA._radius + polyB._radius;
int edgeA = 0;
float separationA = FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
float separationB = FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
{
return;
}
PolygonShape poly1; // reference polygon
PolygonShape poly2; // incident polygon
XForm xf1, xf2;
int edge1; // reference edge
byte flip;
float k_relativeTol = 0.98f;
float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold._type = ManifoldType.FaceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold._type = ManifoldType.FaceA;
flip = 0;
}
FixedArray2 <ClipVertex> incidentEdge;
FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1._vertexCount;
Vector2 v11 = poly1._vertices[edge1];
Vector2 v12 = edge1 + 1 < count1 ? poly1._vertices[edge1 + 1] : poly1._vertices[0];
Vector2 dv = v12 - v11;
Vector2 localNormal = MathUtils.Cross(dv, 1.0f);
localNormal.Normalize();
Vector2 planePoint = 0.5f * (v11 + v12);
Vector2 sideNormal = MathUtils.Multiply(ref xf1.R, v12 - v11);
sideNormal.Normalize();
Vector2 frontNormal = MathUtils.Cross(sideNormal, 1.0f);
v11 = MathUtils.Multiply(ref xf1, v11);
v12 = MathUtils.Multiply(ref xf1, v12);
float frontOffset = Vector2.Dot(frontNormal, v11);
float sideOffset1 = -Vector2.Dot(sideNormal, v11);
float sideOffset2 = Vector2.Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
FixedArray2 <ClipVertex> clipPoints1;
FixedArray2 <ClipVertex> clipPoints2;
int np;
// Clip to box side 1
np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold._localPlaneNormal = localNormal;
manifold._localPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i)
{
float separation = Vector2.Dot(frontNormal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold._points[pointCount];
cp.LocalPoint = MathUtils.MultiplyT(ref xf2, clipPoints2[i].v);
cp.Id = clipPoints2[i].id;
cp.Id.Features.Flip = flip;
manifold._points[pointCount] = cp;
++pointCount;
}
}
manifold._pointCount = pointCount;
}