/// <summary> /// Computes the polygon separation using the specified polygon b /// </summary> /// <param name="polygonB">The polygon</param> /// <param name="v1">The </param> /// <param name="v2">The </param> /// <returns>The axis</returns> private static EpAxis ComputePolygonSeparation(ref TempPolygon polygonB, Vector2 v1, Vector2 v2) { EpAxis axis; axis.Type = EpAxisType.Unknown; axis.Index = -1; axis.Separation = -MathConstants.MaxFloat; axis.Normal = Vector2.Zero; for (int i = 0; i < polygonB.Count; ++i) { Vector2 n = -polygonB.Normals[i]; float s1 = MathUtils.Dot(n, polygonB.Vertices[i] - v1); float s2 = MathUtils.Dot(n, polygonB.Vertices[i] - v2); float s = MathUtils.Min(s1, s2); if (s > axis.Separation) { axis.Type = EpAxisType.EdgeB; axis.Index = i; axis.Separation = s; axis.Normal = n; } } return(axis); }
/// <summary> /// Computes the edge separation using the specified polygon b /// </summary> /// <param name="polygonB">The polygon</param> /// <param name="v1">The </param> /// <param name="normal1">The normal</param> /// <returns>The axis</returns> private static EpAxis ComputeEdgeSeparation(ref TempPolygon polygonB, Vector2 v1, Vector2 normal1) { EpAxis axis; axis.Type = EpAxisType.EdgeA; axis.Index = -1; axis.Separation = -MathConstants.MaxFloat; axis.Normal = Vector2.Zero; Vector2[] axes = { normal1, -normal1 }; // Find axis with least overlap (min-max problem) for (int j = 0; j < 2; ++j) { float sj = MathConstants.MaxFloat; // Find deepest polygon vertex along axis j for (int i = 0; i < polygonB.Count; ++i) { float si = MathUtils.Dot(axes[j], polygonB.Vertices[i] - v1); if (si < sj) { sj = si; } } if (sj > axis.Separation) { axis.Index = j; axis.Separation = sj; axis.Normal = axes[j]; } } return(axis); }
/// <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; }