// This function collides and edge and a polygon.
// This takes into account edge adjacency.
// 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
public static bool CollideEdgeAndPolygon(
Fixture fixtureA,
WorldTransform xfA,
Fixture fixtureB,
WorldTransform xfB,
out Manifold manifold)
{
manifold = new Manifold();
var edgeA = fixtureA as EdgeFixture;
var polygonB = fixtureB as PolygonFixture;
System.Diagnostics.Debug.Assert(edgeA != null);
System.Diagnostics.Debug.Assert(polygonB != null);
// This holds polygon B expressed in frame A.
var tpv = new Vector2[Settings.MaxPolygonVertices];
var tpn = new Vector2[Settings.MaxPolygonVertices];
Vector2 normal0 = Vector2.Zero, normal1, normal2 = Vector2.Zero;
var xf = xfA.MulT(xfB);
var centroidB = xf.ToOther(polygonB.Centroid);
var v0 = edgeA.Vertex0;
var v1 = edgeA.Vertex1;
var v2 = edgeA.Vertex2;
var v3 = edgeA.Vertex3;
var hasVertex0 = edgeA.HasVertex0;
var hasVertex3 = edgeA.HasVertex3;
var edge1 = v2 - v1;
edge1.Normalize();
normal1.X = edge1.Y;
normal1.Y = -edge1.X;
var offset1 = Vector2Util.Dot(normal1, centroidB - v1);
var offset0 = 0.0f;
var offset2 = 0.0f;
var convex1 = false;
var convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
var edge0 = v1 - v0;
edge0.Normalize();
normal0.X = edge0.Y;
normal0.Y = -edge0.X;
convex1 = Vector2Util.Cross(ref edge0, ref edge1) >= 0.0f;
offset0 = Vector2Util.Dot(normal0, centroidB - v0);
}
// Is there a following edge?
if (hasVertex3)
{
var edge2 = v3 - v2;
edge2.Normalize();
normal2.X = edge2.Y;
normal2.Y = -edge2.X;
convex2 = Vector2Util.Cross(ref edge1, ref edge2) > 0.0f;
offset2 = Vector2Util.Dot(normal2, centroidB - v2);
}
// Determine front or back collision. Determine collision normal limits.
bool front;
Vector2 normal, lowerLimit, upperLimit;
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.
var tpc = polygonB.Count;
for (var i = 0; i < tpc; ++i)
{
tpv[i] = xf.ToOther(polygonB.Vertices[i]);
tpn[i] = xf.Rotation * polygonB.Normals[i];
}
const float radius = 2.0f * Settings.PolygonRadius;
Axis edgeAxis;
edgeAxis.Type = Axis.AxisType.EdgeA;
edgeAxis.Index = front ? 0 : 1;
edgeAxis.Separation = float.MaxValue;
for (var i = 0; i < tpc; ++i)
{
var s = Vector2Util.Dot(normal, tpv[i] - v1);
if (s < edgeAxis.Separation)
{
edgeAxis.Separation = s;
}
}
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.Type == Axis.AxisType.None)
{
return(false);
}
if (edgeAxis.Separation > radius)
{
return(false);
}
Axis polygonAxis;
polygonAxis.Type = Axis.AxisType.None;
polygonAxis.Index = -1;
polygonAxis.Separation = float.MinValue;
Vector2 perp;
perp.X = -normal.Y;
perp.Y = normal.X;
for (var i = 0; i < tpc; ++i)
{
var n = -tpn[i];
var s1 = Vector2Util.Dot(n, tpv[i] - v1);
var s2 = Vector2Util.Dot(n, tpv[i] - v2);
var s = System.Math.Min(s1, s2);
if (s > radius)
{
// No collision
polygonAxis.Type = Axis.AxisType.EdgeB;
polygonAxis.Index = i;
polygonAxis.Separation = s;
break;
}
// Adjacency
if (Vector2Util.Dot(ref n, ref perp) >= 0.0f)
{
if (Vector2Util.Dot(n - upperLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
else
{
if (Vector2Util.Dot(n - lowerLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
if (s > polygonAxis.Separation)
{
polygonAxis.Type = Axis.AxisType.EdgeB;
polygonAxis.Index = i;
polygonAxis.Separation = s;
}
}
if (polygonAxis.Type != Axis.AxisType.None && polygonAxis.Separation > radius)
{
return(false);
}
// Use hysteresis for jitter reduction.
const float relativeTol = 0.98f;
const float absoluteTol = 0.001f;
Axis primaryAxis;
if (polygonAxis.Type == Axis.AxisType.None)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.Separation > relativeTol * edgeAxis.Separation + absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
FixedArray2 <ClipVertex> incidentEdge;
// Reference face used for clipping
int rfi1, rfi2;
Vector2 rfv1, rfv2;
Vector2 rfnormal;
Vector2 rfsideNormal1;
if (primaryAxis.Type == Axis.AxisType.EdgeA)
{
manifold.Type = Manifold.ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
var bestIndex = 0;
var bestValue = Vector2Util.Dot(ref normal, ref tpn[0]);
for (var i = 1; i < tpc; ++i)
{
var value = Vector2Util.Dot(ref normal, ref tpn[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
var i1 = bestIndex;
var i2 = i1 + 1 < tpc ? i1 + 1 : 0;
incidentEdge = new FixedArray2 <ClipVertex>
{
Item1 = new ClipVertex
{
Vertex = tpv[i1],
Id =
{
Feature =
{
IndexA = 0,
IndexB = (byte)i1,
TypeA = (byte)ContactFeature.FeatureType.Face,
TypeB = (byte)ContactFeature.FeatureType.Vertex
}
}
},
Item2 = new ClipVertex
{
Vertex = tpv[i2],
Id =
{
Feature =
{
IndexA = 0,
IndexB = (byte)i2,
TypeA = (byte)ContactFeature.FeatureType.Face,
TypeB = (byte)ContactFeature.FeatureType.Vertex
}
}
}
};
if (front)
{
rfi1 = 0;
rfi2 = 1;
rfv1 = v1;
rfv2 = v2;
rfnormal = normal1;
}
else
{
rfi1 = 1;
rfi2 = 0;
rfv1 = v2;
rfv2 = v1;
rfnormal = -normal1;
}
}
else
{
manifold.Type = Manifold.ManifoldType.FaceB;
incidentEdge = new FixedArray2 <ClipVertex>
{
Item1 = new ClipVertex
{
Vertex = v1,
Id =
{
Feature =
{
IndexA = 0,
IndexB = (byte)primaryAxis.Index,
TypeA = (byte)ContactFeature.FeatureType.Vertex,
TypeB = (byte)ContactFeature.FeatureType.Face
}
}
},
Item2 = new ClipVertex
{
Vertex = v2,
Id =
{
Feature =
{
IndexA = 0,
IndexB = (byte)primaryAxis.Index,
TypeA = (byte)ContactFeature.FeatureType.Vertex,
TypeB = (byte)ContactFeature.FeatureType.Face
}
}
}
};
rfi1 = primaryAxis.Index;
rfi2 = rfi1 + 1 < tpc ? rfi1 + 1 : 0;
rfv1 = tpv[rfi1];
rfv2 = tpv[rfi2];
rfnormal = tpn[rfi1];
}
rfsideNormal1.X = rfnormal.Y;
rfsideNormal1.Y = -rfnormal.X;
var rfsideNormal2 = -rfsideNormal1;
var rfsideOffset1 = Vector2Util.Dot(ref rfsideNormal1, ref rfv1);
var rfsideOffset2 = Vector2Util.Dot(ref rfsideNormal2, ref rfv2);
// Clip incident edge against extruded edge1 side edges.
FixedArray2 <ClipVertex> clipPoints1, clipPoints2;
// Clip to box side 1
var np = ClipSegmentToLine(
out clipPoints1,
incidentEdge,
rfsideNormal1,
rfsideOffset1,
rfi1);
if (np < 2)
{
return(false);
}
// Clip to negative box side 1
np = ClipSegmentToLine(
out clipPoints2,
clipPoints1,
rfsideNormal2,
rfsideOffset2,
rfi2);
if (np < 2)
{
return(false);
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == Axis.AxisType.EdgeA)
{
manifold.LocalPoint = rfv1;
manifold.LocalNormal = rfnormal;
}
else
{
manifold.LocalPoint = polygonB.Vertices[rfi1];
manifold.LocalNormal = polygonB.Normals[rfi1];
}
var pointCount = 0;
for (var i = 0; i < 2; ++i)
{
if (Vector2Util.Dot(rfnormal, clipPoints2[i].Vertex - rfv1) <= radius)
{
var cp = manifold.Points[pointCount];
if (primaryAxis.Type == Axis.AxisType.EdgeA)
{
cp.LocalPoint = xf.FromOther(clipPoints2[i].Vertex);
cp.Id = clipPoints2[i].Id;
}
else
{
cp.LocalPoint = clipPoints2[i].Vertex;
cp.Id.Feature.TypeA = clipPoints2[i].Id.Feature.TypeB;
cp.Id.Feature.TypeB = clipPoints2[i].Id.Feature.TypeA;
cp.Id.Feature.IndexA = clipPoints2[i].Id.Feature.IndexB;
cp.Id.Feature.IndexB = clipPoints2[i].Id.Feature.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
return(pointCount > 0);
}
// 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
// The normal points from 1 to 2
public static bool CollidePolygons(
Fixture fixtureA,
WorldTransform transformA,
Fixture fixtureB,
WorldTransform transformB,
out Manifold manifold)
{
manifold = new Manifold();
var polygonA = fixtureA as PolygonFixture;
var polygonB = fixtureB as PolygonFixture;
System.Diagnostics.Debug.Assert(polygonA != null);
System.Diagnostics.Debug.Assert(polygonB != null);
var totalRadius = polygonA.Radius + polygonB.Radius;
int edgeA;
var separationA = FindMaxSeparation(out edgeA, polygonA, transformA, polygonB, transformB);
if (separationA > totalRadius)
{
return(false);
}
int edgeB;
var separationB = FindMaxSeparation(out edgeB, polygonB, transformB, polygonA, transformA);
if (separationB > totalRadius)
{
return(false);
}
PolygonFixture polygon1; // reference polygon
PolygonFixture polygon2; // incident polygon
WorldTransform transform1, transform2;
int edge1; // reference edge
bool flip;
const float relativeTol = 0.98f;
const float absoluteTol = 0.001f;
if (separationB > relativeTol * separationA + absoluteTol)
{
polygon1 = polygonB;
polygon2 = polygonA;
transform1 = transformB;
transform2 = transformA;
edge1 = edgeB;
manifold.Type = Manifold.ManifoldType.FaceB;
flip = true;
}
else
{
polygon1 = polygonA;
polygon2 = polygonB;
transform1 = transformA;
transform2 = transformB;
edge1 = edgeA;
manifold.Type = Manifold.ManifoldType.FaceA;
flip = false;
}
// Transformation mapping from the second polygon's frame of reference to
// first one's. We use this to directly map points around, without getting
// into the global coordinate system.
var transform21 = transform1.MulT(transform2);
// Begin inlined FindIncidentEdge()
FixedArray2 <ClipVertex> incidentEdge;
{
System.Diagnostics.Debug.Assert(0 <= edge1 && edge1 < polygon1.Count);
var normals1 = polygon1.Normals;
var vertices2 = polygon2.Vertices;
var normals2 = polygon2.Normals;
var count2 = polygon2.Count;
// Get the normal of the reference edge in poly2's frame.
var normal12 = -transform2.Rotation * (transform1.Rotation * normals1[edge1]);
// Find the incident edge on poly2 by finding the clip vertices
// for the incident edge.
// Get the face whose own normal has the smallest angular
// difference to the incident normal.
var edge2 = 0;
var minDot = float.MaxValue;
for (var i = 0; i < count2; ++i)
{
var dot = Vector2Util.Dot(ref normal12, ref normals2[i]);
if (dot < minDot)
{
minDot = dot;
edge2 = i;
}
}
// The edge's index coincides with the first vertex used to define
// that edge, so we can use that and wrap around as necessary for the
// second one.
var index21 = edge2;
var index22 = index21 + 1 < count2 ? index21 + 1 : 0;
// Get the incident edge as defined by its two vertices, in the first
// polygon's frame of reference.
incidentEdge = new FixedArray2 <ClipVertex>
{
Item1 = new ClipVertex
{
//Vertex = xf2.ToGlobal(vertices2[i1]),
Vertex = transform21.ToOther(vertices2[index21]),
Id =
{
Feature =
{
IndexA = (byte)edge1,
IndexB = (byte)index21,
TypeA = (byte)ContactFeature.FeatureType.Face,
TypeB = (byte)ContactFeature.FeatureType.Vertex
}
}
},
Item2 = new ClipVertex
{
//Vertex = xf2.ToGlobal(vertices2[i2]),
Vertex = transform21.ToOther(vertices2[index22]),
Id =
{
Feature =
{
IndexA = (byte)edge1,
IndexB = (byte)index22,
TypeA = (byte)ContactFeature.FeatureType.Face,
TypeB = (byte)ContactFeature.FeatureType.Vertex
}
}
}
};
}
// End inlined FindIncidentEdge()
var vertices1 = polygon1.Vertices;
var count1 = polygon1.Count;
var index11 = edge1;
var index12 = edge1 + 1 < count1 ? edge1 + 1 : 0;
var vertex11 = vertices1[index11];
var vertex12 = vertices1[index12];
var tangent1 = vertex12 - vertex11;
tangent1.Normalize();
var normal1 = Vector2Util.Cross(ref tangent1, 1);
var planePoint1 = 0.5f * (vertex11 + vertex12);
// Face offset.
var frontOffset = Vector2Util.Dot(ref normal1, ref vertex11);
// Side offsets, extended by polytope skin thickness.
var sideOffset1 = -Vector2Util.Dot(ref tangent1, ref vertex11) + totalRadius;
var sideOffset2 = Vector2Util.Dot(ref tangent1, ref vertex12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
FixedArray2 <ClipVertex> clipPoints1, clipPoints2;
// Clip to box side 1
var np = ClipSegmentToLine(out clipPoints1, incidentEdge, -tangent1, sideOffset1, index11);
if (np < 2)
{
return(false);
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, clipPoints1, tangent1, sideOffset2, index12);
if (np < 2)
{
return(false);
}
// Now clipPoints2 contains the clipped points.
manifold.LocalNormal = normal1;
manifold.LocalPoint = planePoint1;
var pointCount = 0;
for (var i = 0; i < 2; ++i)
{
//if (Vector2Util.Dot(normal1g, clipPoints2[i].Vertex) - frontOffset <= totalRadius)
if (Vector2Util.Dot(normal1, clipPoints2[i].Vertex) - frontOffset <= totalRadius)
{
var cp = manifold.Points[pointCount];
//cp.localPoint = transform2.ToLocal(clipPoints2[i].Vertex);
cp.LocalPoint = transform21.FromOther(clipPoints2[i].Vertex);
cp.Id = clipPoints2[i].Id;
if (flip)
{
// Swap features
var cf = cp.Id.Feature;
cp.Id.Feature.IndexA = cf.IndexB;
cp.Id.Feature.IndexB = cf.IndexA;
cp.Id.Feature.TypeA = cf.TypeB;
cp.Id.Feature.TypeB = cf.TypeA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
return(pointCount > 0);
}