/// <summary>
/// Compute contact points for edge versus circle.
/// This accounts for edge connectivity.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="edgeA">The edge A.</param>
/// <param name="transformA">The transform A.</param>
/// <param name="circleB">The circle B.</param>
/// <param name="transformB">The transform B.</param>
public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edgeA, ref Transform transformA, CircleShape circleB, ref Transform transformB)
{
manifold.PointCount = 0;
// Compute circle in frame of edge
Vector2 Q = MathUtils.MulT(ref transformA, MathUtils.Mul(ref transformB, ref circleB._position));
Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2;
Vector2 e = B - A;
// Barycentric coordinates
GGame.Math.Fix64 u = Vector2.Dot(e, B - Q);
GGame.Math.Fix64 v = Vector2.Dot(e, Q - A);
GGame.Math.Fix64 radius = edgeA.Radius + circleB.Radius;
ContactFeature cf;
cf.IndexB = 0;
cf.TypeB = ContactFeatureType.Vertex;
// Region A
if (v <= 0.0f)
{
Vector2 P1 = A;
Vector2 d1 = Q - P1;
GGame.Math.Fix64 dd1 = Vector2.Dot(d1, d1);
if (dd1 > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.HasVertex0)
{
Vector2 A1 = edgeA.Vertex0;
Vector2 B1 = A;
Vector2 e1 = B1 - A1;
GGame.Math.Fix64 u1 = Vector2.Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.IndexA = 0;
cf.TypeA = ContactFeatureType.Vertex;
manifold.PointCount = 1;
manifold.Type = ManifoldType.Circles;
manifold.LocalNormal = Vector2.Zero;
manifold.LocalPoint = P1;
manifold.Points.Value0.Id.Key = 0;
manifold.Points.Value0.Id.ContactFeature = cf;
manifold.Points.Value0.LocalPoint = circleB.Position;
return;
}
// Region B
if (u <= 0.0f)
{
Vector2 P2 = B;
Vector2 d2 = Q - P2;
GGame.Math.Fix64 dd2 = Vector2.Dot(d2, d2);
if (dd2 > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.HasVertex3)
{
Vector2 B2 = edgeA.Vertex3;
Vector2 A2 = B;
Vector2 e2 = B2 - A2;
GGame.Math.Fix64 v2 = Vector2.Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.IndexA = 1;
cf.TypeA = (byte)ContactFeatureType.Vertex;
manifold.PointCount = 1;
manifold.Type = ManifoldType.Circles;
manifold.LocalNormal = Vector2.Zero;
manifold.LocalPoint = P2;
manifold.Points.Value0.Id.Key = 0;
manifold.Points.Value0.Id.ContactFeature = cf;
manifold.Points.Value0.LocalPoint = circleB.Position;
return;
}
// Region AB
GGame.Math.Fix64 den = Vector2.Dot(e, e);
Debug.Assert(den > 0.0f);
Vector2 P = (1.0f / den) * (u * A + v * B);
Vector2 d = Q - P;
GGame.Math.Fix64 dd = Vector2.Dot(d, d);
if (dd > radius * radius)
{
return;
}
Vector2 n = new Vector2(-e.Y, e.X);
if (Vector2.Dot(n, Q - A) < 0.0f)
{
n = new Vector2(-n.X, -n.Y);
}
n.Normalize();
cf.IndexA = 0;
cf.TypeA = ContactFeatureType.Face;
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = n;
manifold.LocalPoint = A;
manifold.Points.Value0.Id.Key = 0;
manifold.Points.Value0.Id.ContactFeature = cf;
manifold.Points.Value0.LocalPoint = circleB.Position;
}
/// <summary>
/// Compute the collision manifold between a polygon and a circle.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="polygonA">The polygon A.</param>
/// <param name="xfA">The transform of A.</param>
/// <param name="circleB">The circle B.</param>
/// <param name="xfB">The transform of B.</param>
public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygonA, ref Transform xfA, CircleShape circleB, ref Transform xfB)
{
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vector2 c = MathUtils.Mul(ref xfB, circleB.Position);
Vector2 cLocal = MathUtils.MulT(ref xfA, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.MaxFloat;
float radius = polygonA.Radius + circleB.Radius;
int vertexCount = polygonA.Vertices.Count;
Vertices vertices = polygonA.Vertices;
Vertices normals = polygonA.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;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vector2 v1 = vertices[vertIndex1];
Vector2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Settings.Epsilon)
{
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = normals[normalIndex];
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points.Value0.LocalPoint = circleB.Position;
manifold.Points.Value0.Id.Key = 0;
return;
}
// Compute barycentric coordinates
float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (Vector2.DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = cLocal - v1;
manifold.LocalNormal.Normalize();
manifold.LocalPoint = v1;
manifold.Points.Value0.LocalPoint = circleB.Position;
manifold.Points.Value0.Id.Key = 0;
}
else if (u2 <= 0.0f)
{
if (Vector2.DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = cLocal - v2;
manifold.LocalNormal.Normalize();
manifold.LocalPoint = v2;
manifold.Points.Value0.LocalPoint = circleB.Position;
manifold.Points.Value0.Id.Key = 0;
}
else
{
Vector2 faceCenter = 0.5f * (v1 + v2);
float s = Vector2.Dot(cLocal - faceCenter, normals[vertIndex1]);
if (s > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = normals[vertIndex1];
manifold.LocalPoint = faceCenter;
manifold.Points.Value0.LocalPoint = circleB.Position;
manifold.Points.Value0.Id.Key = 0;
}
}
/// <summary>
/// Collides and edge and a polygon, taking into account edge adjacency.
/// </summary>
public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
{
EPCollider.Collide(ref manifold, edgeA, ref xfA, polygonB, ref xfB);
}
public static void GetPointStates(out FixedArray2 <PointState> state1, out FixedArray2 <PointState> state2, ref Manifold manifold1, ref Manifold manifold2)
{
state1 = new FixedArray2 <PointState>();
state2 = new FixedArray2 <PointState>();
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
state1[i] = PointState.Null;
state2[i] = PointState.Null;
}
// Detect persists and removes.
for (int i = 0; i < manifold1.PointCount; ++i)
{
ContactID id = manifold1.Points[i].Id;
state1[i] = PointState.Remove;
for (int j = 0; j < manifold2.PointCount; ++j)
{
if (manifold2.Points[j].Id.Key == id.Key)
{
state1[i] = PointState.Persist;
break;
}
}
}
// Detect persists and adds.
for (int i = 0; i < manifold2.PointCount; ++i)
{
ContactID id = manifold2.Points[i].Id;
state2[i] = PointState.Add;
for (int j = 0; j < manifold1.PointCount; ++j)
{
if (manifold1.Points[j].Id.Key == id.Key)
{
state2[i] = PointState.Persist;
break;
}
}
}
}