// ReSharper disable RedundantCast Necessary for FarPhysics.
private static void InitializePositionSolverManifold(
ContactPositionConstraint pc,
WorldTransform xfA,
WorldTransform xfB,
int index,
out Vector2 normal,
out WorldPoint point,
out float separation)
{
System.Diagnostics.Debug.Assert(pc.PointCount > 0);
switch (pc.Type)
{
case Manifold.ManifoldType.Circles:
{
var pointA = xfA.ToGlobal(pc.LocalPoint);
var pointB = xfB.ToGlobal(pc.LocalPoints[0]);
normal.X = (float)(pointB.X - pointA.X);
normal.Y = (float)(pointB.Y - pointA.Y);
normal.Normalize();
#if FARMATH
// Avoid multiplication of far values.
point.X = pointA.X + 0.5f * (float)(pointB.X - pointA.X);
point.Y = pointA.Y + 0.5f * (float)(pointB.Y - pointA.Y);
#else
point.X = 0.5f * (pointA.X + pointB.X);
point.Y = 0.5f * (pointA.Y + pointB.Y);
#endif
separation = Vector2Util.Dot((Vector2)(pointB - pointA), normal) - pc.RadiusA - pc.RadiusB;
break;
}
case Manifold.ManifoldType.FaceA:
{
normal.X = xfA.Rotation.Cos * pc.LocalNormal.X - xfA.Rotation.Sin * pc.LocalNormal.Y;
normal.Y = xfA.Rotation.Sin * pc.LocalNormal.X + xfA.Rotation.Cos * pc.LocalNormal.Y;
var planePoint = xfA.ToGlobal(pc.LocalPoint);
point = xfB.ToGlobal(pc.LocalPoints[index]);
separation = Vector2Util.Dot((Vector2)(point - planePoint), normal) - pc.RadiusA - pc.RadiusB;
break;
}
case Manifold.ManifoldType.FaceB:
{
normal.X = xfB.Rotation.Cos * pc.LocalNormal.X - xfB.Rotation.Sin * pc.LocalNormal.Y;
normal.Y = xfB.Rotation.Sin * pc.LocalNormal.X + xfB.Rotation.Cos * pc.LocalNormal.Y;
var planePoint = xfB.ToGlobal(pc.LocalPoint);
point = xfA.ToGlobal(pc.LocalPoints[index]);
separation = Vector2Util.Dot((Vector2)(point - planePoint), normal) - pc.RadiusA - pc.RadiusB;
// Ensure normal points from A to B
normal = -normal;
break;
}
default:
throw new InvalidOperationException();
}
}
