//BSD License  
//DXGfx® - http://www.eteractions.com
//Copyright (c) 2005
//by Guillaume Randon
//Permission is hereby granted, free of charge, to any person obtaining a copy of this software
//and associated documentation files (the "Software"), to deal in the Software without restriction,
//including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
//and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
//subject to the following conditions:
//The above copyright notice and this permission notice shall be included in all copies or substantial
//portions of the Software.
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 
//INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE 
//AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
//DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
using System;
using System.Collections.Specialized;
using System.Collections.Generic;
using System.Text;
using Microsoft.DirectX;
using Microsoft.DirectX.Direct3D;
using System.Drawing;

namespace DXGfxLib
{
    /// <summary>
    /// This is a helper class in order to display a wireframe bounding box for our geometries
    /// </summary>
    public class DrawableBBox
    {
        VertexBuffer vBuff;
        IndexBuffer iBuff;

        public DrawableBBox(Device d3ddevice, BBox bbox)
        {
            CustomVertex.PositionOnly[] points = new CustomVertex.PositionOnly[8];

            Vector3[] submits = bbox.BuildSumits();

            for (int i = 0; i < submits.Length; i++)
            {
                points[i].Position = submits[i];
            }

            vBuff = new VertexBuffer(typeof(CustomVertex.PositionOnly), 8, d3ddevice, Usage.Dynamic, 
                CustomVertex.PositionOnly.Format,
                Pool.SystemMemory);

            GraphicsStream vbuffStm = vBuff.Lock(0, 0, 0);
            vbuffStm.Write(points);
            vBuff.Unlock();

            Int16[] indices = { 0, 1, 1, 2, 2, 3, 3, 0,
                                4, 5, 5, 6, 6, 7, 7, 4,
                                0, 4, 1, 5, 2, 6, 3, 7};


            iBuff = new IndexBuffer(d3ddevice, 24 * sizeof(Int16), Usage.Dynamic, Pool.SystemMemory, true);

            GraphicsStream ibuffStm = iBuff.Lock(0, 0, 0);
            ibuffStm.Write(indices);
            iBuff.Unlock();
        }

        public void Draw(Device d3ddevice)
        {
            d3ddevice.Indices = iBuff;
            d3ddevice.SetStreamSource(0, vBuff, 0);
            d3ddevice.VertexFormat = CustomVertex.PositionOnly.Format;
            //d3ddevice.DrawPrimitives(PrimitiveType.PointList, 0, 8);
            d3ddevice.DrawIndexedPrimitives(PrimitiveType.LineList, 0, 0, 8, 0, 12);
        }
    }

    /// <summary>
    /// Culling and collision detection is based on this class.
    /// </summary>
    public class BBox
    {
        /// <summary>
        /// A bbox is defined by two point in space that are at the extreme of the box diag. vMin is one of these two points,
        /// it is called that way because for an AABBox this is guaranteed to have the smallest values of any of the submits
        /// along all axes. For a generic BBox this is not guaranted at all as the bbox might have been rotated in space and
        /// local axes might differ from world space axes.
        /// </summary>
        public Vector3 vMin;

        /// <summary>
        /// See remarks for vMin. This is the other point in space used to define the bbox.
        /// </summary>
        public Vector3 vMax;

        /// <summary>
        /// In case we need to know where the center of the bbox is. This computed from vMin and vMax each time it is
        /// needed.
        /// </summary>
        public Vector3 Center
        {
            get
            {
                return new Vector3(vMax.X - vMin.X, vMax.Y - vMin.Y, vMax.Z - vMin.Z);
            }
        }

        /// <summary>
        /// Defautl constructor. For most of our algorithms having a bbox of null dimensions would be risky. This
        /// is purely due to our implementation. Still because of that the default
        /// constructor will in fact build a bbox with:
        /// vMin = new Vector3(-0.0005f, -0.0005f, -0.0005f);
        /// vMax = new Vector3(0.0005f, 0.0005f, 0.0005f);
        /// </summary>
        public BBox()
        {
            vMin = new Vector3(-0.0005f, -0.0005f, -0.0005f);
            vMax = new Vector3(0.0005f, 0.0005f, 0.0005f);
        }

        public void Reset()
        {
            vMin = new Vector3(-0.0005f, -0.0005f, -0.0005f);
            vMax = new Vector3(0.0005f, 0.0005f, 0.0005f);
        }

        /// <summary>
        /// This constructor will build a bbox instance from another bbox.
        /// </summary>
        /// <param name="bbox"></param>
        public BBox(BBox bbox)
        {
            vMin = bbox.vMin;
            vMax = bbox.vMax;
        }

