IntersectionHelper.cs - WindowsPhoneDemoGame

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace WindowsPhoneGameDemoObjects
{
    class IntersectionHelper
    {
        /// <summary>
        /// Проверка пересечения модели и BoundingSphere камеры
        /// </summary>
        /// <param name="model">Модель</param>
        /// <param name="modelWorld">Матрица мира модели</param>
        /// <param name="sphere">BoundingSphere камеры</param>
        /// <param name="movement">Вектор движения камеры для выхода из коллизии</param>
        /// <returns>true если есть пересечение, иначе false</returns>
        public static bool Intersects(Model model, Matrix modelWorld, BoundingSphere sphere, out Vector3 movement)
        {
            movement = Vector3.Zero;
            sphere = sphere.Transform(Matrix.Invert(modelWorld));
            Vector3 untransformedMove;
            if (!Intersects(model, sphere, out untransformedMove))
                return false;
            movement = Vector3.Transform(untransformedMove, modelWorld);
            movement -= modelWorld.Translation;
            return true;
        }


        private static bool Intersects(Model model, BoundingSphere sphere, out Vector3 movement)
        {
            movement = Vector3.Zero;

            //Вначале извлекаем и трансформируем BoundingSphere модели, если нет пересечения с BoundingSphere камеры, то возвращаем false
            Dictionary<string, object> tagData = (Dictionary<string, object>)model.Tag;
            BoundingSphere modelSphere = (BoundingSphere)tagData["BoundingSphere"];
            if (!modelSphere.Intersects(sphere))
                return false;

            Vector3[] vertices = (Vector3[])tagData["Vertices"];
            Vector3 sphereCenter = sphere.Center;
            float sphereRadiusSquared = sphere.Radius * sphere.Radius;  //Возведем радиус в квадрат для быстрой проверки

            bool hasCollision = false;

            Ray downRay = new Ray(sphere.Center, Vector3.Down); //Луч, исходяший из центра камеры и направленный вниз

            float rx = 0, ry = 0, rz = 0; //Итоговое движение назад

            Vector3 qMove, point1, point2, point3, v1, v2, normal, Offset, centerMinusOffset, centerPlusOffset;
            //point1, point2, point3 - вершины из модели

            for (int i = 0; i < vertices.Length; i += 3)
            {
                //default CullMode - CCW (против часовой)

                point1 = vertices[i];
                point2 = vertices[i + 1];
                point3 = vertices[i + 2];

                v1 = point3 - point1;
                v2 = point3 - point2;

                //Скрещиваем направляющие, получаем вектор нормали
                Vector3.Cross(ref v1, ref v2, out normal);
                normal.Normalize();
                //normal - вектор нормали к треугольнику из точек p1, p2, p3
                //distanceSquared - квадрат расстояния от центра сферы камеры до плоскости треугльника
                //Уравнение плоскости normal.X * x0 + normal.Y * y0 + normal.Z * z0 + d = 0;

                float d = 0 - normal.X * point1.X - normal.Y * point1.Y - normal.Z * point1.Z;

                float distanceSquared = (float)(Math.Pow(normal.X * sphereCenter.X + normal.Y * sphereCenter.Y + normal.Z * sphereCenter.Z + d, 2)
                    / (normal.X * normal.X + normal.Y * normal.Y + normal.Z * normal.Z));

                if (distanceSquared >= sphereRadiusSquared)
                    continue;

                //distance - расстояние от центра сферы до плоскости
                float distance = (float)(Math.Abs(normal.X * sphereCenter.X + normal.Y * sphereCenter.Y + normal.Z * sphereCenter.Z + d)
                    / Math.Sqrt(normal.X * normal.X + normal.Y * normal.Y + normal.Z * normal.Z));

                //Вектор смещения относительно центра сферы
                Offset = normal * distance;

                qMove = Vector3.Zero;

                centerMinusOffset = sphere.Center - Offset;
                centerPlusOffset = sphere.Center + Offset;

                if (IsPointInTriangle(ref centerMinusOffset, ref point1, ref point2, ref point3))
                {
                    float moveAmount = sphere.Radius - distance;
                    qMove = normal * moveAmount;
                    hasCollision = true;
                }

                if (IsPointInTriangle(ref centerPlusOffset, ref point1, ref point2, ref point3))
                {
                    float moveAmount = sphere.Radius - distance;
                    qMove = normal * (-moveAmount);
                    hasCollision = true;
                }

                rx = Math.Abs(qMove.X) > Math.Abs(rx) ? qMove.X : rx;
                ry = Math.Abs(qMove.Y) > Math.Abs(ry) ? qMove.Y : ry;
                rz = Math.Abs(qMove.Z) > Math.Abs(rz) ? qMove.Z : rz;

                Vector3 edgeMove;
                if (EdgeSphereCollision(ref sphere, ref point1, ref point2, ref point3, out edgeMove))
                {
                    if (edgeMove.LengthSquared() > (rx * rx + ry * ry + rz * rz))
                    {
                        rx = edgeMove.X;
                        ry = edgeMove.Y;
                        rz = edgeMove.Z;
                    }
                    hasCollision = true;
                }
            }

            movement = new Vector3(rx, ry, rz);

            return hasCollision;
        }


        /// <summary>
        /// Проверка пересечения сферы и граней треугольника
        /// </summary>
        /// <param name="sphere">Сфера</param>
        /// <param name="p1">Первая вершина треугольника</param>
        /// <param name="p2">Вторая вершина треугольника</param>
        /// <param name="p3">Третья вершина треугольника</param>
        /// <param name="movement">Вектор движения камеры для выхода из коллизии</param>
        /// <returns>true если есть пересечение, иначе false</returns>
        private static bool EdgeSphereCollision(ref BoundingSphere sphere, ref Vector3 p1, ref Vector3 p2, ref Vector3 p3, out Vector3 movement)
        {
            movement = Vector3.Zero;


            Vector3 center = sphere.Center;
            float radius = sphere.Radius * 1.0f;

            Vector3 lp1 = ClosestPointOnLine(ref center, ref p1, ref p2);
            Vector3 lp2 = ClosestPointOnLine(ref center, ref p2, ref p3);
            Vector3 lp3 = ClosestPointOnLine(ref center, ref p3, ref p1);

            float lp1Dist = (center - lp1).Length();
            float lp2Dist = (center - lp2).Length();
            float lp3Dist = (center - lp3).Length();

            Vector3 dir = Vector3.Zero;
            float dist = 0;

            if (lp1Dist < radius)
            {
                dir = center - lp1;
                dist = radius - lp1Dist;
            }

            if (lp2Dist < radius)
            {
                dir = center - lp2;
                dist = radius - lp2Dist;
            }

            if (lp3Dist < radius)
            {
                dir = center - lp3;
                dist = radius - lp3Dist;
            }

            if (dist == 0)
                return false;

            dir.Normalize();
            dir *= dist;
            movement = dir;

            return true;
        }

        /// <summary>
        /// Принадлежит ли точка треугольнику
        /// </summary>
        /// <param name="p">Искомая точка</param>
        /// <param name="a">Первая точка треугольника</param>
        /// <param name="b">Вторая точка треугольника</param>
        /// <param name="c">Третья точка треугольника</param>
        /// <returns>true если принадлежит, иначе false</returns>
        private static bool IsPointInTriangle(ref Vector3 p, ref Vector3 a, ref Vector3 b, ref Vector3 c)
        {
            //Коэффициенты для приближенного сравнения
            float lowRange = 0.999995f;
            float highRange = 1.000005f;

            //Считаем площадь треугольнка abc, потом площади abp, acp, bcp, сравниваем
            float abc = TriangleSquare(ref a, ref b, ref c);

            float abp = TriangleSquare(ref a, ref b, ref p);
            float acp = TriangleSquare(ref a, ref c, ref p);
            float bcp = TriangleSquare(ref b, ref c, ref p);

            //Площадь возможных внутренних треугольников
            float summ = abp + acp + bcp;

            //Отношение площадей
            float qEff = summ / abc;

            //Если отношение площадей примерно равно едиинице, то точка в треугольнике
            if (qEff > lowRange && qEff < highRange)
                return true;

            return false;
        }

        /// <summary>
        /// Площадь треуглольника по трем вершинам
        /// </summary>
        /// <param name="p1">Первая вершина</param>
        /// <param name="p2">Вторая вершина</param>
        /// <param name="p3">Третья вершина</param>
        /// <returns></returns>
        private static float TriangleSquare(ref Vector3 p1, ref Vector3 p2, ref Vector3 p3)
        {
            Vector3 v1 = p1 - p2;
            Vector3 v2 = p1 - p3;
            return (float)Math.Abs(Vector3.Cross(v1, v2).Length()) / 2;
        }

        /// <summary>
        /// Находит ближайшую точку на линии для точки point
        /// </summary>
        /// <param name="point">Исходная точка</param>
        /// <param name="lineBegin">Начало линии</param>
        /// <param name="lineEnd">Конец линии</param>
        /// <returns></returns>
        private static Vector3 ClosestPointOnLine(ref Vector3 point, ref Vector3 lineBegin, ref Vector3 lineEnd)
        {
            Vector3 diff = point - lineBegin;
            Vector3 lineDirection = lineEnd - lineBegin;
            float lineLength = lineDirection.Length();
            lineDirection.Normalize();
            float t = Vector3.Dot(lineDirection, diff);
            if (t <= 0) return lineBegin;
            if (t >= lineLength) return lineEnd;

            Vector3 ret = lineBegin + lineDirection * t;
            return ret;
        }
    }
}