/// 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;
}
}