        /// <summary>
        /// This constructor will create a Bbox for centered on the pos you give with the same size along all three axes.
        /// </summary>
        /// <param name="pos">Position of the bbox.</param>
        /// <param name="size">desired size, will be used along all three axes.</param>
        public BBox(Vector3 pos, float size)
        {
            vMin.X = pos.X - 0.5f * size;
            vMin.Y = pos.Y - 0.5f * size;
            vMin.Z = pos.Z - 0.5f * size;

            vMax.X = pos.X + 0.5f * size;
            vMax.Y = pos.Y + 0.5f * size;
            vMax.Z = pos.Z + 0.5f * size;
        }

        /// <summary>
        /// This method can transform a bbox for you from the given transformation matrix.
        /// </summary>
        /// <param name="bbox">The box you want to transform. This bbox is unafected by the call. If you want
        /// to tranform it call the non static Transform method instead!</param>
        /// <param name="mat">Transformation matrix to use. Should be a worldspace transform.</param>
        /// <returns>a reference to a bbox resulting of the transformation.</returns>
        public static BBox Transform(BBox bbox, Matrix mat)
        {
            BBox bboxRes = new BBox();

            bboxRes.vMin = Vector3.TransformCoordinate(bbox.vMin, mat);
            bboxRes.vMax = Vector3.TransformCoordinate(bbox.vMax, mat);

            return bboxRes;
        }

        /// <summary>
        /// This method will transform the bbox on which you call this method by the given transformation matrix.
        /// </summary>
        /// <param name="mat">the transform matrix to use. Should be a worldspace transform.</param>
        /// <returns>a ref to the transformed bbox.</returns>
        public BBox Transform(Matrix mat)
        {
            this.vMin.TransformCoordinate(mat);
            this.vMax.TransformCoordinate(mat);

            return this;
        }

        /// <summary>
        /// This method computes and return the size of the bbox along the three axes.
        /// </summary>
        /// <returns></returns>
        public Vector3 Size()
        {
            return (new Vector3(System.Math.Abs(vMax.X - vMin.X), System.Math.Abs(vMax.Y - vMin.Y), System.Math.Abs(vMax.Z - vMin.Z)));
        }

        /// <summary>
        /// This is in fact the center of the bbox again.
        /// </summary>
        /// <returns></returns>
        public Vector3 MinPoint()
        {
            return (new Vector3((vMax.X - vMin.X) * 0.5f + vMin.X,
                (vMax.Y - vMin.Y) * 0.5f + vMin.Y,
                (vMax.Z - vMin.Z) * 0.5f + vMin.Y));
        }

        public float MaxAAXZSize()
        {
            Vector3 diag = vMax - vMin;
            return System.Math.Max(Vector3.Dot(diag, new Vector3(1.0f, 0.0f, 0.0f)), Vector3.Dot(diag, new Vector3(0.0f, 0.0f, 1.0f)));
        }

        /// <summary>
        /// If you want to compute all the submits of your bbox this method is for you. It will return an array of 8
        /// Vector3
        /// </summary>
        /// <returns></returns>
        public Vector3[] BuildSumits()
        {
            Vector3[] summits = new Vector3[8];

            summits[0].X = vMin.X;
            summits[0].Y = vMin.Y;
            summits[0].Z = vMin.Z;

            summits[1].X = vMax.X;
            summits[1].Y = vMin.Y;
            summits[1].Z = vMin.Z;

            summits[2].X = vMax.X;
            summits[2].Y = vMin.Y;
            summits[2].Z = vMax.Z;

            summits[3].X = vMin.X;
            summits[3].Y = vMin.Y;
            summits[3].Z = vMax.Z;

            summits[4].X = vMin.X;
            summits[4].Y = vMax.Y;
            summits[4].Z = vMin.Z;

            summits[5].X = vMax.X;
            summits[5].Y = vMax.Y;
            summits[5].Z = vMin.Z;

            summits[6].X = vMax.X;
            summits[6].Y = vMax.Y;
            summits[6].Z = vMax.Z;

            summits[7].X = vMin.X;
            summits[7].Y = vMax.Y;
            summits[7].Z = vMax.Z;

            return summits;
        }

        /// <summary>
        /// This can be interesting if you attempt to build sweeping collision algorithm. It does compute a bbox from the bbox
        /// on which you call this method, from a direction vector and an elapsedTime. If elapsedTime is big and your movement
        /// is not along the same direction over that period of time this method is useless.
        /// </summary>
        /// <param name="linVel"></param>
        /// <param name="elapsedTime"></param>
        /// <returns></returns>
        public BBox BuildSwepingAABBox(Vector3 linVel, float elapsedTime)
        {
            return new AABBox(this, linVel, elapsedTime);
        }

