public static void CollideCircles(ref Manifold manifold, CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2)
{
manifold.PointCount = 0;
Vector2 p1 = CommonMath.Mul(xf1, circle1.GetLocalPosition());
Vector2 p2 = CommonMath.Mul(xf2, circle2.GetLocalPosition());
Vector2 d = p2 - p1;
float distSqr = Vector2.Dot(d, d);
float r1 = circle1.GetRadius();
float r2 = circle2.GetRadius();
float radiusSum = r1 + r2;
if (distSqr > radiusSum * radiusSum)
{
return;
}
float separation;
if (distSqr < Settings.FLT_EPSILON)
{
separation = -radiusSum;
manifold.Normal = new Vector2(0.0f, 1.0f);
}
else
{
float dist = CommonMath.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;
Vector2 p = 0.5f * (p1 + p2);
manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, p);
manifold.Points[0].LocalPoint2 = CommonMath.MulT(xf2, p);
}
public static float DistanceGeneric <T1, T2>(out Vector2 x1, out Vector2 x2, T1 shape1_, XForm xf1, T2 shape2_, XForm xf2)
{
IGenericShape shape1 = shape1_ as IGenericShape;
IGenericShape shape2 = shape2_ as IGenericShape;
if (shape1 == null || shape2 == null)
{
//Box2DXDebug.Assert(false, "Can not cast some parameters to IGenericShape");
}
Vector2[] p1s = new Vector2[3], p2s = new Vector2[3];
Vector2[] points = new Vector2[3];
int pointCount = 0;
x1 = shape1.GetFirstVertex(xf1);
x2 = shape2.GetFirstVertex(xf2);
float vSqr = 0.0f;
int maxIterations = 20;
for (int iter = 0; iter < maxIterations; ++iter)
{
Vector2 v = x2 - x1;
Vector2 w1 = shape1.Support(xf1, v);
Vector2 w2 = shape2.Support(xf2, -v);
vSqr = Vector2.Dot(v, v);
Vector2 w = w2 - w1;
float vw = Vector2.Dot(v, w);
if (vSqr - vw <= 0.01f * vSqr || InPoints(w, points, pointCount)) // or w in points
{
if (pointCount == 0)
{
x1 = w1;
x2 = w2;
}
//GJKIterations = iter;
return(CommonMath.Sqrt(vSqr));
}
switch (pointCount)
{
case 0:
p1s[0] = w1;
p2s[0] = w2;
points[0] = w;
x1 = p1s[0];
x2 = p2s[0];
++pointCount;
break;
case 1:
p1s[1] = w1;
p2s[1] = w2;
points[1] = w;
pointCount = ProcessTwo(out x1, out x2, ref p1s, ref p2s, ref points);
break;
case 2:
p1s[2] = w1;
p2s[2] = w2;
points[2] = w;
pointCount = ProcessThree(out x1, out x2, ref p1s, ref p2s, ref points);
break;
}
// If we have three points, then the origin is in the corresponding triangle.
if (pointCount == 3)
{
//GJKIterations = iter;
return(0.0f);
}
float maxSqr = -Settings.FLT_MAX;
for (int i = 0; i < pointCount; ++i)
{
maxSqr = CommonMath.Max(maxSqr, Vector2.Dot(points[i], points[i]));
}
#if TARGET_FLOAT32_IS_FIXED
if (pointCount == 3 || vSqr <= 5.0 * Common.Settings.FLT_EPSILON * maxSqr)
#else
if (vSqr <= 100.0f * Settings.FLT_EPSILON * maxSqr)
#endif
{
//GJKIterations = iter;
v = x2 - x1;
vSqr = Vector2.Dot(v, v);
return(CommonMath.Sqrt(vSqr));
}
}
//GJKIterations = maxIterations;
return(CommonMath.Sqrt(vSqr));
}