// This is used for polygon-vs-circle distance.
// 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 Vector2 x1, out Vector2 x2, PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2)
{
Point point = new Point();
point.p = CommonMath.Mul(xf2, circle.GetLocalPosition());
float distance = DistanceGeneric(out x1, out x2, polygon, xf1, point, XForm.Identity);
float r = circle.GetRadius() - Settings.ToiSlop;
if (distance > r)
{
distance -= r;
Vector2 d = x2 - x1;
d.Normalize();
x2 -= r * d;
}
else
{
distance = 0.0f;
x2 = x1;
}
return(distance);
}
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 override void Step(Settings settings)
{
base.Step(settings);
// Traverse the contact results. Apply a force on shapes
// that overlap the sensor.
for (int i = 0; i < _pointCount; ++i)
{
MyContactPoint point = _points[i];
if ((int)point.state == 2)
{
continue;
}
Shape shape1 = point.shape1;
Shape shape2 = point.shape2;
Body other;
if (shape1 == _sensor)
{
other = shape2.GetBody();
}
else if (shape2 == _sensor)
{
other = shape1.GetBody();
}
else
{
continue;
}
Body ground = _sensor.GetBody();
CircleShape circle = (CircleShape)_sensor;
Vec2 center = ground.GetWorldPoint(circle.GetLocalPosition());
Vec2 d = center - point.position;
if (d.LengthSquared() < Box2DX.Common.Settings.FLT_EPSILON * Box2DX.Common.Settings.FLT_EPSILON)
{
continue;
}
d.Normalize();
Vec2 F = 100.0f * d;
other.ApplyForce(F, point.position);
}
}
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 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;
}
