// 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 void CollidePolygons(ref Manifold manifold, PolygonShape polyA, Transform xfA, PolygonShape polyB, Transform xfB) { manifold.PointCount = 0; float totalRadius = polyA._radius + polyB._radius; int edgeA = 0; float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB); if (separationA > totalRadius) { return; } int edgeB = 0; float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA); if (separationB > totalRadius) { return; } PolygonShape poly1; // reference poly PolygonShape poly2; // incident poly Transform xf1, xf2; int edge1; // reference edge byte flip; const float k_relativeTol = 0.98f; const 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; } ClipVertex[] incidentEdge; Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2); int count1 = poly1._vertexCount; Vector2[] vertices1 = poly1._vertices; Vector2 v11 = vertices1[edge1]; Vector2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]; Vector2 dv = v12 - v11; Vector2 localNormal = dv.CrossScalarPostMultiply(1.0f); localNormal.Normalize(); Vector2 planePoint = 0.5f * (v11 + v12); Vector2 sideNormal = xf1.TransformDirection(v12 - v11); sideNormal.Normalize(); Vector2 frontNormal = sideNormal.CrossScalarPostMultiply(1.0f); v11 = Common.Math.Mul(xf1, v11); v12 = Common.Math.Mul(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. ClipVertex[] clipPoints1; ClipVertex[] clipPoints2; int np; // Clip to box side 1 np = Collision.ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1); if (np < 2) { return; } // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, 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.MaxManifoldPoints; ++i) { float separation = Vector2.Dot(frontNormal, clipPoints2[i].V) - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold.Points[pointCount]; cp.LocalPoint = xf2.InverseTransformPoint(clipPoints2[i].V); 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 public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB) { manifold.PointCount = 0; int edgeA = 0; float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB); if (separationA > 0.0f) { return; } int edgeB = 0; float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA); if (separationB > 0.0f) { return; } PolygonShape poly1; // reference poly PolygonShape poly2; // incident poly XForm xf1, xf2; int edge1; // reference edge byte flip; float k_relativeTol = 0.98f; 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 = 1; } else { poly1 = polyA; poly2 = polyB; xf1 = xfA; xf2 = xfB; edge1 = edgeA; flip = 0; } ClipVertex[] incidentEdge; Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2); int count1 = poly1.VertexCount; Vec2[] vertices1 = poly1.GetVertices(); Vec2 v11 = vertices1[edge1]; Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]; Vec2 dv = v12 - v11; Vec2 sideNormal = Common.Math.Mul(xf1.R, v12 - v11); sideNormal.Normalize(); Vec2 frontNormal = Vec2.Cross(sideNormal, 1.0f); v11 = Common.Math.Mul(xf1, v11); v12 = Common.Math.Mul(xf1, v12); float frontOffset = Vec2.Dot(frontNormal, v11); float sideOffset1 = -Vec2.Dot(sideNormal, v11); float sideOffset2 = Vec2.Dot(sideNormal, v12); // Clip incident edge against extruded edge1 side edges. ClipVertex[] clipPoints1; ClipVertex[] clipPoints2; int np; // Clip to box side 1 np = Collision.ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1); if (np < 2) { return; } // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2); if (np < 2) { return; } // Now clipPoints2 contains the clipped points. manifold.Normal = flip != 0 ? -frontNormal : frontNormal; int pointCount = 0; for (int i = 0; i < Settings.MaxManifoldPoints; ++i) { float separation = Vec2.Dot(frontNormal, clipPoints2[i].V) - frontOffset; if (separation <= 0.0f) { ManifoldPoint cp = manifold.Points[pointCount]; cp.Separation = separation; cp.LocalPoint1 = Box2DX.Common.Math.MulT(xfA, clipPoints2[i].V); cp.LocalPoint2 = Box2DX.Common.Math.MulT(xfB, clipPoints2[i].V); cp.ID = clipPoints2[i].ID; cp.ID.Features.Flip = flip; ++pointCount; } } manifold.PointCount = pointCount; }