// GJK is more robust with polygon-vs-point than polygon-vs-circle. // So we convert polygon-vs-circle to polygon-vs-point. public static float DistancePC(out Vec2 x1, out Vec2 x2, PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2) { Point point = new Point(); point.p = Common.Math.Mul(xf2, circle.GetLocalPosition()); float distance = DistanceGeneric <PolygonShape, Point>(out x1, out x2, polygon, xf1, point, XForm.Identity); float r = circle.GetRadius() - Settings.ToiSlop; if (distance > r) { distance -= r; Vec2 d = x2 - x1; d.Normalize(); x2 -= r * d; } else { distance = 0.0f; x2 = x1; } return(distance); }
public static float DistanceCC(out Vec2 x1, out Vec2 x2, CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2) { Vec2 p1 = Common.Math.Mul(xf1, circle1.GetLocalPosition()); Vec2 p2 = Common.Math.Mul(xf2, circle2.GetLocalPosition()); Vec2 d = p2 - p1; float dSqr = Vec2.Dot(d, d); float r1 = circle1.GetRadius() - Settings.ToiSlop; float r2 = circle2.GetRadius() - Settings.ToiSlop; float r = r1 + r2; if (dSqr > r * r) { float dLen = d.Normalize(); float distance = dLen - r; x1 = p1 + r1 * d; x2 = p2 - r2 * d; return(distance); } else if (dSqr > Common.Settings.FLT_EPSILON * Common.Settings.FLT_EPSILON) { d.Normalize(); x1 = p1 + r1 * d; x2 = x1; return(0.0f); } x1 = p1; x2 = x1; return(0.0f); }
public static void CollideCircles(ref Manifold manifold, CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2) { manifold.PointCount = 0; Vec2 p1 = Common.Math.Mul(xf1, circle1.GetLocalPosition()); Vec2 p2 = Common.Math.Mul(xf2, circle2.GetLocalPosition()); Vec2 d = p2 - p1; float distSqr = Vec2.Dot(d, d); float r1 = circle1.GetRadius(); float r2 = circle2.GetRadius(); float radiusSum = r1 + r2; if (distSqr > radiusSum * radiusSum) { return; } float separation; if (distSqr < Common.Settings.FLT_EPSILON) { separation = -radiusSum; manifold.Normal.Set(0.0f, 1.0f); } else { float dist = Common.Math.Sqrt(distSqr); separation = dist - radiusSum; float a = 1.0f / dist; manifold.Normal.X = a * d.X; manifold.Normal.Y = a * d.Y; } manifold.PointCount = 1; manifold.Points[0].ID.Key = 0; manifold.Points[0].Separation = separation; p1 += r1 * manifold.Normal; p2 -= r2 * manifold.Normal; Vec2 p = 0.5f * (p1 + p2); manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, p); manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, p); }
public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2) { manifold.PointCount = 0; // Compute circle position in the frame of the polygon. Vec2 c = Common.Math.Mul(xf2, circle.GetLocalPosition()); Vec2 cLocal = Common.Math.MulT(xf1, c); // Find the min separating edge. int normalIndex = 0; float separation = -Settings.FLT_MAX; float radius = circle.GetRadius(); int vertexCount = polygon.VertexCount; Vec2[] vertices = polygon.GetVertices(); Vec2[] normals = polygon.Normals; for (int i = 0; i < vertexCount; ++i) { float s = Vec2.Dot(normals[i], cLocal - vertices[i]); if (s > radius) { // Early out. return; } if (s > separation) { separation = s; normalIndex = i; } } // If the center is inside the polygon ... if (separation < Common.Settings.FLT_EPSILON) { manifold.PointCount = 1; manifold.Normal = Common.Math.Mul(xf1.R, normals[normalIndex]); manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex; manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature; manifold.Points[0].ID.Features.ReferenceEdge = 0; manifold.Points[0].ID.Features.Flip = 0; Vec2 position = c - radius * manifold.Normal; manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position); manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position); manifold.Points[0].Separation = separation - radius; return; } // Project the circle center onto the edge segment. int vertIndex1 = normalIndex; int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; Vec2 e = vertices[vertIndex2] - vertices[vertIndex1]; float length = e.Normalize(); Box2DXDebug.Assert(length > Settings.FLT_EPSILON); // Project the center onto the edge. float u = Vec2.Dot(cLocal - vertices[vertIndex1], e); Vec2 p; if (u <= 0.0f) { p = vertices[vertIndex1]; manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature; manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1; } else if (u >= length) { p = vertices[vertIndex2]; manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature; manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex2; } else { p = vertices[vertIndex1] + u * e; manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex; manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature; } Vec2 d = cLocal - p; float dist = d.Normalize(); if (dist > radius) { return; } manifold.PointCount = 1; manifold.Normal = Common.Math.Mul(xf1.R, d); Vec2 position_ = c - radius * manifold.Normal; manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position_); manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position_); manifold.Points[0].Separation = dist - radius; manifold.Points[0].ID.Features.ReferenceEdge = 0; manifold.Points[0].ID.Features.Flip = 0; }