using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using VisiMath.Geometry;

namespace VisiMath.Collisions
{
    /// <summary>
    /// GJK-EPA convex hulls collision solver
    /// </summary>
    /// <remarks>Based on GJK EPA solver by Nathanael Person</remarks>
    public class GJK_EPASolver
    {
        #region Privates

        private static readonly int HashTabSize = 128;  //must be 2^n!
        private static readonly int HashMask = HashTabSize - 1;

        private static readonly float GJKSimplexEpsilon = 0.01f;
        private static readonly float GJKSimplexEpsilonSQ = GJKSimplexEpsilon * GJKSimplexEpsilon;
        private static readonly float EPAFaceEpsilon = 0.01f;

        private static readonly int GJKMaxIterations = 128;
        private static readonly int EPAMaxIterations = 256;

        #region Hashtable for supporting vertices
        class HashTabItem
        {
            public Vector3 Ray;
            public HashTabItem Next;

            public HashTabItem() { }
            
            public HashTabItem(ref Vector3 ray, HashTabItem next)
            {
                Ray.Assign(ref ray);
                Next = next;
            }
        }

        private HashTabItem[] HashTable = new HashTabItem[HashTabSize];

        #endregion

        #region minkowski
        struct MinkowskiVertice
        {
            public Vector3 Vertice;
            public Vector3 Ray;
        }

        private static void SwapSimplex(ref MinkowskiVertice ver1, ref MinkowskiVertice ver2)
        {
            MinkowskiVertice t = ver1;
            ver1 = ver2;
            ver2 = t;
        }

        private MinkowskiVertice[] Simplex = new MinkowskiVertice[5];
        #endregion
        #region EPA data
        //note, faces are currently made as a linked list... which means new objects creation in each frame for a lot of objects - garbage compiler problem
        //todo - use some fixed array

        private class Face
        {
            public MinkowskiVertice[] Vertices = new MinkowskiVertice[3];
            public Face[] Faces = new Face[3];
            public int[] e = new int[3];
            public Vector3 Normal;
            public float Distance;
            public int Mark;
            public Face Next;
            public Face Previous;
        }

        private Face RootFace;
        private int Faces;
        private Vector3[,] Features = new Vector3[2, 3];
        public bool EPAFailed = false;

        #endregion


        protected IConvexObject[] Shapes = new IConvexObject[2];
        protected Matrix4 MatrixTo0;  //from 1 to 0 space
        protected Matrix3 MatrixTo1;  //from 0 to 1 space
        protected Vector3[] Positions = new Vector3[2];
        protected int GJKOrder = -1;

        protected Vector3 _Ray;

        #endregion

        /// <summary>
        /// GJK phase error margin
        /// </summary>
        public float GJKMargin = 0.05f;

        /// <summary>
        /// GJK iterations used
        /// </summary>
        public int GJKIterations;

        /// <summary>
        /// EPA iterations used
        /// </summary>
        public int EPAIterations;
        public float EPAAccuracy = 0.001f;


        protected Matrix4 Identity = Matrix4.Identitity;

        /// <summary>
        /// Performs quick collision check using GJK algorithm, both objects are already transformed to world space
        /// </summary>
        /// <param name="shape1">Physical shape 1 (should be more complicated one)</param>
        /// <param name="shape2">Physical shape 2</param>
        /// <returns>True if objects (with their envelopes) collide</returns>
        public bool Collide(IConvexObject shape1, IConvexObject shape2)
        {
            //Use GJK algorithm to detect origin
            Init(shape1, ref Identity, shape2, ref Identity);   //make support for no transformations here!!!
            return GJKCollisionTest();
        }


