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