// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
public static void CollideEdgeAndCircle(ref Manifold manifold,
EdgeShape edgeA, ref Transform xfA,
CircleShape circleB, ref Transform xfB)
{
manifold._pointCount = 0;
// Compute circle in frame of edge
Vector2 Q = MathUtils.MultiplyT(ref xfA, MathUtils.Multiply(ref xfB, circleB._p));
Vector2 A = edgeA._vertex1, B = edgeA._vertex2;
Vector2 e = B - A;
// Barycentric coordinates
float u = Vector2.Dot(e, B - Q);
float v = Vector2.Dot(e, Q - A);
float radius = edgeA._radius + circleB._radius;
ContactFeature cf;
cf.indexB = 0;
cf.typeB = (byte)ContactFeatureType.Vertex;
Vector2 P, d;
// Region A
if (v <= 0.0f)
{
P = A;
d = Q - P;
float dd = Vector2.Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA._hasVertex0)
{
Vector2 A1 = edgeA._vertex0;
Vector2 B1 = A;
Vector2 e1 = B1 - A1;
float u1 = Vector2.Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = (byte)ContactFeatureType.Vertex;
manifold._pointCount = 1;
manifold._type = ManifoldType.Circles;
manifold._localNormal = Vector2.Zero;
manifold._localPoint = P;
var mp = new ManifoldPoint();
mp.Id.Key = 0;
mp.Id.Features = cf;
mp.LocalPoint = circleB._p;
manifold._points[0] = mp;
return;
}
// Region B
if (u <= 0.0f)
{
P = B;
d = Q - P;
float dd = Vector2.Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA._hasVertex3)
{
Vector2 B2 = edgeA._vertex3;
Vector2 A2 = B;
Vector2 e2 = B2 - A2;
float v2 = Vector2.Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = (byte)ContactFeatureType.Vertex;
manifold._pointCount = 1;
manifold._type = ManifoldType.Circles;
manifold._localNormal = Vector2.Zero;
manifold._localPoint = P;
var mp = new ManifoldPoint();
mp.Id.Key = 0;
mp.Id.Features = cf;
mp.LocalPoint = circleB._p;
manifold._points[0] = mp;
return;
}
// Region AB
float den = Vector2.Dot(e, e);
Debug.Assert(den > 0.0f);
P = (1.0f / den) * (u * A + v * B);
d = Q - P;
float dd2 = Vector2.Dot(d, d);
if (dd2 > radius * radius)
{
return;
}
Vector2 n = new Vector2(-e.Y, e.X);
if (Vector2.Dot(n, Q - A) < 0.0f)
{
n = new Vector2(-n.X, -n.Y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = (byte)ContactFeatureType.Face;
manifold._pointCount = 1;
manifold._type = ManifoldType.FaceA;
manifold._localNormal = n;
manifold._localPoint = A;
var mp2 = new ManifoldPoint();
mp2.Id.Key = 0;
mp2.Id.Features = cf;
mp2.LocalPoint = circleB._p;
manifold._points[0] = mp2;
}
/// <summary>
/// Compute the collision manifold between two polygons.
/// </summary>
/// <param name="manifold"></param>
/// <param name="polyA"></param>
/// <param name="xfA"></param>
/// <param name="polyB"></param>
/// <param name="xfB"></param>
public static void CollidePolygons(ref Manifold manifold,
PolygonShape polyA, ref Transform xfA,
PolygonShape polyB, ref Transform 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
Transform xf1, xf2;
int edge1; // reference edge
byte flip;
const float k_relativeTol = 0.98f;
const 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 localTangent = v12 - v11;
localTangent.Normalize();
Vector2 localNormal = MathUtils.Cross(localTangent, 1.0f);
Vector2 planePoint = 0.5f * (v11 + v12);
Vector2 tangent = MathUtils.Multiply(ref xf1.R, localTangent);
Vector2 normal = MathUtils.Cross(tangent, 1.0f);
v11 = MathUtils.Multiply(ref xf1, v11);
v12 = MathUtils.Multiply(ref xf1, v12);
// Face offset.
float frontOffset = Vector2.Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius;
float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius;
// 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, -tangent, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold._localNormal = localNormal;
manifold._localPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i)
{
float separation = Vector2.Dot(normal, 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;
}
// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
internal void Update(IContactListener listener)
{
Manifold oldManifold = _manifold;
// Re-enable this contact.
_flags |= ContactFlags.Enabled;
bool touching = false;
bool wasTouching = (_flags & ContactFlags.Touching) == ContactFlags.Touching;
bool sensorA = _fixtureA.IsSensor();
bool sensorB = _fixtureB.IsSensor();
bool sensor = sensorA || sensorB;
Body bodyA = _fixtureA.GetBody();
Body bodyB = _fixtureB.GetBody();
Transform xfA; bodyA.GetTransform(out xfA);
Transform xfB; bodyB.GetTransform(out xfB);
// Is this contact a sensor?
if (sensor)
{
Shape shapeA = _fixtureA.GetShape();
Shape shapeB = _fixtureB.GetShape();
touching = AABB.TestOverlap(shapeA, _indexA, shapeB, _indexB, ref xfA, ref xfB);
// Sensors don't generate manifolds.
_manifold._pointCount = 0;
}
else
{
Evaluate(ref _manifold, ref xfA, ref xfB);
touching = _manifold._pointCount > 0;
// 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;
bool found = false;
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;
found = true;
break;
}
}
if (found == false)
{
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
}
_manifold._points[i] = mp2;
}
if (touching != wasTouching)
{
bodyA.SetAwake(true);
bodyB.SetAwake(true);
}
}
if (touching)
{
_flags |= ContactFlags.Touching;
}
else
{
_flags &= ~ContactFlags.Touching;
}
if (wasTouching == false && touching == true && null != listener)
{
listener.BeginContact(this);
}
if (wasTouching == true && touching == false && null != listener)
{
listener.EndContact(this);
}
if (sensor == false && null != listener)
{
listener.PreSolve(this, ref oldManifold);
}
}
public void Reset(Contact[] contacts, int contactCount, float impulseRatio)
{
_contacts = contacts;
_constraintCount = contactCount;
// grow the array
if (_constraints == null || _constraints.Length < _constraintCount)
{
_constraints = new ContactConstraint[_constraintCount * 2];
}
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.localNormal = manifold._localNormal;
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 = impulseRatio * cp.NormalImpulse;
ccp.tangentImpulse = impulseRatio * cp.TangentImpulse;
ccp.localPoint = cp.LocalPoint;
ccp.rA = worldManifold._points[j] - bodyA._sweep.c;
ccp.rB = worldManifold._points[j] - bodyB._sweep.c;
#if MATH_OVERLOADS
float rnA = MathUtils.Cross(ccp.rA, cc.normal);
float rnB = MathUtils.Cross(ccp.rB, cc.normal);
#else
float rnA = ccp.rA.X * cc.normal.Y - ccp.rA.Y * cc.normal.X;
float rnB = ccp.rB.X * cc.normal.Y - ccp.rB.Y * cc.normal.X;
#endif
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;
Debug.Assert(kNormal > Settings.b2_epsilon);
ccp.normalMass = 1.0f / kNormal;
#if MATH_OVERLOADS
Vector2 tangent = MathUtils.Cross(cc.normal, 1.0f);
float rtA = MathUtils.Cross(ccp.rA, tangent);
float rtB = MathUtils.Cross(ccp.rB, tangent);
#else
Vector2 tangent = new Vector2(cc.normal.Y, -cc.normal.X);
float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X;
float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X;
#endif
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;
Debug.Assert(kTangent > Settings.b2_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 = -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.
const 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;
}
}
