BEPUphysics.CollisionTests.CollisionAlgorithms.SphereTester.AreSpheresColliding C# (CSharp) Метод

        public static bool AreSpheresColliding(SphereShape a, SphereShape b, ref Vector3 positionA, ref Vector3 positionB, out ContactData contact)
        {
            contact = new ContactData();

            float radiusSum = a.collisionMargin + b.collisionMargin;
            Vector3 centerDifference;
            Vector3.Subtract(ref positionB, ref positionA, out centerDifference);
            float centerDistance = centerDifference.LengthSquared();

            if (centerDistance < (radiusSum + CollisionDetectionSettings.maximumContactDistance) * (radiusSum + CollisionDetectionSettings.maximumContactDistance))
            {
                //In collision!

                if (radiusSum > Toolbox.Epsilon) //This would be weird, but it is still possible to cause a NaN.
                    Vector3.Multiply(ref centerDifference, a.collisionMargin / (radiusSum), out  contact.Position);
                else contact.Position = new Vector3();
                Vector3.Add(ref contact.Position, ref positionA, out contact.Position);

                centerDistance = (float)Math.Sqrt(centerDistance);
                if (centerDistance > Toolbox.BigEpsilon)
                {
                    Vector3.Divide(ref centerDifference, centerDistance, out contact.Normal);
                }
                else
                {
                    contact.Normal = Toolbox.UpVector;
                }
                contact.PenetrationDepth = radiusSum - centerDistance;

                return true;

            }
            return false;
        }
    }