        /// <summary>
        /// Check if provide bbox and bbox on which you call this method interpenetrate. This is a very basic test it returns
        /// only a boolean if the two bbox overlap. This can be used for positioning of scene node if you don't want to put any
        /// scene node or scene object where there is already something, it can also be interesting to perform a first selection
        /// of nodes whith which you might collide before performing finer collision testing.
        /// </summary>
        /// <param name="bbox"></param>
        /// <returns></returns>
        public bool CheckCollision(BBox bbox)
        {
            BBoxPlanes planes = new BBoxPlanes(this);


            return planes.CheckCollision(bbox);
        }

        /// <summary>
        /// This method return true if the two bbox collide with each other.
        /// </summary>
        /// <param name="bbox"></param>
        /// <returns></returns>
        public bool Collide(BBox bbox)
        {
            AABBox aabb1 = new AABBox(this);
            AABBox aabb2 = new AABBox(bbox);

            return aabb1.Collide(aabb2);
        }

        /// <summary>
        /// Use this method if you want to scale a specific bounding box.
        /// </summary>
        /// <param name="scale"></param>
        public void Scale(Vector3 scale)
        {
            Vector3 center = Center;

            Vector3 newSize = new Vector3(Size().X * scale.X, Size().Y * scale.Y, Size().Z * scale.Z);

            vMin = center - 0.5f * newSize;
            vMax = center + 0.5f * newSize;
        }
    }

    /// <summary>
    /// This enumeration contains all possible situation to a plane.
    /// </summary>
    public enum planeRelativePosition { front, back, intersect };

    /// <summary>
    /// AABBox stand for axis aligned bounding box. This is means a bouding box which remain aligned with world space axis. In
    /// some situation this can become a very coarse approximation of an ideal bounding volume. For efficiency reason the scene
    /// graph perform only on AABBox this is more efficient to build bounding volume from children. As the scene graph uses
    /// these information only to reject portion of the graph when we are sure the are out of the viewing frustrum or to 
    /// perform a first selection of node potentially colliding with another box or a ray, having
    /// big bounding volume is not that bad.
    /// </summary>
    public class AABBox : BBox
    {
        /// <summary>
        /// Default constructor. See remarks for BBox default constructor.
        /// </summary>
        public AABBox() : base()
        {
        }

        /// <summary>
        /// This build an AABBox around an existing bbox which might not be an AABBox itself.
        /// </summary>
        /// <param name="bbox"></param>
        public AABBox(BBox bbox)
        {
            Vector3[] summits = bbox.BuildSumits();

            vMin.X = MathUtil.GetSmallest(new float[] { summits[0].X, summits[1].X,
														summits[2].X, summits[3].X,
														summits[4].X, summits[5].X,
														summits[6].X, summits[7].X});
            vMin.Y = MathUtil.GetSmallest(new float[] { summits[0].Y, summits[1].Y,
														summits[2].Y, summits[3].Y,
														summits[4].Y, summits[5].Y,
														summits[6].Y, summits[7].Y});
            vMin.Z = MathUtil.GetSmallest(new float[] { summits[0].Z, summits[1].Z,
														summits[2].Z, summits[3].Z,
														summits[4].Z, summits[5].Z,
														summits[6].Z, summits[7].Z});

            vMax.X = MathUtil.GetHighest(new float[] { summits[0].X, summits[1].X,
													   summits[2].X, summits[3].X,
													   summits[4].X, summits[5].X,
													   summits[6].X, summits[7].X});
            vMax.Y = MathUtil.GetHighest(new float[] { summits[0].Y, summits[1].Y,
													   summits[2].Y, summits[3].Y,
													   summits[4].Y, summits[5].Y,
													   summits[6].Y, summits[7].Y});
            vMax.Z = MathUtil.GetHighest(new float[] { summits[0].Z, summits[1].Z,
													   summits[2].Z, summits[3].Z,
													   summits[4].Z, summits[5].Z,
													   summits[6].Z, summits[7].Z});
        }

