C:/Doug/CSharp/XnaCollisionLib/XnaCollisionLib/CollisionManager.cs

Go to the documentation of this file.

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

namespace XnaCollisionLib
{
    public struct CollisionData
    {
        public Vector3 ERadius;
        public Vector3 R3Velocity;
        public Vector3 R3Position;
        public Vector3 Velocity;
        public Vector3 NormalizedVelocity;
        public Vector3 BasePoint;
        public bool FoundCollision;
        public float NearestDistance;
        public Vector3 IntersectionPoint;
        public Triangle IntersectionTriangle;
        public int TriangleIndex;
        public int TriangleHits;
        public float SlidingSpeed;
    }

    public class CollisionManager
    {
        private CollisionSource _Source;
        private List<Triangle> _Triangles;

        public CollisionManager()
        {
            _Source = new CollisionSource();
            _Triangles = new List<Triangle>();
        }

        public CollisionSource Source
        {
            get { return _Source; }
        }

        public Nullable<float> GetRayCollisionResult(Ray ray, out Triangle hitTriangle)
        {
            return _Source.ISect(ray, out hitTriangle);
        }

        public Vector3 GetCollisionResult(
            Vector3 position,
            Vector3 radius,
            Vector3 velocity,
            Vector3 gravity,
            float slidingSpeed,
            out Triangle hitTriangle,
            out Vector3 hitPosition,
            out bool falling
            )
        {
            hitTriangle = new Triangle();
            hitPosition = new Vector3();
            falling = false;

            return CollideWithSource(position, radius, velocity, gravity, slidingSpeed, ref hitTriangle, ref hitPosition, ref falling);
        }

        private Vector3 CollideWithSource(
            Vector3 position,
            Vector3 radius,
            Vector3 velocity,
            Vector3 gravity,
            float slidingSpeed,
            ref Triangle hitTriangle,
            ref Vector3 hitPosition,
            ref bool falling
            )
        {
            if (Source.Count == 0 || radius == Vector3.Zero)
                return position + velocity;

            _Triangles.Capacity = Source.Count;

            CollisionData colData = new CollisionData();

            colData.R3Position = position;
            colData.R3Velocity = velocity;
            colData.ERadius = radius;
            colData.NearestDistance = float.MaxValue;
            colData.SlidingSpeed = slidingSpeed;
            colData.TriangleHits = 0;
            colData.TriangleIndex = -1;

            Vector3 eSpacePosition = colData.R3Position / colData.ERadius;
            Vector3 eSpaceVelocity = colData.R3Velocity / colData.ERadius;
            Vector3 finalPosition = CollideWithSource(0, ref colData, eSpacePosition, eSpaceVelocity);

            falling = false;
            if (gravity != Vector3.Zero)
            {
                colData.R3Position = finalPosition * colData.ERadius;
                colData.R3Velocity = gravity;
                colData.TriangleHits = 0;

                eSpaceVelocity = gravity / colData.ERadius;
                finalPosition = CollideWithSource(0, ref colData, finalPosition, eSpaceVelocity);
                falling = colData.TriangleHits == 0;
            }
            if (colData.TriangleHits != 0)
            {
                hitTriangle = colData.IntersectionTriangle;
                hitTriangle.A *= colData.ERadius;
                hitTriangle.B *= colData.ERadius;
                hitTriangle.C *= colData.ERadius;
            }
            finalPosition *= colData.ERadius;
            hitPosition = colData.IntersectionPoint * colData.ERadius;

            return finalPosition;
        }

