// 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(); } }