public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygon, Transform xf1, CircleShape circle, Transform xf2) { manifold.PointCount = 0; // Compute circle position in the frame of the polygon. Vector2 c = xf2.TransformPoint(circle._position); Vector2 cLocal = xf1.InverseTransformPoint(c); // Find the min separating edge. int normalIndex = 0; float separation = -Settings.FLT_MAX; float radius = polygon._radius + circle._radius; int vertexCount = polygon._vertexCount; Vector2[] vertices = polygon._vertices; Vector2[] normals = polygon._normals; for (int i = 0; i < vertexCount; ++i) { float s = Vector2.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; Vector2 v1 = vertices[vertIndex1]; Vector2 v2 = vertices[vertIndex2]; // If the center is inside the polygon ... if (separation < Common.Settings.FLT_EPSILON) { 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 = Vector2.Dot(cLocal - v1, v2 - v1); float u2 = Vector2.Dot(cLocal - v2, v1 - v2); if (u1 <= 0.0f) { if ((cLocal - v1).LengthSquared() > 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 ((cLocal - v2).LengthSquared() > 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 { Vector2 faceCenter = 0.5f * (v1 + v2); float separation_ = Vector2.Dot(cLocal - faceCenter, normals[vertIndex1]); if (separation_ > 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; } }
/// <summary> /// Gets a local point relative to the body's origin given a world point. /// </summary> /// <param name="worldPoint">A point in world coordinates.</param> /// <returns>Return the corresponding local point relative to the body's origin.</returns> public Vector2 GetLocalPoint(Vector2 worldPoint) { return(_xf.InverseTransformPoint(worldPoint)); }