public void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB) { // Algorithm: // 1. Classify v1 and v2 // 2. Classify polygon centroid as front or back // 3. Flip normal if necessary // 4. Initialize normal range to [-pi, pi] about face normal // 5. Adjust normal range according to adjacent edges // 6. Visit each separating axes, only accept axes within the range // 7. Return if _any_ axis indicates separation // 8. Clip _xf = MathUtils.MulT(xfA, xfB); _centroidB = MathUtils.Mul(ref _xf, polygonB.MassData.Centroid); _v0 = edgeA.Vertex0; _v1 = edgeA._vertex1; _v2 = edgeA._vertex2; _v3 = edgeA.Vertex3; bool hasVertex0 = edgeA.HasVertex0; bool hasVertex3 = edgeA.HasVertex3; Vector2 edge1 = _v2 - _v1; edge1.Normalize(); _normal1 = new Vector2(edge1.Y, -edge1.X); float offset1 = Vector2.Dot(_normal1, _centroidB - _v1); float offset0 = 0.0f, offset2 = 0.0f; bool convex1 = false, convex2 = false; // Is there a preceding edge? if (hasVertex0) { Vector2 edge0 = _v1 - _v0; edge0.Normalize(); _normal0 = new Vector2(edge0.Y, -edge0.X); convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f; offset0 = Vector2.Dot(_normal0, _centroidB - _v0); } // Is there a following edge? if (hasVertex3) { Vector2 edge2 = _v3 - _v2; edge2.Normalize(); _normal2 = new Vector2(edge2.Y, -edge2.X); convex2 = MathUtils.Cross(edge1, edge2) > 0.0f; offset2 = Vector2.Dot(_normal2, _centroidB - _v2); } // Determine front or back collision. Determine collision normal limits. if (hasVertex0 && hasVertex3) { if (convex1 && convex2) { _front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = _normal0; _upperLimit = _normal2; } else { _normal = -_normal1; _lowerLimit = -_normal1; _upperLimit = -_normal1; } } else if (convex1) { _front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); if (_front) { _normal = _normal1; _lowerLimit = _normal0; _upperLimit = _normal1; } else { _normal = -_normal1; _lowerLimit = -_normal2; _upperLimit = -_normal1; } } else if (convex2) { _front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); if (_front) { _normal = _normal1; _lowerLimit = _normal1; _upperLimit = _normal2; } else { _normal = -_normal1; _lowerLimit = -_normal1; _upperLimit = -_normal0; } } else { _front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = _normal1; _upperLimit = _normal1; } else { _normal = -_normal1; _lowerLimit = -_normal2; _upperLimit = -_normal0; } } } else if (hasVertex0) { if (convex1) { _front = offset0 >= 0.0f || offset1 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = _normal0; _upperLimit = -_normal1; } else { _normal = -_normal1; _lowerLimit = _normal1; _upperLimit = -_normal1; } } else { _front = offset0 >= 0.0f && offset1 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = _normal1; _upperLimit = -_normal1; } else { _normal = -_normal1; _lowerLimit = _normal1; _upperLimit = -_normal0; } } } else if (hasVertex3) { if (convex2) { _front = offset1 >= 0.0f || offset2 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = -_normal1; _upperLimit = _normal2; } else { _normal = -_normal1; _lowerLimit = -_normal1; _upperLimit = _normal1; } } else { _front = offset1 >= 0.0f && offset2 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = -_normal1; _upperLimit = _normal1; } else { _normal = -_normal1; _lowerLimit = -_normal2; _upperLimit = _normal1; } } } else { _front = offset1 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = -_normal1; _upperLimit = -_normal1; } else { _normal = -_normal1; _lowerLimit = _normal1; _upperLimit = _normal1; } } // Get polygonB in frameA _polygonB.Count = polygonB.Vertices.Count; for (int i = 0; i < polygonB.Vertices.Count; ++i) { _polygonB.Vertices[i] = MathUtils.Mul(ref _xf, polygonB.Vertices[i]); _polygonB.Normals[i] = MathUtils.Mul(_xf.q, polygonB.Normals[i]); } _radius = polygonB.Radius + edgeA.Radius; manifold.PointCount = 0; EPAxis edgeAxis = ComputeEdgeSeparation(); // If no valid normal can be found than this edge should not collide. if (edgeAxis.Type == EPAxisType.Unknown) { return; } if (edgeAxis.Separation > _radius) { return; } EPAxis polygonAxis = ComputePolygonSeparation(); if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius) { return; } // Use hysteresis for jitter reduction. const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; EPAxis primaryAxis; if (polygonAxis.Type == EPAxisType.Unknown) { primaryAxis = edgeAxis; } else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } FixedArray2 <ClipVertex> ie = new FixedArray2 <ClipVertex>(); ReferenceFace rf; 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 = Vector2.Dot(_normal, _polygonB.Normals[0]); for (int i = 1; i < _polygonB.Count; ++i) { float value = Vector2.Dot(_normal, _polygonB.Normals[i]); if (value < bestValue) { bestValue = value; bestIndex = i; } } int i1 = bestIndex; int i2 = i1 + 1 < _polygonB.Count ? i1 + 1 : 0; ie.Value0.V = _polygonB.Vertices[i1]; ie.Value0.ID.ContactFeature.IndexA = 0; ie.Value0.ID.ContactFeature.IndexB = (byte)i1; ie.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Face; ie.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Vertex; ie.Value1.V = _polygonB.Vertices[i2]; ie.Value1.ID.ContactFeature.IndexA = 0; ie.Value1.ID.ContactFeature.IndexB = (byte)i2; ie.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Face; ie.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Vertex; if (_front) { rf.i1 = 0; rf.i2 = 1; rf.v1 = _v1; rf.v2 = _v2; rf.Normal = _normal1; } else { rf.i1 = 1; rf.i2 = 0; rf.v1 = _v2; rf.v2 = _v1; rf.Normal = -_normal1; } } else { manifold.Type = ManifoldType.FaceB; ie.Value0.V = _v1; ie.Value0.ID.ContactFeature.IndexA = 0; ie.Value0.ID.ContactFeature.IndexB = (byte)primaryAxis.Index; ie.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Vertex; ie.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Face; ie.Value1.V = _v2; ie.Value1.ID.ContactFeature.IndexA = 0; ie.Value1.ID.ContactFeature.IndexB = (byte)primaryAxis.Index; ie.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Vertex; ie.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Face; rf.i1 = primaryAxis.Index; rf.i2 = rf.i1 + 1 < _polygonB.Count ? rf.i1 + 1 : 0; rf.v1 = _polygonB.Vertices[rf.i1]; rf.v2 = _polygonB.Vertices[rf.i2]; rf.Normal = _polygonB.Normals[rf.i1]; } rf.SideNormal1 = new Vector2(rf.Normal.Y, -rf.Normal.X); rf.SideNormal2 = -rf.SideNormal1; rf.SideOffset1 = Vector2.Dot(rf.SideNormal1, rf.v1); rf.SideOffset2 = Vector2.Dot(rf.SideNormal2, rf.v2); // Clip incident edge against extruded edge1 side edges. FixedArray2 <ClipVertex> clipPoints1; FixedArray2 <ClipVertex> clipPoints2; int np; // Clip to box side 1 np = Collision.ClipSegmentToLine(out clipPoints1, ref ie, rf.SideNormal1, rf.SideOffset1, rf.i1); if (np < Settings.MaxManifoldPoints) { return; } // Clip to negative box side 1 np = Collision.ClipSegmentToLine(out clipPoints2, ref clipPoints1, rf.SideNormal2, rf.SideOffset2, rf.i2); if (np < Settings.MaxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.Type == EPAxisType.EdgeA) { manifold.LocalNormal = rf.Normal; manifold.LocalPoint = rf.v1; } else { manifold.LocalNormal = polygonB.Normals[rf.i1]; manifold.LocalPoint = polygonB.Vertices[rf.i1]; } int pointCount = 0; for (int i = 0; i < Settings.MaxManifoldPoints; ++i) { float separation = Vector2.Dot(rf.Normal, clipPoints2[i].V - rf.v1); if (separation <= _radius) { ManifoldPoint cp = manifold.Points[pointCount]; if (primaryAxis.Type == EPAxisType.EdgeA) { cp.LocalPoint = MathUtils.MulT(ref _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; }
/// <summary> /// Evaluate the manifold with supplied transforms. This assumes /// modest motion from the original state. This does not change the /// point count, impulses, etc. The radii must come from the Shapes /// that generated the manifold. /// </summary> public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB, float radiusB, out Vector2 normal, out FixedArray2 <Vector2> points, out FixedArray2 <float> separations) { normal = Vector2.Zero; points = new FixedArray2 <Vector2>(); separations = new FixedArray2 <float>(); if (manifold.PointCount == 0) { return; } switch (manifold.Type) { case ManifoldType.Circles: { normal = new Vector2(1.0f, 0.0f); Vector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint); Vector2 pointB = MathUtils.Mul(ref xfB, manifold.Points.Value0.LocalPoint); if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon) { normal = pointB - pointA; normal.Normalize(); } Vector2 cA = pointA + radiusA * normal; Vector2 cB = pointB - radiusB * normal; points.Value0 = 0.5f * (cA + cB); separations.Value0 = Vector2.Dot(cB - cA, normal); } break; case ManifoldType.FaceA: { normal = MathUtils.Mul(xfA.q, manifold.LocalNormal); Vector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint); for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint); Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal; Vector2 cB = clipPoint - radiusB * normal; points[i] = 0.5f * (cA + cB); separations[i] = Vector2.Dot(cB - cA, normal); } } break; case ManifoldType.FaceB: { normal = MathUtils.Mul(xfB.q, manifold.LocalNormal); Vector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint); for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint); Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal; Vector2 cA = clipPoint - radiusA * normal; points[i] = 0.5f * (cA + cB); separations[i] = Vector2.Dot(cA - cB, normal); } // Ensure normal points from A to B. normal = -normal; } break; } }