static void FindIncidentEdge(ClipVertex[] c,
Box2dShape poly1, ref IndexedMatrix xf1, int edge1,
Box2dShape poly2, ref IndexedMatrix xf2)
{
IndexedVector3[] normals1 = poly1.GetNormals();
int count2 = poly2.GetVertexCount();
IndexedVector3[] vertices2 = poly2.GetVertices();
IndexedVector3[] normals2 = poly2.GetNormals();
Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount());
// Get the normal of the reference edge in poly2's frame.
IndexedVector3 normal1 = xf2._basis.Transpose() * (xf1._basis * normals1[edge1]);
// Find the incident edge on poly2.
int index = 0;
float minDot = MathUtil.BT_LARGE_FLOAT;
for (int i = 0; i < count2; ++i)
{
float dot = normal1.Dot(normals2[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;
c[0].v = xf2 * vertices2[i1];
// c[0].id.features.referenceEdge = (unsigned char)edge1;
// c[0].id.features.incidentEdge = (unsigned char)i1;
// c[0].id.features.incidentVertex = 0;
c[1].v = xf2 * vertices2[i2];
// c[1].id.features.referenceEdge = (unsigned char)edge1;
// c[1].id.features.incidentEdge = (unsigned char)i2;
// c[1].id.features.incidentVertex = 1;
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject col0 = body0;
CollisionObject col1 = body1;
Box2dShape box0 = (Box2dShape)col0.GetCollisionShape();
Box2dShape box1 = (Box2dShape)col1.GetCollisionShape();
resultOut.SetPersistentManifold(m_manifoldPtr);
B2CollidePolygons(ref resultOut, box0, col0.GetWorldTransform(), box1, col1.GetWorldTransform());
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
// Find the separation between poly1 and poly2 for a give edge normal on poly1.
static float EdgeSeparation(Box2dShape poly1, ref IndexedMatrix xf1, int edge1,
Box2dShape poly2, ref IndexedMatrix xf2)
{
IndexedVector3[] vertices1 = poly1.GetVertices();
IndexedVector3[] normals1 = poly1.GetNormals();
int count2 = poly2.GetVertexCount();
IndexedVector3[] vertices2 = poly2.GetVertices();
Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount());
// Convert normal from poly1's frame into poly2's frame.
IndexedVector3 normal1World = xf1._basis * normals1[edge1];
IndexedVector3 normal1 = xf1._basis.Transpose() * normal1World;
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = MathUtil.BT_LARGE_FLOAT;
for (int i = 0; i < count2; ++i)
{
float dot = vertices2[i].Dot(normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
IndexedVector3 v1 = xf1 * vertices1[edge1];
IndexedVector3 v2 = xf2 * vertices2[index];
float separation = (v2 - v1).Dot(normal1World);
return(separation);
}
static void FindIncidentEdge(ClipVertex[] c,
Box2dShape poly1, ref IndexedMatrix xf1, int edge1,
Box2dShape poly2, ref IndexedMatrix xf2)
{
IndexedVector3[] normals1 = poly1.GetNormals();
int count2 = poly2.GetVertexCount();
IndexedVector3[] vertices2 = poly2.GetVertices();
IndexedVector3[] normals2 = poly2.GetNormals();
Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount());
// Get the normal of the reference edge in poly2's frame.
IndexedVector3 normal1 = xf2._basis.Transpose() * (xf1._basis * normals1[edge1]);
// Find the incident edge on poly2.
int index = 0;
float minDot = MathUtil.BT_LARGE_FLOAT;
for (int i = 0; i < count2; ++i)
{
float dot = normal1.Dot(normals2[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;
c[0].v = xf2 * vertices2[i1];
// c[0].id.features.referenceEdge = (unsigned char)edge1;
// c[0].id.features.incidentEdge = (unsigned char)i1;
// c[0].id.features.incidentVertex = 0;
c[1].v = xf2 * vertices2[i2];
// c[1].id.features.referenceEdge = (unsigned char)edge1;
// c[1].id.features.incidentEdge = (unsigned char)i2;
// c[1].id.features.incidentVertex = 1;
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
static float FindMaxSeparation(ref int edgeIndex,
Box2dShape poly1, ref IndexedMatrix xf1,
Box2dShape poly2, ref IndexedMatrix xf2)
{
int count1 = poly1.GetVertexCount();
IndexedVector3[] normals1 = poly1.GetNormals();
// Vector pointing from the centroid of poly1 to the centroid of poly2.
IndexedVector3 d = xf2 * poly2.GetCentroid() - xf1 * poly1.GetCentroid();
IndexedVector3 dLocal1 = xf1._basis.Transpose() * d;
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -MathUtil.BT_LARGE_FLOAT;
for (int i = 0; i < count1; ++i)
{
float dot = normals1[i].Dot(ref 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);
if (s > 0.0f)
{
return s;
}
// 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);
if (sPrev > 0.0f)
{
return sPrev;
}
// 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);
if (sNext > 0.0f)
{
return sNext;
}
// 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 > 0.0f)
{
return s;
}
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return bestSeparation;
}
// Find the separation between poly1 and poly2 for a give edge normal on poly1.
static float EdgeSeparation(Box2dShape poly1, ref IndexedMatrix xf1, int edge1,
Box2dShape poly2, ref IndexedMatrix xf2)
{
IndexedVector3[] vertices1 = poly1.GetVertices();
IndexedVector3[] normals1 = poly1.GetNormals();
int count2 = poly2.GetVertexCount();
IndexedVector3[] vertices2 = poly2.GetVertices();
Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount());
// Convert normal from poly1's frame into poly2's frame.
IndexedVector3 normal1World = xf1._basis * normals1[edge1];
IndexedVector3 normal1 = xf1._basis.Transpose() * normal1World;
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = MathUtil.BT_LARGE_FLOAT;
for (int i = 0; i < count2; ++i)
{
float dot = vertices2[i].Dot(normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
IndexedVector3 v1 = xf1 * vertices1[edge1];
IndexedVector3 v2 = xf2 * vertices2[index];
float separation = (v2 - v1).Dot(normal1World);
return separation;
}
void B2CollidePolygons(ref ManifoldResult manifold,
Box2dShape polyA, ref IndexedMatrix xfA,
Box2dShape polyB, ref IndexedMatrix xfB)
{
int edgeA = 0;
float separationA = FindMaxSeparation(ref edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > 0.0f)
{
return;
}
int edgeB = 0;
float separationB = FindMaxSeparation(ref edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > 0.0f)
{
return;
}
Box2dShape poly1; // reference poly
Box2dShape poly2; // incident poly
IndexedMatrix xf1, xf2;
int edge1; // reference edge
bool flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
// TODO_ERIN use "radius" of poly for absolute tolerance.
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
flip = false;
}
ClipVertex[] incidentEdge = new ClipVertex[2];
FindIncidentEdge(incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1.GetVertexCount();
IndexedVector3[] vertices1 = poly1.GetVertices();
IndexedVector3 v11 = vertices1[edge1];
IndexedVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
IndexedVector3 dv = v12 - v11;
IndexedVector3 sideNormal = xf1._basis * (v12 - v11);
sideNormal.Normalize();
IndexedVector3 frontNormal = CrossS(ref sideNormal, 1.0f);
v11 = xf1 * v11;
v12 = xf1 * v12;
float frontOffset = frontNormal.Dot(ref v11);
float sideOffset1 = -(sideNormal.Dot(ref v11));
float sideOffset2 = sideNormal.Dot(ref v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1 = new ClipVertex[2];
clipPoints1[0].v = IndexedVector3.Zero;
clipPoints1[1].v = IndexedVector3.Zero;
ClipVertex[] clipPoints2 = new ClipVertex[2];
clipPoints2[0].v = IndexedVector3.Zero;
clipPoints2[1].v = IndexedVector3.Zero;
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
IndexedVector3 manifoldNormal = flip ? -frontNormal : frontNormal;
int pointCount = 0;
for (int i = 0; i < b2_maxManifoldPoints; ++i)
{
float separation = frontNormal.Dot(clipPoints2[i].v) - frontOffset;
if (separation <= 0.0f)
{
//b2ManifoldPoint* cp = manifold.points + pointCount;
//float separation = separation;
//cp.localPoint1 = b2MulT(xfA, clipPoints2[i].v);
//cp.localPoint2 = b2MulT(xfB, clipPoints2[i].v);
manifold.AddContactPoint(-manifoldNormal,clipPoints2[i].v,separation);
// cp.id = clipPoints2[i].id;
// cp.id.features.flip = flip;
++pointCount;
}
}
// manifold.pointCount = pointCount;}
}
// 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
void B2CollidePolygons(ref ManifoldResult manifold,
Box2dShape polyA, IndexedMatrix xfA,
Box2dShape polyB, IndexedMatrix xfB)
{
B2CollidePolygons(ref manifold, polyA, ref xfA, polyB, ref xfB);
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
static float FindMaxSeparation(ref int edgeIndex,
Box2dShape poly1, ref IndexedMatrix xf1,
Box2dShape poly2, ref IndexedMatrix xf2)
{
int count1 = poly1.GetVertexCount();
IndexedVector3[] normals1 = poly1.GetNormals();
// Vector pointing from the centroid of poly1 to the centroid of poly2.
IndexedVector3 d = xf2 * poly2.GetCentroid() - xf1 * poly1.GetCentroid();
IndexedVector3 dLocal1 = xf1._basis.Transpose() * d;
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -MathUtil.BT_LARGE_FLOAT;
for (int i = 0; i < count1; ++i)
{
float dot = normals1[i].Dot(ref 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);
if (s > 0.0f)
{
return(s);
}
// 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);
if (sPrev > 0.0f)
{
return(sPrev);
}
// 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);
if (sNext > 0.0f)
{
return(sNext);
}
// 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 > 0.0f)
{
return(s);
}
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return(bestSeparation);
}
void B2CollidePolygons(ref ManifoldResult manifold,
Box2dShape polyA, ref IndexedMatrix xfA,
Box2dShape polyB, ref IndexedMatrix xfB)
{
int edgeA = 0;
float separationA = FindMaxSeparation(ref edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > 0.0f)
{
return;
}
int edgeB = 0;
float separationB = FindMaxSeparation(ref edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > 0.0f)
{
return;
}
Box2dShape poly1; // reference poly
Box2dShape poly2; // incident poly
IndexedMatrix xf1, xf2;
int edge1; // reference edge
bool flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
// TODO_ERIN use "radius" of poly for absolute tolerance.
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
flip = false;
}
ClipVertex[] incidentEdge = new ClipVertex[2];
FindIncidentEdge(incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1.GetVertexCount();
IndexedVector3[] vertices1 = poly1.GetVertices();
IndexedVector3 v11 = vertices1[edge1];
IndexedVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
IndexedVector3 dv = v12 - v11;
IndexedVector3 sideNormal = xf1._basis * (v12 - v11);
sideNormal.Normalize();
IndexedVector3 frontNormal = CrossS(ref sideNormal, 1.0f);
v11 = xf1 * v11;
v12 = xf1 * v12;
float frontOffset = frontNormal.Dot(ref v11);
float sideOffset1 = -(sideNormal.Dot(ref v11));
float sideOffset2 = sideNormal.Dot(ref v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1 = new ClipVertex[2];
clipPoints1[0].v = IndexedVector3.Zero;
clipPoints1[1].v = IndexedVector3.Zero;
ClipVertex[] clipPoints2 = new ClipVertex[2];
clipPoints2[0].v = IndexedVector3.Zero;
clipPoints2[1].v = IndexedVector3.Zero;
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
IndexedVector3 manifoldNormal = flip ? -frontNormal : frontNormal;
int pointCount = 0;
for (int i = 0; i < b2_maxManifoldPoints; ++i)
{
float separation = frontNormal.Dot(clipPoints2[i].v) - frontOffset;
if (separation <= 0.0f)
{
//b2ManifoldPoint* cp = manifold.points + pointCount;
//float separation = separation;
//cp.localPoint1 = b2MulT(xfA, clipPoints2[i].v);
//cp.localPoint2 = b2MulT(xfB, clipPoints2[i].v);
manifold.AddContactPoint(-manifoldNormal, clipPoints2[i].v, separation);
// cp.id = clipPoints2[i].id;
// cp.id.features.flip = flip;
++pointCount;
}
}
// manifold.pointCount = pointCount;}
}
// 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
void B2CollidePolygons(ref ManifoldResult manifold,
Box2dShape polyA, IndexedMatrix xfA,
Box2dShape polyB, IndexedMatrix xfB)
{
B2CollidePolygons(ref manifold, polyA, ref xfA, polyB, ref xfB);
}
