/// <summary> /// Initializes a new instance of the <see cref="Manifold" /> class /// </summary> public Manifold() { for (int i = 0; i < Settings.MaxManifoldPoints; i++) { Points[i] = new ManifoldPoint(); } }
/// <summary> /// Clones this instance /// </summary> /// <returns>The new point</returns> public ManifoldPoint Clone() { ManifoldPoint newPoint = new ManifoldPoint(); newPoint.LocalPoint = LocalPoint; newPoint.NormalImpulse = NormalImpulse; newPoint.TangentImpulse = TangentImpulse; newPoint.Id = Id; return(newPoint); }
/// <summary> /// Clones this instance /// </summary> /// <returns>The new manifold</returns> public Manifold Clone() { Manifold newManifold = new Manifold(); newManifold.LocalPlaneNormal = LocalPlaneNormal; newManifold.LocalPoint = LocalPoint; newManifold.Type = Type; newManifold.PointCount = PointCount; int pointCount = Points.Length; ManifoldPoint[] tmp = new ManifoldPoint[pointCount]; for (int i = 0; i < pointCount; i++) { tmp[i] = Points[i].Clone(); } newManifold.Points = tmp; return(newManifold); }
// 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 /// <summary> /// Collides the polygons using the specified manifold /// </summary> /// <param name="manifold">The manifold</param> /// <param name="polyA">The poly</param> /// <param name="xfA">The xf</param> /// <param name="polyB">The poly</param> /// <param name="xfB">The xf</param> public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB) { manifold.PointCount = 0; float totalRadius = polyA.Radius + polyB.Radius; int edgeA = 0; float separationA = FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB); if (separationA > totalRadius) { return; } int edgeB = 0; float separationB = FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA); if (separationB > totalRadius) { return; } PolygonShape poly1; // reference poly PolygonShape poly2; // incident poly XForm xf1, xf2; int edge1; // reference edge byte flip; const float kRelativeTol = 0.98f; const float kAbsoluteTol = 0.001f; if (separationB > kRelativeTol * separationA + kAbsoluteTol) { 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; FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2); int count1 = poly1.VertexCount; Vec2[] vertices1 = poly1.Vertices; Vec2 v11 = vertices1[edge1]; Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]; Vec2 dv = v12 - v11; Vec2 localNormal = Vec2.Cross(dv, 1.0f); localNormal.Normalize(); Vec2 planePoint = 0.5f * (v11 + v12); Vec2 sideNormal = Math.Mul(xf1.R, v12 - v11); sideNormal.Normalize(); Vec2 frontNormal = Vec2.Cross(sideNormal, 1.0f); v11 = Math.Mul(xf1, v11); v12 = 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 = 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 = Vec2.Dot(frontNormal, clipPoints2[i].V) - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold.Points[pointCount]; cp.LocalPoint = Math.MulT(xf2, clipPoints2[i].V); cp.Id = clipPoints2[i].Id; cp.Id.Features.Flip = flip; ++pointCount; } } manifold.PointCount = pointCount; }