/// Implement Shape.
public override Shape Clone()
{
var clone = new PolygonShape();
clone.ShapeType = ShapeType;
clone._radius = _radius;
clone._vertexCount = _vertexCount;
clone._centroid = _centroid;
clone._vertices = _vertices;
clone._normals = _normals;
return clone;
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
static float FindMaxSeparation( out int edgeIndex,
PolygonShape poly1, ref XForm xf1,
PolygonShape poly2, ref XForm xf2)
{
edgeIndex = -1;
int count1 = poly1._vertexCount;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Vector2 d = MathUtils.Multiply(ref xf2, poly2._centroid) - MathUtils.Multiply(ref xf1, poly1._centroid);
Vector2 dLocal1 = MathUtils.MultiplyT(ref xf1.R, d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -Settings.b2_FLT_MAX;
for (int i = 0; i < count1; ++i)
{
float dot = Vector2.Dot(poly1._normals[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2);
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
edgeIndex = edge;
return s;
}
// Perform a local search for the best edge normal.
for ( ; ; )
{
if (increment == -1)
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
else
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return bestSeparation;
}
static void FindIncidentEdge(out FixedArray2<ClipVertex> c,
PolygonShape poly1, ref XForm xf1, int edge1,
PolygonShape poly2, ref XForm xf2)
{
c = new FixedArray2<ClipVertex>();
int count1 = poly1._vertexCount;
int count2 = poly2._vertexCount;
Debug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Vector2 normal1 = MathUtils.MultiplyT(ref xf2.R, MathUtils.Multiply(ref xf1.R, poly1._normals[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.b2_FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(normal1, poly2._normals[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
var cv0 = c[0];
cv0.v = MathUtils.Multiply(ref xf2, poly2._vertices[i1]);
cv0.id.Features.ReferenceEdge = (byte)edge1;
cv0.id.Features.IncidentEdge = (byte)i1;
cv0.id.Features.IncidentVertex = 0;
c[0] = cv0;
var cv1 = c[1];
cv1.v = MathUtils.Multiply(ref xf2, poly2._vertices[i2]);
cv1.id.Features.ReferenceEdge = (byte)edge1;
cv1.id.Features.IncidentEdge = (byte)i2;
cv1.id.Features.IncidentVertex = 1;
c[1] = cv1;
}
// Find the separation between poly1 and poly2 for a give edge normal on poly1.
static float EdgeSeparation(PolygonShape poly1, ref XForm xf1, int edge1,
PolygonShape poly2, ref XForm xf2)
{
int count1 = poly1._vertexCount;
int count2 = poly2._vertexCount;
Debug.Assert(0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
Vector2 normal1World = MathUtils.Multiply(ref xf1.R, poly1._normals[edge1]);
Vector2 normal1 = MathUtils.MultiplyT(ref xf2.R, normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Settings.b2_FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(poly2._vertices[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
Vector2 v1 = MathUtils.Multiply(ref xf1, poly1._vertices[edge1]);
Vector2 v2 = MathUtils.Multiply(ref xf2, poly2._vertices[index]);
float separation = Vector2.Dot(v2 - v1, normal1World);
return separation;
}
/// 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;
}
/// Compute the collision manifold between a polygon and a circle.
public static void CollidePolygonAndCircle(ref Manifold manifold,
PolygonShape polygon, ref XForm xf1,
CircleShape circle, ref XForm xf2)
{
manifold._pointCount = 0;
// Compute circle position in the frame of the polygon.
Vector2 c = MathUtils.Multiply(ref xf2, circle._p);
Vector2 cLocal = MathUtils.MultiplyT(ref xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.b2_FLT_MAX;
float radius = polygon._radius + circle._radius;
int vertexCount = polygon._vertexCount;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vector2.Dot(polygon._normals[i], cLocal - polygon._vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vector2 v1 = polygon._vertices[vertIndex1];
Vector2 v2 = polygon._vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Settings.b2_FLT_EPSILON)
{
manifold._pointCount = 1;
manifold._type = ManifoldType.FaceA;
manifold._localPlaneNormal = polygon._normals[normalIndex];
manifold._localPoint = 0.5f * (v1 + v2);
var p0 = manifold._points[0];
p0.LocalPoint = circle._p;
p0.Id.Key = 0;
manifold._points[0] = p0;
return;
}
// Compute barycentric coordinates
float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (Vector2.DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold._pointCount = 1;
manifold._type = ManifoldType.FaceA;
manifold._localPlaneNormal = cLocal - v1;
manifold._localPlaneNormal.Normalize();
manifold._localPoint = v1;
var p0b = manifold._points[0];
p0b.LocalPoint = circle._p;
p0b.Id.Key = 0;
manifold._points[0] = p0b;
}
else if (u2 <= 0.0f)
{
if (Vector2.DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold._pointCount = 1;
manifold._type = ManifoldType.FaceA;
manifold._localPlaneNormal = cLocal - v2;
manifold._localPlaneNormal.Normalize();
manifold._localPoint = v2;
var p0c = manifold._points[0];
p0c.LocalPoint = circle._p;
p0c.Id.Key = 0;
manifold._points[0] = p0c;
}
else
{
Vector2 faceCenter = 0.5f * (v1 + v2);
float separation2 = Vector2.Dot(cLocal - faceCenter, polygon._normals[vertIndex1]);
if (separation2 > radius)
{
return;
}
manifold._pointCount = 1;
manifold._type = ManifoldType.FaceA;
manifold._localPlaneNormal = polygon._normals[vertIndex1];
manifold._localPoint = faceCenter;
var p0d = manifold._points[0];
p0d.LocalPoint = circle._p;
p0d.Id.Key = 0;
manifold._points[0] = p0d;
}
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
static float FindMaxSeparation(out int edgeIndex,
PolygonShape poly1, ref XForm xf1,
PolygonShape poly2, ref XForm xf2)
{
edgeIndex = -1;
int count1 = poly1._vertexCount;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Vector2 d = MathUtils.Multiply(ref xf2, poly2._centroid) - MathUtils.Multiply(ref xf1, poly1._centroid);
Vector2 dLocal1 = MathUtils.MultiplyT(ref xf1.R, d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -Settings.b2_FLT_MAX;
for (int i = 0; i < count1; ++i)
{
float dot = Vector2.Dot(poly1._normals[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2);
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
edgeIndex = edge;
return(s);
}
// Perform a local search for the best edge normal.
for ( ; ;)
{
if (increment == -1)
{
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
}
else
{
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
}
s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return(bestSeparation);
}
/// 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;
}
/// Compute the collision manifold between a polygon and a circle.
public static void CollidePolygonAndCircle(ref Manifold manifold,
PolygonShape polygon, ref XForm xf1,
CircleShape circle, ref XForm xf2)
{
manifold._pointCount = 0;
// Compute circle position in the frame of the polygon.
Vector2 c = MathUtils.Multiply(ref xf2, circle._p);
Vector2 cLocal = MathUtils.MultiplyT(ref xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.b2_FLT_MAX;
float radius = polygon._radius + circle._radius;
int vertexCount = polygon._vertexCount;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vector2.Dot(polygon._normals[i], cLocal - polygon._vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vector2 v1 = polygon._vertices[vertIndex1];
Vector2 v2 = polygon._vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Settings.b2_FLT_EPSILON)
{
manifold._pointCount = 1;
manifold._type = ManifoldType.FaceA;
manifold._localPlaneNormal = polygon._normals[normalIndex];
manifold._localPoint = 0.5f * (v1 + v2);
var p0 = manifold._points[0];
p0.LocalPoint = circle._p;
p0.Id.Key = 0;
manifold._points[0] = p0;
return;
}
// Compute barycentric coordinates
float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (Vector2.DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold._pointCount = 1;
manifold._type = ManifoldType.FaceA;
manifold._localPlaneNormal = cLocal - v1;
manifold._localPlaneNormal.Normalize();
manifold._localPoint = v1;
var p0b = manifold._points[0];
p0b.LocalPoint = circle._p;
p0b.Id.Key = 0;
manifold._points[0] = p0b;
}
else if (u2 <= 0.0f)
{
if (Vector2.DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold._pointCount = 1;
manifold._type = ManifoldType.FaceA;
manifold._localPlaneNormal = cLocal - v2;
manifold._localPlaneNormal.Normalize();
manifold._localPoint = v2;
var p0c = manifold._points[0];
p0c.LocalPoint = circle._p;
p0c.Id.Key = 0;
manifold._points[0] = p0c;
}
else
{
Vector2 faceCenter = 0.5f * (v1 + v2);
float separation2 = Vector2.Dot(cLocal - faceCenter, polygon._normals[vertIndex1]);
if (separation2 > radius)
{
return;
}
manifold._pointCount = 1;
manifold._type = ManifoldType.FaceA;
manifold._localPlaneNormal = polygon._normals[vertIndex1];
manifold._localPoint = faceCenter;
var p0d = manifold._points[0];
p0d.LocalPoint = circle._p;
p0d.Id.Key = 0;
manifold._points[0] = p0d;
}
}