        /// <summary>
        /// Performs quick collision check using GJK algorithm
        /// </summary>
        /// <param name="shape1">Physical shape 1 (should be more complicated one)</param>
        /// <param name="mat1">world transformation for shape 1</param>
        /// <param name="shape2">Physical shape 2</param>
        /// <param name="mat2">world transformation for shape 2</param>
        /// <returns>True if objects (with their envelopes) collide</returns>
        public bool Collide(IConvexObject shape1, ref Matrix4 mat1, IConvexObject shape2, ref Matrix4 mat2)
        {
            //Use GJK algorithm to detect origin
            Init(shape1, ref mat1, shape2, ref mat2);
            return GJKCollisionTest();            
        }

        /// <summary>
        /// Performs full collision test for two objects using GJK and EPA algorithm
        /// </summary>
        /// <param name="shape1">Physical shape 1 (should be more complicated one)</param>
        /// <param name="mat1">world transformation for shape 1</param>
        /// <param name="shape2">Physical shape 2</param>
        /// <param name="mat2">world transformation for shape 2</param>
        /// <returns>True if objects (with their envelopes) collide</returns>
        public bool Collide(IConvexObject shape1, ref Matrix4 mat1, IConvexObject shape2, ref Matrix4 mat2, ref CollisionInfo info)
        {
            //Use GJK algorithm to quick intersection detection
            if (!Collide(shape1, ref mat1, shape2, ref mat2))
                return false;
            //collision occured, use EPA to fill the data
            return EPAPenetration(ref info);
        }

        /// <summary>
        /// Returns closest distance between two objects
        /// </summary>
        /// <param name="shape1">Physical shape 1 (should be more complicated one)</param>
        /// <param name="mat1">world transformation for shape 1</param>
        /// <param name="shape2">Physical shape 2</param>
        /// <param name="mat2">world transformation for shape 2</param>
        /// <returns>Distance between objects or zero if penetrating</returns>
        public float ClosestDistance(IConvexObject shape1, ref Matrix4 mat1, IConvexObject shape2, ref Matrix4 mat2)
        {
            if (Collide(shape1, ref mat1, shape2, ref mat2))
                return 0;
            CollisionInfo info = new CollisionInfo();
            EPAPenetration(ref info);
            return info.ContactPoint1.Distance(ref info.ContactPoint2);//check this!
        }

        /// <summary>
        /// Initializes solver data
        /// </summary>
        /// <param name="shape1">Physical shape 1 (should be more complicated one)</param>
        /// <param name="mat1">world transformation for shape 1</param>
        /// <param name="shape2">Physical shape 2</param>
        /// <param name="mat2">world transformation for shape 2</param>
        private void Init(IConvexObject shape1, ref Matrix4 mat1, IConvexObject shape2, ref Matrix4 mat2)
        {
            Shapes[0] = shape1;
            Shapes[1] = shape2;

            Matrix3 tmp = mat1.Rotation;

            MatrixTo1 = mat2.Rotation;
            MatrixTo1 = Matrix3.TransposeMultiply(ref MatrixTo1, ref tmp);
            MatrixTo0 = Matrix4.InverseMul(ref mat1, ref mat2);
        }

        /// <summary>
        /// Perform preliminary colliction test using GJK and prepare data for EPA
        /// </summary>
        /// <returns></returns>
        private bool GJKCollisionTest()
        {
            _Ray = new Vector3(0, 0, 1);
            GJKOrder = -1;

            for (int i = 0; i < HashTable.Length; i++)
                HashTable[i] = null;        //OPTIMIZE this for garbage collector
            FetchSupport();

            _Ray = -Simplex[0].Vertice;

            for (GJKIterations = 0; GJKIterations < GJKMaxIterations; ++GJKIterations)
            {
                float rayLength = _Ray.Length;
                if (rayLength > 0)
                    _Ray.Divide(rayLength);     //Use normal normalize??

                if (FetchSupport())
                {
                    Vector3 v1, v2, v3, v4;
                    switch (GJKOrder)
                    {
                        case 1:
                            v1 = new Vector3(ref Simplex[1].Vertice, -1);
                            v2 = Vector3.Substract(ref Simplex[0].Vertice, ref Simplex[1].Vertice);
                            if (SolveSimplex2(ref v1, ref v2))
                                return true;
                            break;
					    case 2: 
                            v1 = new Vector3(ref Simplex[2].Vertice, -1);
                            v2 = Vector3.Substract(ref Simplex[1].Vertice, ref Simplex[2].Vertice);
                            v3 = Vector3.Substract(ref Simplex[0].Vertice, ref Simplex[2].Vertice);
                            if (SolveSimplex3(ref v1, ref v2, ref v3))
                                return true;
                            break;
					    case 3:
                            v1 = new Vector3(ref Simplex[3].Vertice, -1);
                            v2 = Vector3.Substract(ref Simplex[2].Vertice, ref Simplex[3].Vertice);
                            v3 = Vector3.Substract(ref Simplex[1].Vertice, ref Simplex[3].Vertice);
                            v4 = Vector3.Substract(ref Simplex[0].Vertice, ref Simplex[3].Vertice);
                            if (SolveSimplex4(ref v1, ref v2, ref v3, ref v4))
                                return true;
                            break;
                    }
                }
                else return false;
            }
            //searching failed!!!
            return false;
        }