        /// <summary>
        /// Build an AABBox around a BBox extruded along a vector v.
        /// </summary>
        /// <param name="bbox"></param>
        /// <param name="v"></param>
        public AABBox(BBox bbox, Vector3 v)
        {
            Vector3[] summits = bbox.BuildSumits();

            vMin.X = MathUtil.GetSmallest(new float[] { summits[0].X, summits[1].X,
														summits[2].X, summits[3].X,
														summits[4].X, summits[5].X,
														summits[6].X, summits[7].X,
														summits[0].X+v.X, summits[1].X+v.X,
														summits[2].X+v.X, summits[3].X+v.X,
														summits[4].X+v.X, summits[5].X+v.X,
														summits[6].X+v.X, summits[7].X+v.X});
            vMin.Y = MathUtil.GetSmallest(new float[] { summits[0].Y, summits[1].Y,
														summits[2].Y, summits[3].Y,
														summits[4].Y, summits[5].Y,
														summits[6].Y, summits[7].Y,
														summits[0].Y+v.Y, summits[1].Y+v.Y,
														summits[2].Y+v.Y, summits[3].Y+v.Y,
														summits[4].Y+v.Y, summits[5].Y+v.Y,
														summits[6].Y+v.Y, summits[7].Y+v.Y});
            vMin.Z = MathUtil.GetSmallest(new float[] { summits[0].Z, summits[1].Z,
														summits[2].Z, summits[3].Z,
														summits[4].Z, summits[5].Z,
														summits[6].Z, summits[7].Z,
														summits[0].Z+v.Z, summits[1].Z+v.Z,
														summits[2].Z+v.Z, summits[3].Z+v.Z,
														summits[4].Z+v.Z, summits[5].Z+v.Z,
														summits[6].Z+v.Z, summits[7].Z+v.Z});

            vMax.X = MathUtil.GetHighest(new float[] { summits[0].X, summits[1].X,
													   summits[2].X, summits[3].X,
													   summits[4].X, summits[5].X,
													   summits[6].X, summits[7].X,
													   summits[0].X+v.X, summits[1].X+v.X,
													   summits[2].X+v.X, summits[3].X+v.X,
													   summits[4].X+v.X, summits[5].X+v.X,
													   summits[6].X+v.X, summits[7].X+v.X});
            vMax.Y = MathUtil.GetHighest(new float[] { summits[0].Y, summits[1].Y,
													   summits[2].Y, summits[3].Y,
													   summits[4].Y, summits[5].Y,
													   summits[6].Y, summits[7].Y,
													   summits[0].Y+v.Y, summits[1].Y+v.Y,
													   summits[2].Y+v.Y, summits[3].Y+v.Y,
													   summits[4].Y+v.Y, summits[5].Y+v.Y,
													   summits[6].Y+v.Y, summits[7].Y+v.Y});
            vMax.Z = MathUtil.GetHighest(new float[] { summits[0].Z, summits[1].Z,
													   summits[2].Z, summits[3].Z,
													   summits[4].Z, summits[5].Z,
													   summits[6].Z, summits[7].Z,
													   summits[0].Z+v.Z, summits[1].Z+v.Z,
													   summits[2].Z+v.Z, summits[3].Z+v.Z,
													   summits[4].Z+v.Z, summits[5].Z+v.Z,
													   summits[6].Z+v.Z, summits[7].Z+v.Z});
        }

        /// <summary>
        /// Same as above except that the vector is used as a direction, its length will be scaled by f.
        /// </summary>
        /// <param name="bbox"></param>
        /// <param name="v"></param>
        /// <param name="f"></param>
        public AABBox(BBox bbox, Vector3 v, float f)
            : this(bbox, Vector3.Scale(v, f))
        {
        }

        public AABBox(float ExtendX, float ExtendY, float ExtendZ)
        {
            vMin.X = -0.5f * ExtendX;
            vMin.Y = -0.5f * ExtendY;
            vMin.Z = -0.5f * ExtendZ;

            vMax.X = 0.5f * ExtendX;
            vMax.Y = 0.5f * ExtendY;
            vMax.Z = 0.5f * ExtendZ;
        }

        /// <summary>
        /// This method can be used to check if an aabbox overlap with the aabbox on which the method is called.
        /// </summary>
        /// <param name="aabb"></param>
        /// <returns>The method return true if the two aabbox do overlap!</returns>
        public bool Collide(AABBox aabb)
        {
            if (aabb.vMax.X < vMin.X)
                return false;
            if (aabb.vMin.X > vMax.X)
                return false;

            if (aabb.vMax.Y < vMin.Y)
                return false;
            if (aabb.vMin.Y > vMax.Y)
                return false;

            if (aabb.vMax.Z < vMin.Z)
                return false;
            if (aabb.vMin.Z > vMax.Z)
                return false;

            // When we end up here we have not found any separating axes for our
            // AABBox
            return true;
        }

