/// <summary> /// Collides the polygon and circle using the specified manifold /// </summary> /// <param name="manifold">The manifold</param> /// <param name="polygon">The polygon</param> /// <param name="xf1">The xf</param> /// <param name="circle">The circle</param> /// <param name="xf2">The xf</param> 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 = Math.Mul(xf2, circle.Position); Vec2 cLocal = Math.MulT(xf1, c); // Find the min separating edge. int normalIndex = 0; float separation = -Settings.FltMax; float radius = polygon.Radius + circle.Radius; int vertexCount = polygon.VertexCount; Vec2[] vertices = polygon.Vertices; 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; } } // Vertices that subtend the incident face. int vertIndex1 = normalIndex; int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; Vec2 v1 = vertices[vertIndex1]; Vec2 v2 = vertices[vertIndex2]; // If the center is inside the polygon ... if (separation < Settings.FltEpsilon) { manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalPlaneNormal = normals[normalIndex]; manifold.LocalPoint = 0.5f * (v1 + v2); manifold.Points[0].LocalPoint = circle.Position; manifold.Points[0].Id.Key = 0; return; } // Compute barycentric coordinates float u1 = Vec2.Dot(cLocal - v1, v2 - v1); float u2 = Vec2.Dot(cLocal - v2, v1 - v2); if (u1 <= 0.0f) { if (Vec2.DistanceSquared(cLocal, v1) > radius * radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalPlaneNormal = cLocal - v1; manifold.LocalPlaneNormal.Normalize(); manifold.LocalPoint = v1; manifold.Points[0].LocalPoint = circle.Position; manifold.Points[0].Id.Key = 0; } else if (u2 <= 0.0f) { if (Vec2.DistanceSquared(cLocal, v2) > radius * radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalPlaneNormal = cLocal - v2; manifold.LocalPlaneNormal.Normalize(); manifold.LocalPoint = v2; manifold.Points[0].LocalPoint = circle.Position; manifold.Points[0].Id.Key = 0; } else { Vec2 faceCenter = 0.5f * (v1 + v2); float dot = Vec2.Dot(cLocal - faceCenter, normals[vertIndex1]); if (dot > radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalPlaneNormal = normals[vertIndex1]; manifold.LocalPoint = faceCenter; manifold.Points[0].LocalPoint = circle.Position; manifold.Points[0].Id.Key = 0; } }