        private bool EncloseOrigin()
        {
            switch (GJKOrder)
            {
                case 0: 
                    //point
                    break;
                case 1:
                    //line
                    Vector3 ab = Vector3.Substract(ref Simplex[1].Vertice, ref Simplex[0].Vertice);

                    Vector3 b0 = ab.Cross(Vector3.UnitX);
                    Vector3 b1 = ab.Cross(Vector3.UnitY);
                    Vector3 b2 = ab.Cross(Vector3.UnitZ);

                    float m0 = b0.LengthSquared;
                    float m1 = b1.LengthSquared;
                    float m2 = b2.LengthSquared;

                    ab.Normalize();
                    Matrix4 mat = Matrix4.CreateRotationAxisAngle(ab, MathTools.DoublePi / 3);

                    Vector3 w;
                    if (m0 > m1)
                    {
                        if (m0 > m2)
                            w = b0;
                        else w = b2;
                    }
                    else
                    {
                        if (m1 > m2)
                            w = b1;
                        else 
                            w = b2;
                    }

                    w.Normalize();

                    Support(ref w, ref Simplex[4]);
                    mat.TransformNormalFast(ref w);
                    w.Normalize();
                    Support(ref w, ref Simplex[2]); 
                    mat.TransformNormalFast(ref w);
                    Support(ref w, ref Simplex[3]); 
                    GJKOrder = 4;
                    return true;
                
                case 2:
                    //Triangle
                    
                    Vector3 n = Vector3.Normalize(Vector3.Cross(Simplex[1].Vertice - Simplex[0].Vertice, Simplex[2].Vertice - Simplex[0].Vertice));
                    Support(ref n, ref Simplex[3]);
                    n.Mul(-1);
                    Support(ref n, ref Simplex[4]);
                    GJKOrder = 4;
                    return true;

                case 3: 
                    //tetrahedron
                    return true;
                case 4: 
                    //hexahedron
                    return true;
            }
            return false;   //either point or unknown
        }

        private int[,] fidx;
        private int[,] eidx;

        MinkowskiVertice[] EPAbasemkv = new MinkowskiVertice[5];

