/// <summary>
/// Compute the collision manifold between two circles.
/// </summary>
public static void CollideCircles(ref Manifold manifold, CircleShape circleA, ref Transform xfA, CircleShape circleB, ref Transform xfB)
{
manifold.PointCount = 0;
Vector2 pA = MathUtils.Mul(ref xfA, circleA.Position);
Vector2 pB = MathUtils.Mul(ref xfB, circleB.Position);
Vector2 d = pB - pA;
float distSqr = Vector2.Dot(d, d);
float rA = circleA.Radius, rB = circleB.Radius;
float radius = rA + rB;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = ManifoldType.Circles;
manifold.LocalPoint = circleA.Position;
manifold.LocalNormal = Vector2.Zero;
manifold.PointCount = 1;
ManifoldPoint p0 = manifold.Points[0];
p0.LocalPoint = circleB.Position;
p0.Id.Key = 0;
manifold.Points[0] = p0;
}
/// <summary>
/// Compute the collision manifold between two polygons.
/// </summary>
public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform xfA, PolygonShape polyB, ref Transform xfB)
{
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
manifold.PointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
int edgeA;
float separationA = FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB;
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
bool flip;
const float k_tol = 0.1f * Settings.LinearSlop;
if (separationB > separationA + k_tol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.Type = ManifoldType.FaceB;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.Type = ManifoldType.FaceA;
flip = false;
}
FixedArray2 <ClipVertex> incidentEdge;
FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1.Vertices.Count;
Vertices vertices1 = poly1.Vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
Vector2 v11 = vertices1[iv1];
Vector2 v12 = vertices1[iv2];
Vector2 localTangent = v12 - v11;
localTangent.Normalize();
Vector2 localNormal = MathUtils.Cross(localTangent, 1.0f);
Vector2 planePoint = 0.5f * (v11 + v12);
Vector2 tangent = MathUtils.Mul(ref xf1.q, localTangent);
Vector2 normal = MathUtils.Cross(tangent, 1.0f);
v11 = MathUtils.Mul(ref xf1, v11);
v12 = MathUtils.Mul(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;
// Clip to box side 1
int np = Collision.ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = Collision.ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);
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.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(normal, clipPoints2[i].V) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.LocalPoint = MathUtils.MulT(ref xf2, clipPoints2[i].V);
cp.Id = clipPoints2[i].ID;
if (flip)
{
// Swap features
ContactFeature cf = cp.Id.ContactFeature;
cp.Id.ContactFeature.IndexA = cf.IndexB;
cp.Id.ContactFeature.IndexB = cf.IndexA;
cp.Id.ContactFeature.TypeA = cf.TypeB;
cp.Id.ContactFeature.TypeB = cf.TypeA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
public void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
{
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
_xf = MathUtils.MulT(xfA, xfB);
_centroidB = MathUtils.Mul(ref _xf, polygonB.MassData.Centroid);
_v0 = edgeA.Vertex0;
_v1 = edgeA._vertex1;
_v2 = edgeA._vertex2;
_v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
Vector2 edge1 = _v2 - _v1;
edge1.Normalize();
_normal1 = new Vector2(edge1.Y, -edge1.X);
float offset1 = Vector2.Dot(_normal1, _centroidB - _v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
Vector2 edge0 = _v1 - _v0;
edge0.Normalize();
_normal0 = new Vector2(edge0.Y, -edge0.X);
convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f;
offset0 = Vector2.Dot(_normal0, _centroidB - _v0);
}
// Is there a following edge?
if (hasVertex3)
{
Vector2 edge2 = _v3 - _v2;
edge2.Normalize();
_normal2 = new Vector2(edge2.Y, -edge2.X);
convex2 = MathUtils.Cross(edge1, edge2) > 0.0f;
offset2 = Vector2.Dot(_normal2, _centroidB - _v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = _normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal1;
}
}
else if (convex1)
{
_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = _normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = -_normal1;
}
}
else if (convex2)
{
_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = _normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal0;
}
}
else
{
_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = _normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = -_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = -_normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal1;
}
}
else
{
_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal1;
}
}
else
{
_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = _normal1;
}
}
}
else
{
_front = offset1 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = _normal1;
}
}
// Get polygonB in frameA
_polygonB.Count = polygonB.Vertices.Count;
for (int i = 0; i < polygonB.Vertices.Count; ++i)
{
_polygonB.Vertices[i] = MathUtils.Mul(ref _xf, polygonB.Vertices[i]);
_polygonB.Normals[i] = MathUtils.Mul(_xf.q, polygonB.Normals[i]);
}
_radius = polygonB.Radius + edgeA.Radius;
manifold.PointCount = 0;
EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.Type == EPAxisType.Unknown)
{
return;
}
if (edgeAxis.Separation > _radius)
{
return;
}
EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
EPAxis primaryAxis;
if (polygonAxis.Type == EPAxisType.Unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
FixedArray2 <ClipVertex> ie = new FixedArray2 <ClipVertex>();
ReferenceFace rf;
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.Type = ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Vector2.Dot(_normal, _polygonB.Normals[0]);
for (int i = 1; i < _polygonB.Count; ++i)
{
float value = Vector2.Dot(_normal, _polygonB.Normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < _polygonB.Count ? i1 + 1 : 0;
ie.Value0.V = _polygonB.Vertices[i1];
ie.Value0.ID.ContactFeature.IndexA = 0;
ie.Value0.ID.ContactFeature.IndexB = (byte)i1;
ie.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Face;
ie.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Vertex;
ie.Value1.V = _polygonB.Vertices[i2];
ie.Value1.ID.ContactFeature.IndexA = 0;
ie.Value1.ID.ContactFeature.IndexB = (byte)i2;
ie.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Face;
ie.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Vertex;
if (_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = _v1;
rf.v2 = _v2;
rf.Normal = _normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = _v2;
rf.v2 = _v1;
rf.Normal = -_normal1;
}
}
else
{
manifold.Type = ManifoldType.FaceB;
ie.Value0.V = _v1;
ie.Value0.ID.ContactFeature.IndexA = 0;
ie.Value0.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
ie.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Vertex;
ie.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Face;
ie.Value1.V = _v2;
ie.Value1.ID.ContactFeature.IndexA = 0;
ie.Value1.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
ie.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Vertex;
ie.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Face;
rf.i1 = primaryAxis.Index;
rf.i2 = rf.i1 + 1 < _polygonB.Count ? rf.i1 + 1 : 0;
rf.v1 = _polygonB.Vertices[rf.i1];
rf.v2 = _polygonB.Vertices[rf.i2];
rf.Normal = _polygonB.Normals[rf.i1];
}
rf.SideNormal1 = new Vector2(rf.Normal.Y, -rf.Normal.X);
rf.SideNormal2 = -rf.SideNormal1;
rf.SideOffset1 = Vector2.Dot(rf.SideNormal1, rf.v1);
rf.SideOffset2 = Vector2.Dot(rf.SideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
FixedArray2 <ClipVertex> clipPoints1;
FixedArray2 <ClipVertex> clipPoints2;
int np;
// Clip to box side 1
np = Collision.ClipSegmentToLine(out clipPoints1, ref ie, rf.SideNormal1, rf.SideOffset1, rf.i1);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = Collision.ClipSegmentToLine(out clipPoints2, ref clipPoints1, rf.SideNormal2, rf.SideOffset2, rf.i2);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.LocalNormal = rf.Normal;
manifold.LocalPoint = rf.v1;
}
else
{
manifold.LocalNormal = polygonB.Normals[rf.i1];
manifold.LocalPoint = polygonB.Vertices[rf.i1];
}
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(rf.Normal, clipPoints2[i].V - rf.v1);
if (separation <= _radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MulT(ref _xf, clipPoints2[i].V);
cp.Id = clipPoints2[i].ID;
}
else
{
cp.LocalPoint = clipPoints2[i].V;
cp.Id.ContactFeature.TypeA = clipPoints2[i].ID.ContactFeature.TypeB;
cp.Id.ContactFeature.TypeB = clipPoints2[i].ID.ContactFeature.TypeA;
cp.Id.ContactFeature.IndexA = clipPoints2[i].ID.ContactFeature.IndexB;
cp.Id.ContactFeature.IndexB = clipPoints2[i].ID.ContactFeature.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
