/// Implement Shape. public override Shape Clone() { CircleShape shape = new CircleShape(); shape.ShapeType = ShapeType; shape._radius = _radius; shape._p = _p; return shape; }
/// Compute the collision manifold between two circles. public static void CollideCircles(ref Manifold manifold, CircleShape circle1, ref XForm xf1, CircleShape circle2, ref XForm xf2) { manifold._pointCount = 0; Vector2 p1 = MathUtils.Multiply(ref xf1, circle1._p); Vector2 p2 = MathUtils.Multiply(ref xf2, circle2._p); Vector2 d = p2 - p1; float distSqr = Vector2.Dot(d, d); float radius = circle1._radius + circle2._radius; if (distSqr > radius * radius) { return; } manifold._type = ManifoldType.Circles; manifold._localPoint = circle1._p; manifold._localPlaneNormal = Vector2.Zero; manifold._pointCount = 1; var p0 = manifold._points[0]; p0.LocalPoint = circle2._p; p0.Id.Key = 0; manifold._points[0] = p0; }
/// Compute the collision manifold between a polygon and a circle. public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygon, ref XForm xf1, CircleShape circle, ref XForm xf2) { manifold._pointCount = 0; // Compute circle position in the frame of the polygon. Vector2 c = MathUtils.Multiply(ref xf2, circle._p); Vector2 cLocal = MathUtils.MultiplyT(ref xf1, c); // Find the min separating edge. int normalIndex = 0; float separation = -Settings.b2_FLT_MAX; float radius = polygon._radius + circle._radius; int vertexCount = polygon._vertexCount; for (int i = 0; i < vertexCount; ++i) { float s = Vector2.Dot(polygon._normals[i], cLocal - polygon._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 = polygon._vertices[vertIndex1]; Vector2 v2 = polygon._vertices[vertIndex2]; // If the center is inside the polygon ... if (separation < Settings.b2_FLT_EPSILON) { manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localPlaneNormal = polygon._normals[normalIndex]; manifold._localPoint = 0.5f * (v1 + v2); var p0 = manifold._points[0]; p0.LocalPoint = circle._p; p0.Id.Key = 0; manifold._points[0] = p0; 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 (Vector2.DistanceSquared(cLocal, v1) > radius * radius) { return; } manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localPlaneNormal = cLocal - v1; manifold._localPlaneNormal.Normalize(); manifold._localPoint = v1; var p0b = manifold._points[0]; p0b.LocalPoint = circle._p; p0b.Id.Key = 0; manifold._points[0] = p0b; } else if (u2 <= 0.0f) { if (Vector2.DistanceSquared(cLocal, v2) > radius * radius) { return; } manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localPlaneNormal = cLocal - v2; manifold._localPlaneNormal.Normalize(); manifold._localPoint = v2; var p0c = manifold._points[0]; p0c.LocalPoint = circle._p; p0c.Id.Key = 0; manifold._points[0] = p0c; } else { Vector2 faceCenter = 0.5f * (v1 + v2); float separation2 = Vector2.Dot(cLocal - faceCenter, polygon._normals[vertIndex1]); if (separation2 > radius) { return; } manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localPlaneNormal = polygon._normals[vertIndex1]; manifold._localPoint = faceCenter; var p0d = manifold._points[0]; p0d.LocalPoint = circle._p; p0d.Id.Key = 0; manifold._points[0] = p0d; } }
/// Compute the collision manifold between a polygon and a circle. public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygon, ref XForm xf1, CircleShape circle, ref XForm xf2) { manifold._pointCount = 0; // Compute circle position in the frame of the polygon. Vector2 c = MathUtils.Multiply(ref xf2, circle._p); Vector2 cLocal = MathUtils.MultiplyT(ref xf1, c); // Find the min separating edge. int normalIndex = 0; float separation = -Settings.b2_FLT_MAX; float radius = polygon._radius + circle._radius; int vertexCount = polygon._vertexCount; for (int i = 0; i < vertexCount; ++i) { float s = Vector2.Dot(polygon._normals[i], cLocal - polygon._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 = polygon._vertices[vertIndex1]; Vector2 v2 = polygon._vertices[vertIndex2]; // If the center is inside the polygon ... if (separation < Settings.b2_FLT_EPSILON) { manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localPlaneNormal = polygon._normals[normalIndex]; manifold._localPoint = 0.5f * (v1 + v2); var p0 = manifold._points[0]; p0.LocalPoint = circle._p; p0.Id.Key = 0; manifold._points[0] = p0; 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 (Vector2.DistanceSquared(cLocal, v1) > radius * radius) { return; } manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localPlaneNormal = cLocal - v1; manifold._localPlaneNormal.Normalize(); manifold._localPoint = v1; var p0b = manifold._points[0]; p0b.LocalPoint = circle._p; p0b.Id.Key = 0; manifold._points[0] = p0b; } else if (u2 <= 0.0f) { if (Vector2.DistanceSquared(cLocal, v2) > radius * radius) { return; } manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localPlaneNormal = cLocal - v2; manifold._localPlaneNormal.Normalize(); manifold._localPoint = v2; var p0c = manifold._points[0]; p0c.LocalPoint = circle._p; p0c.Id.Key = 0; manifold._points[0] = p0c; } else { Vector2 faceCenter = 0.5f * (v1 + v2); float separation2 = Vector2.Dot(cLocal - faceCenter, polygon._normals[vertIndex1]); if (separation2 > radius) { return; } manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localPlaneNormal = polygon._normals[vertIndex1]; manifold._localPoint = faceCenter; var p0d = manifold._points[0]; p0d.LocalPoint = circle._p; p0d.Id.Key = 0; manifold._points[0] = p0d; } }