        private bool EPAPenetration(ref CollisionInfo info)
        {
            Face bestface = null;
            int markid = 1;
            RootFace = null;
            Faces = 0;
            EPAFailed = false;
                 
            //hull preparation
            if (EncloseOrigin())
            {
                int nfidx = 0;
                int neidx = 0;

                Face[] basefaces = new Face[6]; //todo - change to field so no new object creation on each call
                switch (GJKOrder)
                {                    
                    case 3:
                        //tetrahedron
                        fidx = new int[,] { { 2, 1, 0 }, { 3, 0, 1 }, { 3, 1, 2 }, { 3, 2, 0 } };
                        eidx = new int[,] { { 0, 0, 2, 1 }, { 0, 1, 1, 1 }, { 0, 2, 3, 1 }, { 1, 0, 3, 2 }, { 2, 0, 1, 2 }, { 3, 0, 2, 2 } };
                        nfidx = 4; neidx = 6;
                        break;
                    case 4:
                        //hexahedron
                        fidx = new int[,] { { 2, 0, 4 }, { 4, 1, 2 }, { 1, 4, 0 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 3, 2 } };
                        eidx = new int[,] { { 0, 0, 4, 0 }, { 0, 1, 2, 1 }, { 0, 2, 1, 2 }, { 1, 1, 5, 2 }, { 1, 0, 2, 0 }, { 2, 2, 3, 2 }, { 3, 1, 5, 0 }, { 3, 0, 4, 2 }, { 5, 1, 4, 1 } };
                        nfidx = 6; neidx = 9;
                        break;
                }

                for (int i = 0; i <= GJKOrder; ++i)
                {
                    EPAbasemkv[i].Vertice = Simplex[i].Vertice;
                    EPAbasemkv[i].Ray = Simplex[i].Ray;
                }
                for (int i = 0; i < nfidx; ++i)
                {
                    basefaces[i] = NewFace(ref EPAbasemkv[fidx[i, 0]], ref EPAbasemkv[fidx[i, 1]], ref EPAbasemkv[fidx[i, 2]]);
                }
                for (int i = 0; i < neidx; ++i)
                {
                    LinkFace(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]);
                }
            }

            if (Faces == 0)
                return false;

            //expand hull

            MinkowskiVertice w = new MinkowskiVertice();

            for (EPAIterations = 0; EPAIterations < EPAMaxIterations; EPAIterations++)
            {
                Face bf = FindBest();
                if (bf != null)
                {
                    bestface = bf;
                    Support(ref bf.Normal, ref w);
                    float d = bf.Normal.Dot(w.Vertice) - bf.Distance;   //changed + to -

                    if (d > EPAAccuracy)            //changed from d < -epa
                    {
                        Face cf = null;
                        Face ff = null;
                        int nf = 0;
                        DetachFaces(ref bf);
                        bf.Mark = ++markid;
                        for (int i = 0; i < 3; ++i)
                        {
                            nf += BuildHorizon(markid, ref w, ref bf.Faces[i], bf.e[i], ref cf, ref ff);
                        }
                        if (nf <= 2) { break; }
                        LinkFace(ref cf, 1, ref ff, 2);
                    }
                    else
                    {
                        break; //enough accuracy
                    }
                }
                else break; //out of vertices
            }

            //prepare contact
            if (bestface == null)
            {
                EPAFailed = true;
                return false;
            }


            info.ContactNormal = bestface.Normal;
            //info.Penetration = MathTools.Max(0, bestface.Distance);
            info.Penetration =  bestface.Distance + GJKMargin;
            info.Penetration *= 10;


            Vector3 b = GetCoordinates(bestface);
            //for (int i = 0; i < 2; ++i)
            //{
                //float s = i != 0 ? -1 : 1;
                for (int j = 0; j < 3; ++j)
                {
                    LocalSupport0(bestface.Vertices[j].Ray, out Features[0,j]);
                    LocalSupport1(-bestface.Vertices[j].Ray, out Features[1, j]);

                    
                    //LocalSupport(s * bestface.Vertices[j].Ray, i, out Features[i, j]);
                }
            //}
            info.ContactPoint1 = new Vector3(ref Features[0, 0], b.X);
            info.ContactPoint1.AddScaled(ref Features[0, 1], b.Y);
            info.ContactPoint1.AddScaled(ref Features[0, 2], b.Z);

            info.ContactPoint2 = new Vector3(ref Features[1, 0], b.X);
            info.ContactPoint2.AddScaled(ref Features[1, 1], b.Y);
            info.ContactPoint2.AddScaled(ref Features[1, 2], b.Z);
             
            return true;
        }

        #region Face handling

