internal PositionSolverManifold(ref ContactConstraint cc, int index)
{
Debug.Assert(cc.PointCount > 0);
switch (cc.Type)
{
case ManifoldType.Circles:
{
Vector2 pointA = cc.BodyA.GetWorldPoint(ref cc.LocalPoint);
Vector2 pointB = cc.BodyB.GetWorldPoint(ref cc.Points[0].LocalPoint);
if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
{
Normal = pointB - pointA;
Normal.Normalize();
}
else
{
Normal = new Vector2(1.0f, 0.0f);
}
Point = 0.5f * (pointA + pointB);
Separation = Vector2.Dot(pointB - pointA, Normal) - cc.Radius;
}
break;
case ManifoldType.FaceA:
{
Normal = cc.BodyA.GetWorldVector(ref cc.LocalNormal);
Vector2 planePoint = cc.BodyA.GetWorldPoint(ref cc.LocalPoint);
Vector2 clipPoint = cc.BodyB.GetWorldPoint(ref cc.Points[index].LocalPoint);
Separation = Vector2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
Point = clipPoint;
}
break;
case ManifoldType.FaceB:
{
Normal = cc.BodyB.GetWorldVector(ref cc.LocalNormal);
Vector2 planePoint = cc.BodyB.GetWorldPoint(ref cc.LocalPoint);
Vector2 clipPoint = cc.BodyA.GetWorldPoint(ref cc.Points[index].LocalPoint);
Separation = Vector2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
Point = clipPoint;
// Ensure normal points from A to B
Normal = -Normal;
}
break;
default:
Normal = Vector2.Zero;
Point = Vector2.Zero;
Separation = 0.0f;
break;
}
}