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 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); }
// 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); }