        private Face FindBest()
        {
            Face bf = null;
            if (RootFace != null)
            {
                Face cf = RootFace;
                float dist = float.PositiveInfinity;
                do
                {
                    if (cf.Distance < dist) 
                    { 
                        dist = cf.Distance; 
                        bf = cf; 
                    }
                } while (null != (cf = cf.Next));
            }
            return bf;
        }

        /// <summary>
        /// Creates new face
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="c"></param>
        /// <returns>New face</returns>
        private Face NewFace(ref MinkowskiVertice a, ref MinkowskiVertice b, ref MinkowskiVertice c)
        {
            Face pf = new Face();
            if (SetFace(pf, ref a, ref b, ref c))
            {
                if (RootFace != null)
                    RootFace.Previous = pf;
                pf.Previous = null;
                pf.Next = RootFace;
                RootFace = pf;
                Faces++;
            }
            else
            {
                pf.Next = pf.Previous = null;
            }
            return (pf);
        }

        /// <summary>
        /// Sets face values
        /// </summary>
        /// <param name="face"></param>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="c"></param>
        /// <returns>True if face is valid</returns>
        private bool SetFace(Face face, ref MinkowskiVertice a, ref MinkowskiVertice b, ref MinkowskiVertice c)
        {
            Vector3 norm = Vector3.Substract(ref b.Vertice, ref a.Vertice);
            norm = norm.Cross(Vector3.Substract(ref c.Vertice, ref a.Vertice));
            float len = norm.Length;

            bool isvalid = false;

            if ((a.Vertice.Cross(ref b.Vertice).Dot(ref norm) >= -EPAFaceEpsilon) &&
                (b.Vertice.Cross(ref c.Vertice).Dot(ref norm) >= -EPAFaceEpsilon) &&
                (c.Vertice.Cross(ref a.Vertice).Dot(ref norm) >= -EPAFaceEpsilon))
                isvalid = true;
            face.Vertices[0] = a;
            face.Vertices[1] = b;
            face.Vertices[2] = c;
            face.Mark = 0;
            if (len > 0)
                face.Normal = norm.NormalizedCopy();
            else face.Normal = Vector3.Zero;

            float dst = -face.Normal.Dot(ref a.Vertice);

            face.Distance = MathTools.Max(0, dst);
            return isvalid;
        }

        /// <summary>
        /// Links faces together
        /// </summary>
        /// <param name="f0"></param>
        /// <param name="e0"></param>
        /// <param name="f1"></param>
        /// <param name="e1"></param>
        private void LinkFace(ref Face f0, int e0, ref Face f1, int e1)
        {
            f0.Faces[e0] = f1;
            f1.e[e1] = e0;
            f1.Faces[e1] = f0;
            f0.e[e0] = e1;
        }

        private void DetachFaces(ref Face face)
        {
            if (face.Previous != null || face.Next != null)
            {
                Faces--;
                if (face == RootFace)
                {
                    RootFace = face.Next;
                    RootFace.Previous = null;
                }
                else
                {
                    if (face.Next == null)
                    {
                        face.Previous.Next = null;
                    }
                    else
                    {
                        face.Previous.Next = face.Next;
                        face.Next.Previous = face.Previous;
                    }
                }
                face.Previous = face.Next = null;
            }
        }

        private Vector3 GetCoordinates(Face face)
        {
            Vector3 o = new Vector3(ref face.Normal, -face.Distance);

            float a0 = Vector3.Cross(face.Vertices[0].Vertice - o, face.Vertices[1].Vertice - o).Length;
            float a1 =  Vector3.Cross(face.Vertices[1].Vertice - o, face.Vertices[2].Vertice - o).Length;
            float a2 = Vector3.Cross(face.Vertices[2].Vertice - o, face.Vertices[0].Vertice - o).Length;

            float size = a0 + a1+a2;

            if (size > 0)
            {
                size = 1 / size;

                return new Vector3(size * a0, size * a1, size * a2);
            }
            else 
                return new Vector3();
        }

        private static int[] mod3 = { 0, 1, 2, 0, 1 };

