| public static DistanceSquared ( Vector2 value1, Vector2 value2 ) : float | ||
| value1 | Vector2 | Source vector. |
| value2 | Vector2 | Source vector. |
| return | float | |
public static bool CollidePolygonAndCircle(
Fixture fixtureA,
WorldTransform xfA,
Fixture fixtureB,
WorldTransform xfB,
out Manifold manifold)
{
manifold = new Manifold();
var polygonA = fixtureA as PolygonFixture;
var circleB = fixtureB as CircleFixture;
System.Diagnostics.Debug.Assert(polygonA != null);
System.Diagnostics.Debug.Assert(circleB != null);
// Compute circle position in the frame of the polygon.
var centerInA = xfA.ToLocal(xfB.ToGlobal(circleB.Center));
var totalRadius = polygonA.Radius + circleB.Radius;
var vertexCountA = polygonA.Count;
var verticesA = polygonA.Vertices;
var normalsA = polygonA.Normals;
// Find the min separating edge.
var normalIndexA = 0;
var separation = float.MinValue;
for (var i = 0; i < vertexCountA; ++i)
{
var s = Vector2Util.Dot(normalsA[i], centerInA - verticesA[i]);
if (s > totalRadius)
{
// Early out.
manifold = new Manifold();
return(false);
}
if (s > separation)
{
separation = s;
normalIndexA = i;
}
}
// Vertices that subtend the incident face.
var vertexIndexA1 = normalIndexA;
var vertexIndexA2 = vertexIndexA1 + 1 < vertexCountA ? vertexIndexA1 + 1 : 0;
var vertexA1 = verticesA[vertexIndexA1];
var vertexA2 = verticesA[vertexIndexA2];
// If the center is inside the polygon ...
if (separation < Settings.Epsilon)
{
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.PointCount = 1;
manifold.Points.Item1.LocalPoint = circleB.Center;
manifold.Points.Item1.Id.Key = 0;
manifold.LocalNormal = normalsA[normalIndexA];
manifold.LocalPoint = 0.5f * (vertexA1 + vertexA2);
return(true);
}
// Compute barycentric coordinates
var u1 = Vector2Util.Dot(centerInA - vertexA1, vertexA2 - vertexA1);
var u2 = Vector2Util.Dot(centerInA - vertexA2, vertexA1 - vertexA2);
if (u1 <= 0.0f)
{
if (LocalPoint.DistanceSquared(centerInA, vertexA1) <= totalRadius * totalRadius)
{
var normalInA = centerInA - vertexA1;
normalInA.Normalize();
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.PointCount = 1;
manifold.Points.Item1.LocalPoint = circleB.Center;
manifold.Points.Item1.Id.Key = 0;
manifold.LocalNormal = normalInA;
manifold.LocalPoint = vertexA1;
return(true);
}
}
else if (u2 <= 0.0f)
{
if (LocalPoint.DistanceSquared(centerInA, vertexA2) <= totalRadius * totalRadius)
{
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.PointCount = 1;
manifold.Points.Item1.LocalPoint = circleB.Center;
manifold.Points.Item1.Id.Key = 0;
manifold.LocalNormal = centerInA - vertexA2;
manifold.LocalNormal.Normalize();
manifold.LocalPoint = vertexA2;
return(true);
}
}
else
{
var faceCenter = 0.5f * (vertexA1 + vertexA2);
if (Vector2Util.Dot(centerInA - faceCenter, normalsA[vertexIndexA1]) <= totalRadius)
{
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.PointCount = 1;
manifold.Points.Item1.LocalPoint = circleB.Center;
manifold.Points.Item1.Id.Key = 0;
manifold.LocalNormal = normalsA[vertexIndexA1];
manifold.LocalPoint = faceCenter;
return(true);
}
}
return(false);
}