        private Vector3 CollideWithSource(int recursionDepth, ref CollisionData colData, Vector3 position, Vector3 velocity)
        {
            float veryCloseDistance = colData.SlidingSpeed;

            if (recursionDepth > 5)
                return position;

            colData.Velocity = velocity;
            colData.NormalizedVelocity = Vector3.Normalize(velocity);
            colData.BasePoint = position;
            colData.FoundCollision = false;
            colData.NearestDistance = float.MaxValue;

            BoundingBox box = new BoundingBox(colData.R3Position, colData.R3Position);

            CollisionHelper.AddToBox(ref box, colData.R3Position + colData.R3Velocity);
            box.Min -= colData.ERadius;
            box.Max += colData.ERadius;

            Matrix scaleMatrix = Matrix.CreateScale(new Vector3(1, 1, 1) / colData.ERadius);

            _Triangles.Clear();

            Source.Query(box, _Triangles, scaleMatrix);

            for (int i = 0; i != _Triangles.Count; i++)
                if (TestTriangleIntersection(ref colData, _Triangles[i]))
                    colData.TriangleIndex = i;

            if (!colData.FoundCollision)
                return position + velocity;

            Vector3 destinationPoint = position + velocity;
            Vector3 newBasePoint = position;

            if (colData.NearestDistance >= veryCloseDistance)
            {
                Vector3 v = Vector3.Normalize(velocity) * (colData.NearestDistance - veryCloseDistance);

                newBasePoint = colData.BasePoint + v;
                v.Normalize();
                colData.IntersectionPoint -= (v * veryCloseDistance);
            }

            Vector3 slidePlaneOrigin = colData.IntersectionPoint;
            Vector3 slidePlaneNormal = Vector3.Normalize(newBasePoint - colData.IntersectionPoint);
            Plane slidingPlane = CollisionHelper.CreatePlane(slidePlaneNormal, slidePlaneOrigin);
            Vector3 newDestinationPoint = destinationPoint - (slidePlaneNormal * CollisionHelper.DistanceTo(slidingPlane, destinationPoint));
            Vector3 newVelocity = newDestinationPoint - colData.IntersectionPoint;

            if (newVelocity.Length() < veryCloseDistance)
                return newBasePoint;

            return CollideWithSource(recursionDepth + 1, ref colData, newBasePoint, newVelocity);
        }