        private int BuildHorizon(int markid, ref MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff)
        {
            int ne = (0);
            if (f.Mark != markid)
            {
                int e1 = (mod3[e + 1]);
                if ((f.Normal.Dot(ref w.Vertice) + f.Distance) > 0)
                {
                    Face nf = NewFace(ref f.Vertices[e1], ref f.Vertices[e], ref w);
                    LinkFace(ref nf, 0, ref f, e);
                    if (cf != null) 
                        LinkFace(ref cf, 1, ref nf, 2); else ff = nf;
                    cf = nf; ne = 1;
                }
                else
                {
                    int e2 = (mod3[e + 2]);
                    DetachFaces(ref f);
                    f.Mark = markid;
                    ne += BuildHorizon(markid, ref w, ref f.Faces[e1], f.e[e1], ref cf, ref ff);
                    ne += BuildHorizon(markid, ref w, ref f.Faces[e2], f.e[e2], ref cf, ref ff);
                }
            }
            return (ne);
        }

        #endregion

        #region Supporting vertices reading

        /// <summary>
        /// returns vector hash for hashtable
        /// </summary>
        /// <param name="v"></param>
        /// <returns></returns>
        protected int Hash(ref Vector3 v)
        {
				int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473));
				return (h * 169639) & HashMask;
        }

        protected bool FetchSupport()
        {
            
            int h = Hash(ref _Ray);

            HashTabItem he = HashTable[h];
            while (he != null)
            {
                if (he.Ray.Equals(ref _Ray))
                {
                    --GJKOrder;
                    return false;
                }
                he = he.Next;
            }
            he = new HashTabItem(ref _Ray, HashTable[h]);
            HashTable[h] = he;
            Support(ref _Ray, ref Simplex[++GJKOrder]);

            float val = _Ray.Dot(ref Simplex[GJKOrder].Vertice);

            if (val >0)
                return true;
            return false;
        }

        void Support(ref Vector3 dir, ref MinkowskiVertice v)
        {
            v.Ray.Assign(ref dir);
            
            Vector3 vert;
            LocalSupport0(dir, out vert);
            v.Vertice.Assign(ref vert);
            LocalSupport1(-dir, out vert);
            v.Vertice.Substract(ref vert);
            v.Vertice.AddScaled(ref dir, GJKMargin);
        }

        void LocalSupport0(Vector3 dir, out Vector3 rval)
        {
            Shapes[0].GetLocalSupportingVertex(ref dir, out rval);
        }

        void LocalSupport1(Vector3 dir, out Vector3 rval)
        {
            MatrixTo1.Transform(ref dir);
            Shapes[1].GetLocalSupportingVertex(ref dir, out rval);
            MatrixTo0.TransformFast(ref rval);
        }

        /*
        /// <summary>
        /// Gets supporting vertex from shape
        /// </summary>
        /// <param name="d">indexing normal</param>
        /// <param name="i">index of shape</param>
        /// <param name="rval">Supporting vertex</param>
        void LocalSupport(ref Vector3 d, int i, out Vector3 rval)
        {
            Matrices[i].TransformNormal(ref d);
            Shapes[i].GetLocalSupportingVertex(ref d, out rval);
            Matrices[i].FastTransform(ref rval);
        }

        /// <summary>
        /// Gets supporting vertex from shape
        /// </summary>
        /// <param name="d">indexing normal</param>
        /// <param name="i">index of shape</param>
        /// <param name="rval">Supporting vertex</param>
        void LocalSupport(Vector3 d, int i, out Vector3 rval)
        {
            LocalSupport(ref d, i, out rval);
        }

        */
        #endregion


        #region Simplex solving
        
        /// <summary>
        /// Solve simplex 2
        /// </summary>
        /// <param name="aOrigin">A to origin</param>
        /// <param name="aB">A to B</param>
        /// <returns>true if intersection</returns>
        public bool SolveSimplex2(ref Vector3 aOrigin, ref Vector3 aB)
        {
            if (aOrigin.Dot(ref aB) >= 0)
            {
                Vector3 cabo = aB.Cross(ref aOrigin);
                if (cabo.LengthSquared > GJKSimplexEpsilon)
                {
                    _Ray = cabo.Cross(ref aB);
                }
                else return true;
            }
            else
            {
                GJKOrder = 0;
                Simplex[0].Ray = Simplex[1].Ray;
                Simplex[0].Vertice = Simplex[1].Vertice;
                _Ray = aOrigin;
            }
            return false;
        }

        public bool SolveSimplex3(ref Vector3 aOrigin, ref Vector3 aB, ref Vector3 aC)
        {
            Vector3 cabc = aB.Cross(ref aC);
            return SolveSimplex3(ref aOrigin, ref aB, ref aC, ref cabc);
        }

        public bool SolveSimplex3(ref Vector3 aOrigin, ref Vector3 aB, ref Vector3 aC, ref Vector3 crossabc)
        {
            if (crossabc.Cross(ref aB).Dot(ref aOrigin) < -GJKSimplexEpsilon)
            {
                GJKOrder = 1;
                Simplex[0].Vertice = Simplex[1].Vertice;
                Simplex[0].Ray = Simplex[1].Ray;

                Simplex[1].Vertice = Simplex[2].Vertice;
                Simplex[1].Ray = Simplex[2].Ray;

                return SolveSimplex2(ref aOrigin, ref aB);
            }
            else if (crossabc.Cross(ref aC).Dot(ref aOrigin) > GJKSimplexEpsilon)
            {
                GJKOrder = 1;
                Simplex[1].Vertice = Simplex[2].Vertice;
                Simplex[1].Ray = Simplex[2].Ray;

                return SolveSimplex2(ref aOrigin, ref aC);
            }
            else
            {
                float dis = crossabc.Dot(ref aOrigin);
                if (MathTools.Abs(dis) > GJKSimplexEpsilon)
                {
                    if (dis > 0)
                        _Ray = crossabc;
                    else
                    {
                        _Ray = -crossabc;
                        SwapSimplex(ref Simplex[0], ref Simplex[1]);
                    }
                    return false;
                }
                else return true;
            }
        }

        public bool SolveSimplex4(ref Vector3 ao, ref Vector3 ab, ref Vector3 ac, ref Vector3 ad)
        {
            Vector3 cross = ab.Cross(ref ac);
            if (cross.Dot(ref ao) > GJKSimplexEpsilon)
            {
                GJKOrder = 2;
                Simplex[0].Vertice = Simplex[1].Vertice;
                Simplex[0].Ray = Simplex[1].Ray;

                Simplex[1].Vertice = Simplex[2].Vertice;
                Simplex[1].Ray = Simplex[2].Ray;

                Simplex[2].Vertice = Simplex[3].Vertice;
                Simplex[2].Ray = Simplex[3].Ray;

                return SolveSimplex3(ref ao, ref ab, ref ac, ref cross);
            }

            cross = ac.Cross(ref ad);            
            if (cross.Dot(ref ao) > GJKSimplexEpsilon)
            {
                GJKOrder = 2;
                Simplex[2].Vertice = Simplex[3].Vertice;
                Simplex[2].Ray = Simplex[3].Ray;

                return SolveSimplex3(ref ao, ref ac, ref ad, ref cross);
            }

            cross =ad.Cross(ref ab);
            if (cross.Dot(ref ao) > GJKSimplexEpsilon)
            {
                GJKOrder = 2;

                Simplex[1].Vertice = Simplex[0].Vertice;
                Simplex[1].Ray = Simplex[0].Ray;

                Simplex[0].Vertice = Simplex[2].Vertice;
                Simplex[0].Ray = Simplex[2].Ray;

                Simplex[2].Vertice = Simplex[3].Vertice;
                Simplex[2].Ray = Simplex[3].Ray;

                return SolveSimplex3(ref ao, ref ad, ref ab, ref cross);
            }
            else return true;
        }
        #endregion


    }
}
