/// <summary>Compute the collision manifold between two circles.</summary> public static void CollideCircles(ref Manifold manifold, CircleShape circleA, ref Transform xfA, CircleShape circleB, ref Transform xfB) { manifold.PointCount = 0; Vector2 pA = MathUtils.Mul(ref xfA, circleA.Position); Vector2 pB = MathUtils.Mul(ref xfB, circleB.Position); Vector2 d = pB - pA; float distSqr = Vector2.Dot(d, d); float rA = circleA.RadiusPrivate, rB = circleB.RadiusPrivate; float radius = rA + rB; if (distSqr > radius * radius) { return; } manifold.Type = ManifoldType.Circles; manifold.LocalPoint = circleA.Position; manifold.LocalNormal = Vector2.Zero; manifold.PointCount = 1; ManifoldPoint p0 = manifold.Points[0]; p0.LocalPoint = circleB.Position; p0.Id.Key = 0; manifold.Points[0] = p0; }
/// <summary>Compute the collision manifold between two polygons.</summary> public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform xfA, PolygonShape polyB, ref Transform xfB) { // 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 manifold.PointCount = 0; float totalRadius = polyA.RadiusPrivate + polyB.RadiusPrivate; float separationA = FindMaxSeparation(out int edgeA, polyA, ref xfA, polyB, ref xfB); if (separationA > totalRadius) { return; } float separationB = FindMaxSeparation(out int edgeB, polyB, ref xfB, polyA, ref xfA); if (separationB > totalRadius) { return; } PolygonShape poly1; // reference polygon PolygonShape poly2; // incident polygon Transform xf1, xf2; int edge1; // reference edge bool flip; float kTol = 0.1f * Settings.LinearSlop; if (separationB > separationA + kTol) { poly1 = polyB; poly2 = polyA; xf1 = xfB; xf2 = xfA; edge1 = edgeB; manifold.Type = ManifoldType.FaceB; flip = true; } else { poly1 = polyA; poly2 = polyB; xf1 = xfA; xf2 = xfB; edge1 = edgeA; manifold.Type = ManifoldType.FaceA; flip = false; } FindIncidentEdge(out FixedArray2 <ClipVertex> incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2); int count1 = poly1.VerticesPrivate.Count; Vertices vertices1 = poly1.VerticesPrivate; int iv1 = edge1; int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; Vector2 v11 = vertices1[iv1]; Vector2 v12 = vertices1[iv2]; Vector2 localTangent = v12 - v11; localTangent = Vector2.Normalize(localTangent); Vector2 localNormal = MathUtils.Cross(localTangent, 1.0f); Vector2 planePoint = 0.5f * (v11 + v12); Vector2 tangent = MathUtils.Mul(ref xf1.Q, localTangent); Vector2 normal = MathUtils.Cross(tangent, 1.0f); v11 = MathUtils.Mul(ref xf1, v11); v12 = MathUtils.Mul(ref xf1, v12); // Face offset. float frontOffset = Vector2.Dot(normal, v11); // Side offsets, extended by polytope skin thickness. float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius; float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius; // Clip incident edge against extruded edge1 side edges. // Clip to box side 1 int np = Collision.ClipSegmentToLine(out FixedArray2 <ClipVertex> clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1); if (np < 2) { return; } // Clip to negative box side 1 np = Collision.ClipSegmentToLine(out FixedArray2 <ClipVertex> clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2); if (np < 2) { return; } // Now clipPoints2 contains the clipped points. manifold.LocalNormal = localNormal; manifold.LocalPoint = planePoint; int pointCount = 0; for (int i = 0; i < Settings.MaxManifoldPoints; ++i) { float separation = Vector2.Dot(normal, clipPoints2[i].V) - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold.Points[pointCount]; cp.LocalPoint = MathUtils.MulT(ref xf2, clipPoints2[i].V); cp.Id = clipPoints2[i].Id; if (flip) { // Swap features ContactFeature cf = cp.Id.ContactFeature; cp.Id.ContactFeature.IndexA = cf.IndexB; cp.Id.ContactFeature.IndexB = cf.IndexA; cp.Id.ContactFeature.TypeA = cf.TypeB; cp.Id.ContactFeature.TypeB = cf.TypeA; } manifold.Points[pointCount] = cp; ++pointCount; } } manifold.PointCount = pointCount; }
/// <summary> /// Collides the edge and polygon using the specified manifold /// </summary> /// <param name="manifold">The manifold</param> /// <param name="edgeA">The edge</param> /// <param name="xfA">The xf</param> /// <param name="polygonB">The polygon</param> /// <param name="xfB">The xf</param> public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB) { manifold.PointCount = 0; Transform xf = MathUtils.MulT(xfA, xfB); Vector2 centroidB = MathUtils.Mul(ref xf, polygonB.MassDataPrivate.Centroid); Vector2 v1 = edgeA.Vertex1; Vector2 v2 = edgeA.Vertex2; Vector2 edge1 = v2 - v1; edge1 = Vector2.Normalize(edge1); // Normal points to the right for a CCW winding Vector2 normal1 = new Vector2(edge1.Y, -edge1.X); float offset1 = MathUtils.Dot(normal1, centroidB - v1); bool oneSided = edgeA.OneSided; if (oneSided && offset1 < 0.0f) { return; } // Get polygonB in frameA TempPolygon tempPolygonB = new TempPolygon(polygonB.VerticesPrivate.Count); for (int i = 0; i < polygonB.VerticesPrivate.Count; ++i) { tempPolygonB.Vertices[i] = MathUtils.Mul(ref xf, polygonB.VerticesPrivate[i]); tempPolygonB.Normals[i] = MathUtils.Mul(xf.Q, polygonB.NormalsPrivate[i]); } float radius = polygonB.RadiusPrivate + edgeA.RadiusPrivate; EpAxis edgeAxis = ComputeEdgeSeparation(ref tempPolygonB, v1, normal1); if (edgeAxis.Separation > radius) { return; } EpAxis polygonAxis = ComputePolygonSeparation(ref tempPolygonB, v1, v2); if (polygonAxis.Separation > radius) { return; } // Use hysteresis for jitter reduction. const float kRelativeTol = 0.98f; const float kAbsoluteTol = 0.001f; EpAxis primaryAxis; if (polygonAxis.Separation - radius > kRelativeTol * (edgeAxis.Separation - radius) + kAbsoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } if (oneSided) { // Smooth collision // See https://box2d.org/posts/2020/06/ghost-collisions/ Vector2 edge0 = v1 - edgeA.Vertex0; edge0 = Vector2.Normalize(edge0); Vector2 normal0 = new Vector2(edge0.Y, -edge0.X); bool convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f; Vector2 edge2 = edgeA.Vertex3 - v2; edge2 = Vector2.Normalize(edge2); Vector2 normal2 = new Vector2(edge2.Y, -edge2.X); bool convex2 = MathUtils.Cross(edge1, edge2) >= 0.0f; const float sinTol = 0.1f; bool side1 = MathUtils.Dot(primaryAxis.Normal, edge1) <= 0.0f; // Check Gauss Map if (side1) { if (convex1) { if (MathUtils.Cross(primaryAxis.Normal, normal0) > sinTol) { // Skip region return; } // Admit region } else { // Snap region primaryAxis = edgeAxis; } } else { if (convex2) { if (MathUtils.Cross(normal2, primaryAxis.Normal) > sinTol) { // Skip region return; } // Admit region } else { // Snap region primaryAxis = edgeAxis; } } } FixedArray2 <ClipVertex> clipPoints = new FixedArray2 <ClipVertex>(); ReferenceFace ref1; if (primaryAxis.Type == EpAxisType.EdgeA) { manifold.Type = ManifoldType.FaceA; // Search for the polygon normal that is most anti-parallel to the edge normal. int bestIndex = 0; float bestValue = MathUtils.Dot(primaryAxis.Normal, tempPolygonB.Normals[0]); for (int i = 1; i < tempPolygonB.Count; ++i) { float value = MathUtils.Dot(primaryAxis.Normal, tempPolygonB.Normals[i]); if (value < bestValue) { bestValue = value; bestIndex = i; } } int i1 = bestIndex; int i2 = i1 + 1 < tempPolygonB.Count ? i1 + 1 : 0; clipPoints.Value0.V = tempPolygonB.Vertices[i1]; clipPoints.Value0.Id.ContactFeature.IndexA = 0; clipPoints.Value0.Id.ContactFeature.IndexB = (byte)i1; clipPoints.Value0.Id.ContactFeature.TypeA = ContactFeatureType.Face; clipPoints.Value0.Id.ContactFeature.TypeB = ContactFeatureType.Vertex; clipPoints.Value1.V = tempPolygonB.Vertices[i2]; clipPoints.Value1.Id.ContactFeature.IndexA = 0; clipPoints.Value1.Id.ContactFeature.IndexB = (byte)i2; clipPoints.Value1.Id.ContactFeature.TypeA = ContactFeatureType.Face; clipPoints.Value1.Id.ContactFeature.TypeB = ContactFeatureType.Vertex; ref1.I1 = 0; ref1.I2 = 1; ref1.V1 = v1; ref1.V2 = v2; ref1.Normal = primaryAxis.Normal; ref1.SideNormal1 = -edge1; ref1.SideNormal2 = edge1; } else { manifold.Type = ManifoldType.FaceB; clipPoints.Value0.V = v2; clipPoints.Value0.Id.ContactFeature.IndexA = 1; clipPoints.Value0.Id.ContactFeature.IndexB = (byte)primaryAxis.Index; clipPoints.Value0.Id.ContactFeature.TypeA = ContactFeatureType.Vertex; clipPoints.Value0.Id.ContactFeature.TypeB = ContactFeatureType.Face; clipPoints.Value1.V = v1; clipPoints.Value1.Id.ContactFeature.IndexA = 0; clipPoints.Value1.Id.ContactFeature.IndexB = (byte)primaryAxis.Index; clipPoints.Value1.Id.ContactFeature.TypeA = ContactFeatureType.Vertex; clipPoints.Value1.Id.ContactFeature.TypeB = ContactFeatureType.Face; ref1.I1 = primaryAxis.Index; ref1.I2 = ref1.I1 + 1 < tempPolygonB.Count ? ref1.I1 + 1 : 0; ref1.V1 = tempPolygonB.Vertices[ref1.I1]; ref1.V2 = tempPolygonB.Vertices[ref1.I2]; ref1.Normal = tempPolygonB.Normals[ref1.I1]; // CCW winding ref1.SideNormal1 = new Vector2(ref1.Normal.Y, -ref1.Normal.X); ref1.SideNormal2 = -ref1.SideNormal1; } ref1.SideOffset1 = MathUtils.Dot(ref1.SideNormal1, ref1.V1); ref1.SideOffset2 = MathUtils.Dot(ref1.SideNormal2, ref1.V2); // Clip incident edge against reference face side planes FixedArray2 <ClipVertex> clipPoints1; FixedArray2 <ClipVertex> clipPoints2; int np; // Clip to side 1 np = Collision.ClipSegmentToLine(out clipPoints1, ref clipPoints, ref1.SideNormal1, ref1.SideOffset1, ref1.I1); if (np < Settings.MaxManifoldPoints) { return; } // Clip to side 2 np = Collision.ClipSegmentToLine(out clipPoints2, ref clipPoints1, ref1.SideNormal2, ref1.SideOffset2, ref1.I2); if (np < Settings.MaxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.Type == EpAxisType.EdgeA) { manifold.LocalNormal = ref1.Normal; manifold.LocalPoint = ref1.V1; } else { manifold.LocalNormal = polygonB.NormalsPrivate[ref1.I1]; manifold.LocalPoint = polygonB.VerticesPrivate[ref1.I1]; } int pointCount = 0; for (int i = 0; i < Settings.MaxManifoldPoints; ++i) { float separation = MathUtils.Dot(ref1.Normal, clipPoints2[i].V - ref1.V1); if (separation <= radius) { ManifoldPoint cp = manifold.Points[pointCount]; if (primaryAxis.Type == EpAxisType.EdgeA) { cp.LocalPoint = MathUtils.MulT(xf, clipPoints2[i].V); cp.Id = clipPoints2[i].Id; } else { cp.LocalPoint = clipPoints2[i].V; cp.Id.ContactFeature.TypeA = clipPoints2[i].Id.ContactFeature.TypeB; cp.Id.ContactFeature.TypeB = clipPoints2[i].Id.ContactFeature.TypeA; cp.Id.ContactFeature.IndexA = clipPoints2[i].Id.ContactFeature.IndexB; cp.Id.ContactFeature.IndexB = clipPoints2[i].Id.ContactFeature.IndexA; } manifold.Points[pointCount] = cp; ++pointCount; } } manifold.PointCount = pointCount; }