        /// <summary>
        /// Check if the aabbox passed as a parameter is completely inside the aabbox on which this method is
        /// called. Can be interesting to know in some scenarios.
        /// </summary>
        /// <param name="bbox"></param>
        /// <returns>The method does return true if the aabbox passed as a parameter is completely inside.</returns>
        public bool IsInside(AABBox bbox)
        {
            if (this.vMin.X < bbox.vMin.X)
                return false;

            if (this.vMin.Y < bbox.vMin.Y)
                return false;

            if (this.vMin.Z < bbox.vMin.Z)
                return false;

            if (this.vMax.X > bbox.vMax.X)
                return false;

            if (this.vMax.Y > bbox.vMax.Y)
                return false;

            if (this.vMax.Z > bbox.vMax.Z)
                return false;

            return true;
        }

        /// <summary>
        /// This method build an aabbox from two aabbox. The resulting aabbox contains the two aabboxes passed as parameter.
        /// </summary>
        /// <param name="bbox1"></param>
        /// <param name="bbox2"></param>
        /// <returns></returns>
        public static AABBox BoxFromBoxes(AABBox bbox1, AABBox bbox2)
        {
            AABBox bbox = new AABBox();

            bbox.vMin.X = System.Math.Min(bbox1.vMin.X, bbox2.vMin.X);
            bbox.vMin.Y = System.Math.Min(bbox1.vMin.Y, bbox2.vMin.Y);
            bbox.vMin.Z = System.Math.Min(bbox1.vMin.Z, bbox2.vMin.Z);

            bbox.vMax.X = System.Math.Max(bbox1.vMax.X, bbox2.vMax.X);
            bbox.vMax.Y = System.Math.Max(bbox1.vMax.Y, bbox2.vMax.Y);
            bbox.vMax.Z = System.Math.Max(bbox1.vMax.Z, bbox2.vMax.Z);

            return bbox;
        }

        /// <summary>
        /// Same as above except the AABBox on which we do call this method will encompass space the previous space and the
        /// space define by the other AABBox...
        /// </summary>
        /// <param name="bbox2"></param>
        /// <returns></returns>
        public AABBox BoxFromBoxes(AABBox bbox2)
        {
            this.vMin.X = System.Math.Min(this.vMin.X, bbox2.vMin.X);
            this.vMin.Y = System.Math.Min(this.vMin.Y, bbox2.vMin.Y);
            this.vMin.Z = System.Math.Min(this.vMin.Z, bbox2.vMin.Z);

            this.vMax.X = System.Math.Max(this.vMax.X, bbox2.vMax.X);
            this.vMax.Y = System.Math.Max(this.vMax.Y, bbox2.vMax.Y);
            this.vMax.Z = System.Math.Max(this.vMax.Z, bbox2.vMax.Z);

            return this;
        }

        /// <summary>
        /// This static method creates a AABBox containing the Mesh passed as a parameter.
        /// </summary>
        /// <param name="meshToUse"></param>
        /// <returns></returns>
        public static AABBox CalculateLocalBBox(Mesh meshToUse)
        {
            AABBox computedAABBox;

            if (meshToUse != null)
            {
                
                computedAABBox = new AABBox();
                // Retrieve the vertex buffer data in the mesh object
                VertexBuffer vb = meshToUse.VertexBuffer;

                // Lock the vertex buffer to generate a simple bounding sphere
                GraphicsStream vertexData = vb.Lock(0, 0, LockFlags.NoSystemLock);
                Geometry.ComputeBoundingBox(vertexData,
                    meshToUse.NumberVertices,
                    meshToUse.VertexFormat,
                    out computedAABBox.vMin, out computedAABBox.vMax);
                vb.Unlock();
                vb.Dispose();
            }
            else
            {
                throw new ArgumentNullException("SceneObject.CalculateLocalAABox() can not complete because meshToUse is null");
            }

            return computedAABBox;
        }
    }

    /// <summary>
    /// This class is used to contain the collection of planes that do constitue a bounding box.
    /// </summary>
    public class BBoxPlanes
    {
        Plane leftPlane;
        Plane rightPlane;
        Plane topPlane;
        Plane bottomPlane;
        Plane nearPlane;
        Plane farPlane;

        /// <summary>
        /// Default constructor doesn't perform anything.
        /// </summary>
        public BBoxPlanes()
        {
        }

        /// <summary>
        /// Build the planes from the BBox passed as parameter.
        /// </summary>
        /// <param name="bbox"></param>
        public BBoxPlanes(BBox bbox)
        {
            BuildPlanes(bbox);
        }

