/// <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.MultiplyT(ref transformA, MathUtils.Multiply(ref transformB, ref circleB._position));
Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2;
Vector2 e = B - A;
// Barycentric coordinates
float u = Vector2.Dot(e, B - Q);
float v = Vector2.Dot(e, Q - A);
float radius = edgeA.Radius + circleB.Radius;
ContactFeature cf;
cf.IndexB = 0;
cf.TypeB = (byte)ContactFeatureType.Vertex;
Vector2 P, d;
// Region A
if (v <= 0.0f)
{
P = A;
d = Q - P;
float dd;
Vector2.Dot(ref d, ref d, out dd);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.HasVertex0)
{
Vector2 A1 = edgeA.Vertex0;
Vector2 B1 = A;
Vector2 e1 = B1 - A1;
float 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 = (byte)ContactFeatureType.Vertex;
manifold.PointCount = 1;
manifold.Type = ManifoldType.Circles;
manifold.LocalNormal = Vector2.Zero;
manifold.LocalPoint = P;
ManifoldPoint mp = new ManifoldPoint();
mp.Id.Key = 0;
mp.Id.Features = cf;
mp.LocalPoint = circleB.Position;
manifold.Points[0] = mp;
return;
}
// Region B
if (u <= 0.0f)
{
P = B;
d = Q - P;
float dd;
Vector2.Dot(ref d, ref d, out dd);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.HasVertex3)
{
Vector2 B2 = edgeA.Vertex3;
Vector2 A2 = B;
Vector2 e2 = B2 - A2;
float 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 = P;
ManifoldPoint mp = new ManifoldPoint();
mp.Id.Key = 0;
mp.Id.Features = cf;
mp.LocalPoint = circleB.Position;
manifold.Points[0] = mp;
return;
}
// Region AB
float den;
Vector2.Dot(ref e, ref e, out den);
Debug.Assert(den > 0.0f);
P = (1.0f / den) * (u * A + v * B);
d = Q - P;
float dd2;
Vector2.Dot(ref d, ref d, out dd2);
if (dd2 > 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 = (byte)ContactFeatureType.Face;
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = n;
manifold.LocalPoint = A;
ManifoldPoint mp2 = new ManifoldPoint();
mp2.Id.Key = 0;
mp2.Id.Features = cf;
mp2.LocalPoint = circleB.Position;
manifold.Points[0] = mp2;
}
/// <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
var Q = MathUtils.mulT( ref transformA, MathUtils.Mul( ref transformB, ref circleB._position ) );
Vector2 A = edgeA.vertex1, B = edgeA.vertex2;
var e = B - A;
// Barycentric coordinates
var u = Vector2.Dot( e, B - Q );
var v = Vector2.Dot( e, Q - A );
var radius = edgeA.radius + circleB.radius;
ContactFeature cf;
cf.indexB = 0;
cf.typeB = (byte)ContactFeatureType.Vertex;
Vector2 P, d;
// Region A
if( v <= 0.0f )
{
P = A;
d = Q - P;
float dd;
Vector2.Dot( ref d, ref d, out dd );
if( dd > radius * radius )
return;
// Is there an edge connected to A?
if( edgeA.hasVertex0 )
{
var A1 = edgeA.vertex0;
var B1 = A;
var e1 = B1 - A1;
var 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 = (byte)ContactFeatureType.Vertex;
manifold.pointCount = 1;
manifold.type = ManifoldType.Circles;
manifold.localNormal = Vector2.Zero;
manifold.localPoint = P;
var mp = new ManifoldPoint();
mp.id.key = 0;
mp.id.features = cf;
mp.localPoint = circleB.position;
manifold.points[0] = mp;
return;
}
// Region B
if( u <= 0.0f )
{
P = B;
d = Q - P;
float dd;
Vector2.Dot( ref d, ref d, out dd );
if( dd > radius * radius )
return;
// Is there an edge connected to B?
if( edgeA.hasVertex3 )
{
var B2 = edgeA.vertex3;
var A2 = B;
var e2 = B2 - A2;
var 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 = P;
var mp = new ManifoldPoint();
mp.id.key = 0;
mp.id.features = cf;
mp.localPoint = circleB.position;
manifold.points[0] = mp;
return;
}
// Region AB
float den;
Vector2.Dot( ref e, ref e, out den );
Debug.Assert( den > 0.0f );
P = ( 1.0f / den ) * ( u * A + v * B );
d = Q - P;
float dd2;
Vector2.Dot( ref d, ref d, out dd2 );
if( dd2 > radius * radius )
return;
var 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 = (byte)ContactFeatureType.Face;
manifold.pointCount = 1;
manifold.type = ManifoldType.FaceA;
manifold.localNormal = n;
manifold.localPoint = A;
ManifoldPoint mp2 = new ManifoldPoint();
mp2.id.key = 0;
mp2.id.features = cf;
mp2.localPoint = circleB.position;
manifold.points[0] = mp2;
}
