static void FindIncidentEdge(ClipVertex[] c, Box2dShape poly1, ref Matrix xf1, int edge1, Box2dShape poly2, ref Matrix xf2) { Vector3[] normals1 = poly1.GetNormals(); int count2 = poly2.GetVertexCount(); Vector3[] vertices2 = poly2.GetVertices(); Vector3[] normals2 = poly2.GetNormals(); Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount()); // Get the normal of the reference edge in poly2's frame. Vector3 normal1 = Vector3.TransformNormal(Vector3.TransformNormal(normals1[edge1],xf1),MathUtil.TransposeBasis(xf2)); // Find the incident edge on poly2. int index = 0; float minDot = MathUtil.BT_LARGE_FLOAT; for (int i = 0; i < count2; ++i) { float dot = Vector3.Dot(normal1, 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 = Vector3.Transform(vertices2[i1],xf2); // 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 = Vector3.Transform(vertices2[i2],xf2); // 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 Matrix xf1, Box2dShape poly2, ref Matrix xf2) { int count1 = poly1.GetVertexCount(); Vector3[] normals1 = poly1.GetNormals(); // Vector pointing from the centroid of poly1 to the centroid of poly2. Vector3 d = Vector3.Transform(poly2.GetCentroid(),xf2) - Vector3.Transform(poly1.GetCentroid(),xf1); Vector3 dLocal1 = Vector3.TransformNormal(d, MathUtil.TransposeBasis(xf1)); // 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 = Vector3.Dot(normals1[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); 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 Matrix xfA, Box2dShape polyB, ref Matrix 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 Matrix 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(); Vector3[] vertices1 = poly1.GetVertices(); Vector3 v11 = vertices1[edge1]; Vector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0]; Vector3 dv = v12 - v11; Vector3 sideNormal = Vector3.TransformNormal( v12 - v11,xf1); sideNormal.Normalize(); Vector3 frontNormal = CrossS(ref sideNormal, 1.0f); v11 = Vector3.Transform(v11,xf1); v12 = Vector3.Transform(v12,xf1); float frontOffset = Vector3.Dot(frontNormal, v11); float sideOffset1 = -Vector3.Dot(sideNormal, v11); float sideOffset2 = Vector3.Dot(sideNormal, v12); // Clip incident edge against extruded edge1 side edges. ClipVertex[] clipPoints1 = new ClipVertex[2]; clipPoints1[0].v = Vector3.Zero; clipPoints1[1].v = Vector3.Zero; ClipVertex[] clipPoints2 = new ClipVertex[2]; clipPoints2[0].v = Vector3.Zero; clipPoints2[1].v = Vector3.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. Vector3 manifoldNormal = flip ? -frontNormal : frontNormal; int pointCount = 0; for (int i = 0; i < b2_maxManifoldPoints; ++i) { float separation = Vector3.Dot(frontNormal, 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 the separation between poly1 and poly2 for a give edge normal on poly1. static float EdgeSeparation(Box2dShape poly1, ref Matrix xf1, int edge1, Box2dShape poly2, ref Matrix xf2) { Vector3[] vertices1 = poly1.GetVertices(); Vector3[] normals1 = poly1.GetNormals(); int count2 = poly2.GetVertexCount(); Vector3[] vertices2 = poly2.GetVertices(); Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount()); // Convert normal from poly1's frame into poly2's frame. Vector3 normal1World = Vector3.TransformNormal(normals1[edge1],xf1); Vector3 normal1 = Vector3.TransformNormal(normal1World,MathUtil.TransposeBasis(xf2)); // 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 = Vector3.Dot(vertices2[i], normal1); if (dot < minDot) { minDot = dot; index = i; } } Vector3 v1 = Vector3.Transform(vertices1[edge1],xf1); Vector3 v2 = Vector3.Transform(vertices2[index],xf2); float separation = Vector3.Dot(v2 - v1, normal1World); return separation; }
// 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, Matrix xfA, Box2dShape polyB, Matrix xfB) { B2CollidePolygons(ref manifold, polyA, ref xfA, polyB, ref xfB); }