        /// <summary>
        /// Build the planes from the BBox passed as parameter.
        /// </summary>
        /// <param name="bbox"></param>
        public void BuildPlanes(BBox bbox)
        {
            leftPlane = Plane.FromPoints(bbox.vMin, new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMin.Z),
                new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMax.Z));
            rightPlane = Plane.FromPoints(bbox.vMax, new Vector3(bbox.vMax.X, bbox.vMin.Y, bbox.vMax.Z),
                new Vector3(bbox.vMax.X, bbox.vMin.Y, bbox.vMin.Z));
            topPlane = Plane.FromPoints(bbox.vMax, new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMax.Z),
                new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMin.Z));
            bottomPlane = Plane.FromPoints(bbox.vMin, new Vector3(bbox.vMax.X, bbox.vMin.Y, bbox.vMin.Z),
                new Vector3(bbox.vMax.X, bbox.vMin.Y, bbox.vMax.Z));
            nearPlane = Plane.FromPoints(bbox.vMin, new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMin.Z),
                new Vector3(bbox.vMax.X, bbox.vMax.Y, bbox.vMin.Z));
            farPlane = Plane.FromPoints(bbox.vMax, new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMax.Z),
                new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMin.Z));
        }

        /// <summary>
        /// Check collision of the specified bbox with the plane currently stored in the member variables.
        /// Return true if there is an overlap between space regions.
        /// </summary>
        /// <param name="bbox"></param>
        /// <returns></returns>
        public bool CheckCollision(BBox bbox)
        {
            if ((MathUtil.GetPlaneRelativePosition(leftPlane, bbox) == planeRelativePosition.back)
                || (MathUtil.GetPlaneRelativePosition(rightPlane, bbox) == planeRelativePosition.back)
                || (MathUtil.GetPlaneRelativePosition(topPlane, bbox) == planeRelativePosition.back)
                || (MathUtil.GetPlaneRelativePosition(bottomPlane, bbox) == planeRelativePosition.back)
                || (MathUtil.GetPlaneRelativePosition(nearPlane, bbox) == planeRelativePosition.back)
                || (MathUtil.GetPlaneRelativePosition(farPlane, bbox) == planeRelativePosition.back))
            {
                return false;
            }

            return true;
        }
    }

    /// <summary>
    /// This class is not meant to be instanciated. It is just a convenient way to regroup helper functions. They are
    /// defined as static method of the class.
    /// </summary>
    public class MathUtil
    {
        /// <summary>
        /// This build a ray in 3D space from screen positions. This is usefull for example when we click on the application
        /// window and we know on which object in our 3D world we did click. This needs a valid viewport and the right
        /// projection and view matrices that has been used to render the image currently displayed.
        /// </summary>
        /// <param name="viewport"></param>
        /// <param name="proj"></param>
        /// <param name="view"></param>
        /// <param name="screenPos"></param>
        /// <param name="rayOrig"></param>
        /// <param name="rayDir"></param>
        public static void RayFromScreenPos(Viewport viewport, Matrix proj, Matrix view, Vector2 screenPos, out Vector3 rayOrig, out Vector3 rayDir)
        {
            rayOrig = new Vector3(screenPos.X, screenPos.Y, 0.0f);
            rayOrig.Unproject(viewport, proj, view, Matrix.Identity);

            Vector3 rayDest = new Vector3(screenPos.X, screenPos.Y, 1.0f);
            rayDest.Unproject(viewport, proj, view, Matrix.Identity);

            rayDir = Vector3.Subtract(rayDest, rayOrig);

            rayDir.Normalize();
        }

        /// <summary>
        /// Extract a vector from a 4x4 matrix. If the 4x4 matrix is the world matrix used to position a node in 3D space
        /// this will extract the node position.
        /// </summary>
        /// <param name="mat"></param>
        /// <returns></returns>
        public static Vector3 PositionFromMat(Matrix mat)
        {
            return new Vector3(mat.M41, mat.M42, mat.M43);
        }

        /// <summary>
        /// This does extract Yaw Pitch Roll value from a quaternion. Values are in radiant.
        /// Yaw = returnVector.X Pitch = returnVector.Y Roll = returnVector.Z
        /// </summary>
        /// <param name="quat"></param>
        /// <returns></returns>
        public static Vector3 YawPitchRollFromQuat(Quaternion quat)
        {
            Matrix mat = Matrix.RotationQuaternion(quat);
            return YawPitchRollFromMat(mat);
        }

        /// <summary>
        /// This does extract Yaw Pitch Roll from a 4x4 matrix. Please see above for description of the return vector.
        /// Once again values are radiant.
        /// </summary>
        /// <param name="mat"></param>
        /// <returns></returns>
        public static Vector3 YawPitchRollFromMat(Matrix mat)
        {
            float yaw = 0.0f;
            float roll = 0.0f;

            float pitch = (float)Math.Asin(-mat.M32);

            double threshold = 0.001; // Hardcoded constant

            double test = Math.Cos(pitch);

            if (test > threshold)
            {

                roll = (float)Math.Atan2(mat.M12, mat.M22);

                yaw = (float)Math.Atan2(mat.M31, mat.M33);

            }

            else
            {

                roll = (float)Math.Atan2(-mat.M21, mat.M11);

                yaw = 0.0f;

            }

            return new Vector3(yaw, pitch, roll);
        }

        public static float ResetToNullIfTooSmall(float val, double threshold)
        {
            float res = 0;
            if (Math.Abs(val) > threshold)
            {
                res = val;
            }
            return res;
        }

        /// <summary>
        /// Same as YawPitchRollFromMat except the return values are in degrees.
        /// </summary>
        /// <param name="mat"></param>
        /// <returns></returns>
        public static Vector3 YawPitchRollFromMatInDegree(Matrix mat)
        {
            Vector3 ypr = YawPitchRollFromMat(mat);

            return new Vector3(Geometry.RadianToDegree(ypr.X), Geometry.RadianToDegree(ypr.Y), 
                Geometry.RadianToDegree(ypr.Z));
        }

        /// <summary>
        /// In case you need to transform radian to degree the result from YawPitchRoll functions.
        /// </summary>
        /// <param name="yawpitchroll"></param>
        /// <returns></returns>
        public static Vector3 RadianToDegree(Vector3 yawpitchroll)
        {
            Vector3 yawpitchrollOut = new Vector3();
            yawpitchrollOut.X = Geometry.RadianToDegree(yawpitchroll.X);
            yawpitchrollOut.Y = Geometry.RadianToDegree(yawpitchroll.Y);
            yawpitchrollOut.Z = Geometry.RadianToDegree(yawpitchroll.Z);
            return yawpitchrollOut;
        }

        /// <summary>
        /// Symetric function to RadianToDegree.
        /// </summary>
        /// <param name="yawpitchroll"></param>
        /// <returns></returns>
        public static Vector3 DegreeToRadian(Vector3 yawpitchroll)
        {
            Vector3 yawpitchrollOut = new Vector3();
            yawpitchrollOut.X = Geometry.DegreeToRadian(yawpitchroll.X);
            yawpitchrollOut.Y = Geometry.DegreeToRadian(yawpitchroll.Y);
            yawpitchrollOut.Z = Geometry.DegreeToRadian(yawpitchroll.Z);
            return yawpitchrollOut;
        }

        /// <summary>
        /// The return value will be val clamped by low and high.
        /// </summary>
        /// <param name="val"></param>
        /// <param name="low"></param>
        /// <param name="high"></param>
        /// <returns></returns>
        public static double Clamp(double val, double low, double high)
        {
            double res;

            if (val > high)
                res = high;
            else if (val < low)
                res = low;
            else
                res = val;

            return res;
        }

        /// <summary>
        /// This does return the signed distance to a plane. Interesting to know on which side we stand.
        /// </summary>
        /// <param name="plane"></param>
        /// <param name="point"></param>
        /// <returns></returns>
        public static float SignedDistance(Plane plane, Vector3 point)
        {
            return plane.Dot(point);
        }

        /// <summary>
        /// This function is helpful to determine on which side we stand or if we do intersect. See PlaneRelativePosition
        /// enumeration for possible results.
        /// </summary>
        /// <param name="plane"></param>
        /// <param name="bbox"></param>
        /// <returns></returns>
        public static planeRelativePosition GetPlaneRelativePosition(Plane plane, BBox bbox)
        {
            Vector3 minPoint, maxPoint;

            if (plane.A > 0.0f)
            {
                minPoint.X = bbox.vMin.X;
                maxPoint.X = bbox.vMax.X;
            }
            else
            {
                minPoint.X = bbox.vMax.X;
                maxPoint.X = bbox.vMin.X;
            }

            if (plane.B > 0.0f)
            {
                minPoint.Y = bbox.vMin.Y;
                maxPoint.Y = bbox.vMax.Y;
            }
            else
            {
                minPoint.Y = bbox.vMax.Y;
                maxPoint.Y = bbox.vMin.Y;
            }

            if (plane.C > 0.0f)
            {
                minPoint.Z = bbox.vMin.Z;
                maxPoint.Z = bbox.vMax.Z;
            }
            else
            {
                minPoint.Z = bbox.vMax.Z;
                maxPoint.Z = bbox.vMin.Z;
            }

            float dmin = SignedDistance(plane, minPoint);
            float dmax = SignedDistance(plane, maxPoint);

            if (dmin * dmax < 0.0f)
            {
                return planeRelativePosition.intersect;
            }
            else if (dmin > 0.0f)
            {
                return planeRelativePosition.front;
            }

            return planeRelativePosition.back;
        }

        /// <summary>
        /// This function will return the smallest value from the provided vector.
        /// </summary>
        /// <param name="vec"></param>
        /// <returns></returns>
        public static float GetSmallest(float[] vec)
        {
            float res = vec[0];

            for (int i = 1; i < vec.Length; i++)
            {
                if (vec[i] < res)
                    res = vec[i];
            }
            return res;
        }

        /// <summary>
        /// Will return the highest value in the procided vector.
        /// </summary>
        /// <param name="vec"></param>
        /// <returns></returns>
        public static float GetHighest(float[] vec)
        {
            float res = vec[0];

            for (int i = 1; i < vec.Length; i++)
            {
                if (vec[i] > res)
                    res = vec[i];
            }
            return res;
        }

        /// <summary>
        /// Will copy the values from matSrc to matDest values.
        /// </summary>
        /// <param name="matDest"></param>
        /// <param name="matSrc"></param>
        public static void CopyMatrixValues(Matrix matDest, Matrix matSrc)
        {
            matDest.M11 = matSrc.M11;
            matDest.M12 = matSrc.M12;
            matDest.M13 = matSrc.M13;
            matDest.M14 = matSrc.M14;

            matDest.M21 = matSrc.M21;
            matDest.M22 = matSrc.M22;
            matDest.M23 = matSrc.M23;
            matDest.M24 = matSrc.M24;

            matDest.M31 = matSrc.M31;
            matDest.M32 = matSrc.M32;
            matDest.M33 = matSrc.M33;
            matDest.M34 = matSrc.M34;

            matDest.M41 = matSrc.M41;
            matDest.M42 = matSrc.M42;
            matDest.M43 = matSrc.M43;
            matDest.M44 = matSrc.M44;
        }

        /// <summary>
        /// Compute the distance of one scene node to another.
        /// </summary>
        /// <param name="n1"></param>
        /// <param name="n2"></param>
        /// <returns></returns>
        public static float Distance(SceneNode n1, SceneNode n2)
        {
            return Distance(n1.Position, n2.Position);
        }

        /// <summary>
        /// Compute the distance from one point in space to another.
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <returns></returns>
        public static float Distance(Vector3 v1, Vector3 v2)
        {
            Vector3 v = Vector3.Subtract(v1,v2);
            return v.Length();
        }

        /// <summary>
        /// Extract a rotation only matrix from a 4x4 matrix.
        /// </summary>
        /// <param name="mat"></param>
        /// <returns></returns>
        public static Matrix ExtractRot(Matrix mat)
        {
            Matrix rot = new Matrix();

            CopyMatrixValues(rot, mat);

            rot.M41 = 0.0f;
            rot.M42 = 0.0f;
            rot.M43 = 0.0f;

            return rot;
        }

        /// <summary>
        /// Extract position from a 4x4 matrix.
        /// </summary>
        /// <param name="mat"></param>
        /// <returns></returns>
        public static Vector3 ExtractPosition(Matrix mat)
        {
            return new Vector3(mat.M41, mat.M42, mat.M43);
        }
    }

    /// <summary>
    /// This class is used to calculate a but mask around bouding boxes. Could be use to further refine frustrum culling.
    /// Not used at the moment.
    /// </summary>
    public class ZMask
    {
        public UInt32 zMask = 0;
        private static UInt32[] masks;

        public ZMask()
        {
            if (masks == null)
            {
                masks = new UInt32[32];

                for (int i = 0; i < 32; i++)
                {
                    masks[i] = (UInt32)System.Math.Pow(2, i);
                }
            }
        }

        public void Fusion(ZMask zMask2)
        {
            zMask = zMask | (zMask2.zMask);
        }

        public void Calculate(BBox objectBBox, BBox worldBBox)
        {
            float zSlice = (worldBBox.vMax.Y - worldBBox.vMin.Y) / 32.0f;

            int baseBit = (int)System.Math.Floor(objectBBox.vMin.Y / zSlice);
            int highBit = (int)System.Math.Ceiling(objectBBox.vMax.Y / zSlice);

            for (UInt32 i = 0; i < 32; i++)
            {
                if ((i >= baseBit) && (i <= highBit))
                {
                    zMask = zMask | masks[i];
                }
            }
        }
    }
}
