//public static int GJKIterations;
// GJK using Voronoi regions (Christer Ericson) and region selection
// optimizations (Casey Muratori).
// The origin is either in the region of points[1] or in the edge region. The origin is
// not in region of points[0] because that is the old point.
public static int ProcessTwo(out Vector2 x1, out Vector2 x2, ref Vector2[] p1s, ref Vector2[] p2s, ref Vector2[] points)
{
// If in point[1] region
Vector2 r = -points[1];
Vector2 d = points[0] - points[1];
float length = CommonMath.Normalize(ref d);
float lambda = Vector2.Dot(r, d);
if (lambda <= 0.0f || length < Settings.FLT_EPSILON)
{
// The simplex is reduced to a point.
x1 = p1s[1];
x2 = p2s[1];
p1s[0] = p1s[1];
p2s[0] = p2s[1];
points[0] = points[1];
return(1);
}
// Else in edge region
lambda /= length;
x1 = p1s[1] + lambda * (p1s[0] - p1s[1]);
x2 = p2s[1] + lambda * (p2s[0] - p2s[1]);
return(2);
}
public static float DistanceCC(out Vector2 x1, out Vector2 x2, CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2)
{
Vector2 p1 = CommonMath.Mul(xf1, circle1.GetLocalPosition());
Vector2 p2 = CommonMath.Mul(xf2, circle2.GetLocalPosition());
Vector2 d = p2 - p1;
float dSqr = Vector2.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 = CommonMath.Normalize(ref d);
float distance = dLen - r;
x1 = p1 + r1 * d;
x2 = p2 - r2 * d;
return(distance);
}
else if (dSqr > Settings.FLT_EPSILON * Settings.FLT_EPSILON)
{
d.Normalize();
x1 = p1 + r1 * d;
x2 = x1;
return(0.0f);
}
x1 = p1;
x2 = x1;
return(0.0f);
}
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.
Vector2 c = CommonMath.Mul(xf2, circle.GetLocalPosition());
Vector2 cLocal = CommonMath.MulT(xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = circle.GetRadius();
int vertexCount = polygon.VertexCount;
Vector2[] vertices = polygon.GetVertices();
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;
}
}
// If the center is inside the polygon ...
if (separation < Settings.FLT_EPSILON)
{
manifold.PointCount = 1;
manifold.Normal = CommonMath.Mul(xf1.R, normals[normalIndex]);
manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
manifold.Points[0].ID.Features.IncidentVertex = NullFeature;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
Vector2 position = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, position);
manifold.Points[0].LocalPoint2 = CommonMath.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;
Vector2 e = vertices[vertIndex2] - vertices[vertIndex1];
float length = CommonMath.Normalize(ref e);
//Box2DXDebug.Assert(length > Settings.FLT_EPSILON);
// Project the center onto the edge.
float u = Vector2.Dot(cLocal - vertices[vertIndex1], e);
Vector2 p;
if (u <= 0.0f)
{
p = vertices[vertIndex1];
manifold.Points[0].ID.Features.IncidentEdge = NullFeature;
manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1;
}
else if (u >= length)
{
p = vertices[vertIndex2];
manifold.Points[0].ID.Features.IncidentEdge = 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 = NullFeature;
}
Vector2 d = cLocal - p;
float dist = CommonMath.Normalize(ref d);
if (dist > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Normal = CommonMath.Mul(xf1.R, d);
Vector2 position_ = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, position_);
manifold.Points[0].LocalPoint2 = CommonMath.MulT(xf2, position_);
manifold.Points[0].Separation = dist - radius;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
}