// 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 bool CollidePolygons( Fixture fixtureA, WorldTransform transformA, Fixture fixtureB, WorldTransform transformB, out Manifold manifold) { manifold = new Manifold(); var polygonA = fixtureA as PolygonFixture; var polygonB = fixtureB as PolygonFixture; System.Diagnostics.Debug.Assert(polygonA != null); System.Diagnostics.Debug.Assert(polygonB != null); var totalRadius = polygonA.Radius + polygonB.Radius; int edgeA; var separationA = FindMaxSeparation(out edgeA, polygonA, transformA, polygonB, transformB); if (separationA > totalRadius) { return(false); } int edgeB; var separationB = FindMaxSeparation(out edgeB, polygonB, transformB, polygonA, transformA); if (separationB > totalRadius) { return(false); } PolygonFixture polygon1; // reference polygon PolygonFixture polygon2; // incident polygon WorldTransform transform1, transform2; int edge1; // reference edge bool flip; const float relativeTol = 0.98f; const float absoluteTol = 0.001f; if (separationB > relativeTol * separationA + absoluteTol) { polygon1 = polygonB; polygon2 = polygonA; transform1 = transformB; transform2 = transformA; edge1 = edgeB; manifold.Type = Manifold.ManifoldType.FaceB; flip = true; } else { polygon1 = polygonA; polygon2 = polygonB; transform1 = transformA; transform2 = transformB; edge1 = edgeA; manifold.Type = Manifold.ManifoldType.FaceA; flip = false; } // Transformation mapping from the second polygon's frame of reference to // first one's. We use this to directly map points around, without getting // into the global coordinate system. var transform21 = transform1.MulT(transform2); // Begin inlined FindIncidentEdge() FixedArray2 <ClipVertex> incidentEdge; { System.Diagnostics.Debug.Assert(0 <= edge1 && edge1 < polygon1.Count); var normals1 = polygon1.Normals; var vertices2 = polygon2.Vertices; var normals2 = polygon2.Normals; var count2 = polygon2.Count; // Get the normal of the reference edge in poly2's frame. var normal12 = -transform2.Rotation * (transform1.Rotation * normals1[edge1]); // Find the incident edge on poly2 by finding the clip vertices // for the incident edge. // Get the face whose own normal has the smallest angular // difference to the incident normal. var edge2 = 0; var minDot = float.MaxValue; for (var i = 0; i < count2; ++i) { var dot = Vector2Util.Dot(ref normal12, ref normals2[i]); if (dot < minDot) { minDot = dot; edge2 = i; } } // The edge's index coincides with the first vertex used to define // that edge, so we can use that and wrap around as necessary for the // second one. var index21 = edge2; var index22 = index21 + 1 < count2 ? index21 + 1 : 0; // Get the incident edge as defined by its two vertices, in the first // polygon's frame of reference. incidentEdge = new FixedArray2 <ClipVertex> { Item1 = new ClipVertex { //Vertex = xf2.ToGlobal(vertices2[i1]), Vertex = transform21.ToOther(vertices2[index21]), Id = { Feature = { IndexA = (byte)edge1, IndexB = (byte)index21, TypeA = (byte)ContactFeature.FeatureType.Face, TypeB = (byte)ContactFeature.FeatureType.Vertex } } }, Item2 = new ClipVertex { //Vertex = xf2.ToGlobal(vertices2[i2]), Vertex = transform21.ToOther(vertices2[index22]), Id = { Feature = { IndexA = (byte)edge1, IndexB = (byte)index22, TypeA = (byte)ContactFeature.FeatureType.Face, TypeB = (byte)ContactFeature.FeatureType.Vertex } } } }; } // End inlined FindIncidentEdge() var vertices1 = polygon1.Vertices; var count1 = polygon1.Count; var index11 = edge1; var index12 = edge1 + 1 < count1 ? edge1 + 1 : 0; var vertex11 = vertices1[index11]; var vertex12 = vertices1[index12]; var tangent1 = vertex12 - vertex11; tangent1.Normalize(); var normal1 = Vector2Util.Cross(ref tangent1, 1); var planePoint1 = 0.5f * (vertex11 + vertex12); // Face offset. var frontOffset = Vector2Util.Dot(ref normal1, ref vertex11); // Side offsets, extended by polytope skin thickness. var sideOffset1 = -Vector2Util.Dot(ref tangent1, ref vertex11) + totalRadius; var sideOffset2 = Vector2Util.Dot(ref tangent1, ref vertex12) + totalRadius; // Clip incident edge against extruded edge1 side edges. FixedArray2 <ClipVertex> clipPoints1, clipPoints2; // Clip to box side 1 var np = ClipSegmentToLine(out clipPoints1, incidentEdge, -tangent1, sideOffset1, index11); if (np < 2) { return(false); } // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, clipPoints1, tangent1, sideOffset2, index12); if (np < 2) { return(false); } // Now clipPoints2 contains the clipped points. manifold.LocalNormal = normal1; manifold.LocalPoint = planePoint1; var pointCount = 0; for (var i = 0; i < 2; ++i) { //if (Vector2Util.Dot(normal1g, clipPoints2[i].Vertex) - frontOffset <= totalRadius) if (Vector2Util.Dot(normal1, clipPoints2[i].Vertex) - frontOffset <= totalRadius) { var cp = manifold.Points[pointCount]; //cp.localPoint = transform2.ToLocal(clipPoints2[i].Vertex); cp.LocalPoint = transform21.FromOther(clipPoints2[i].Vertex); cp.Id = clipPoints2[i].Id; if (flip) { // Swap features var cf = cp.Id.Feature; cp.Id.Feature.IndexA = cf.IndexB; cp.Id.Feature.IndexB = cf.IndexA; cp.Id.Feature.TypeA = cf.TypeB; cp.Id.Feature.TypeB = cf.TypeA; } manifold.Points[pointCount] = cp; ++pointCount; } } manifold.PointCount = pointCount; return(pointCount > 0); }