        private bool TestTriangleIntersection(ref CollisionData colData, Triangle triangle)
        {
            Plane trianglePlane = triangle.Plane;

            if (!triangle.IsFronFacing(colData.NormalizedVelocity))
                return false;

            float t0, t1;
            bool embeddedInPlane = false;
            float signedDistToTrianglePlane = CollisionHelper.DistanceTo(trianglePlane, colData.BasePoint);
            float normalDotVelocity = Vector3.Dot(trianglePlane.Normal, colData.Velocity);

            if (CollisionHelper.IsZero(normalDotVelocity))
            {
                if (signedDistToTrianglePlane >= 1)
                    return false;
                else
                {
                    embeddedInPlane = true;
                    t0 = 0;
                    t1 = 1;
                }
            }
            else
            {
                normalDotVelocity = CollisionHelper.Reciprocal(normalDotVelocity);

                t0 = (-1.0f - signedDistToTrianglePlane) * normalDotVelocity;
                t1 = (1.0f - signedDistToTrianglePlane) * normalDotVelocity;

                if (t0 > t1)
                {
                    float temp = t1;

                    t1 = t0;
                    t0 = temp;
                }

                if (t0 > 1.0f || t1 < 0.0f)
                    return false;

                t0 = CollisionHelper.Clamp(t0, 0.0f, 1.0f);
                t1 = CollisionHelper.Clamp(t1, 0.0f, 1.0f);
            }

            Vector3 collisionPoint = new Vector3(0, 0, 0);
            bool foundCollision = false;
            float t = 1.0f;

            if (!embeddedInPlane)
            {
                Vector3 planeIntersectionPoint = (colData.BasePoint - trianglePlane.Normal) + colData.Velocity * t0;

                if (triangle.IsPointInside(planeIntersectionPoint))
                {
                    foundCollision = true;
                    t = t0;
                    collisionPoint = planeIntersectionPoint;
                }
            }

            if (!foundCollision)
            {
                Vector3 velocity = colData.Velocity;
                Vector3 basePt = colData.BasePoint;
                float velocityLenSq = velocity.LengthSquared();
                float a, b, c, newT;

                a = velocityLenSq;
                b = 2.0f * Vector3.Dot(velocity, basePt - triangle.A);
                c = (triangle.A - basePt).LengthSquared() - 1.0f;
                if (GetLowestRoot(a, b, c, t, out newT))
                {
                    t = newT;
                    foundCollision = true;
                    collisionPoint = triangle.A;
                }

                if (!foundCollision)
                {
                    b = 2.0f * Vector3.Dot(velocity, basePt - triangle.B);
                    c = (triangle.B - basePt).LengthSquared() - 1.0f;
                    if (GetLowestRoot(a, b, c, t, out newT))
                    {
                        t = newT;
                        foundCollision = true;
                        collisionPoint = triangle.B;
                    }
                }

                if (!foundCollision)
                {
                    b = 2.0f * Vector3.Dot(velocity, basePt - triangle.C);
                    c = (triangle.C - basePt).LengthSquared() - 1.0f;
                    if (GetLowestRoot(a, b, c, t, out newT))
                    {
                        t = newT;
                        foundCollision = true;
                        collisionPoint = triangle.C;
                    }
                }

                Vector3 edge = triangle.B - triangle.A;
                Vector3 baseToVertex = triangle.A - basePt;
                float edgeLenSq = edge.LengthSquared();
                float edgeDotVelocity = Vector3.Dot(edge, velocity);
                float edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);

                a = edgeLenSq * -velocityLenSq + edgeDotVelocity * edgeDotVelocity;
                b = edgeLenSq * (2.0f * Vector3.Dot(velocity, baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
                c = edgeLenSq * (1.0f - baseToVertex.LengthSquared()) + edgeDotBaseToVertex * edgeDotBaseToVertex;

                if (GetLowestRoot(a, b, c, t, out newT))
                {
                    float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeLenSq;
                    if (f >= 0.0f && f <= 1.0f)
                    {
                        t = newT;
                        foundCollision = true;
                        collisionPoint = triangle.A + (edge * f);
                    }
                }

                edge = triangle.C - triangle.B;
                baseToVertex = triangle.B - basePt;
                edgeLenSq = edge.LengthSquared();
                edgeDotVelocity = Vector3.Dot(edge,velocity);
                edgeDotBaseToVertex = Vector3.Dot(edge,baseToVertex);

                a = edgeLenSq * -velocityLenSq + edgeDotVelocity * edgeDotVelocity;
                b = edgeLenSq * (2.0f * Vector3.Dot(velocity, baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
                c = edgeLenSq * (1.0f - baseToVertex.LengthSquared()) + edgeDotBaseToVertex * edgeDotBaseToVertex;

                if (GetLowestRoot(a, b, c, t, out newT))
                {
                    float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeLenSq;
                    if (f >= 0.0f && f <= 1.0f)
                    {
                        t = newT;
                        foundCollision = true;
                        collisionPoint = triangle.B + (edge * f);
                    }
                }

                edge = triangle.A - triangle.C;
                baseToVertex = triangle.C - basePt;
                edgeLenSq = edge.LengthSquared();
                edgeDotVelocity = Vector3.Dot(edge, velocity);
                edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);

                a = edgeLenSq * -velocityLenSq + edgeDotVelocity * edgeDotVelocity;
                b = edgeLenSq * (2.0f * Vector3.Dot(velocity, baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
                c = edgeLenSq * (1.0f - baseToVertex.LengthSquared()) + edgeDotBaseToVertex * edgeDotBaseToVertex;

                if (GetLowestRoot(a, b, c, t, out newT))
                {
                    float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeLenSq;
                    if (f >= 0.0f && f <= 1.0f)
                    {
                        t = newT;
                        foundCollision = true;
                        collisionPoint = triangle.C + (edge * f);
                    }
                }
            }

            if (foundCollision)
            {
                float distToCollision = t * colData.Velocity.Length();

                if (!colData.FoundCollision || distToCollision < colData.NearestDistance)
                {
                    colData.NearestDistance = distToCollision;
                    colData.IntersectionPoint = collisionPoint;
                    colData.FoundCollision = true;
                    colData.IntersectionTriangle = triangle;
                    colData.TriangleHits++;

                    return true;
                }
            }
            return false;
        }

        private bool GetLowestRoot(float a, float b, float c, float t, out float newT)
        {
            float determinant = b * b - 4.0f * a * c;

            newT = -1;

            if (determinant < 0.0f) return false;

            float sqrtD = (float)Math.Sqrt(determinant);

            float r1 = (-b - sqrtD) / (2 * a);
            float r2 = (-b + sqrtD) / (2 * a);

            if (r1 > r2)
            {
                float temp = r2;

                r2 = r1;
                r1 = temp;
            }

            if (r1 > 0 && r1 < t)
            {
                newT = r1;

                return true;
            }

            if (r2 > 0 && r2 < t)
            {
                newT = r2;

                return true;
            }
            return false;
        }
    }
}