public bool ClosestPtPointTriangle(ref IndexedVector3 p, ref IndexedVector3 a, ref IndexedVector3 b, ref IndexedVector3 c, ref SubSimplexClosestResult result)
        {
            result.m_usedVertices.SetAll(false);

            // Check if P in vertex region outside A
            IndexedVector3 ab,ac,ap;
            IndexedVector3.Subtract(out ab,ref b,ref a);
            IndexedVector3.Subtract(out ac, ref c, ref a);
            IndexedVector3.Subtract(out ap, ref p, ref a);
            
            float d1 = IndexedVector3.Dot(ref ab, ref ap);
            float d2 = IndexedVector3.Dot(ref ac, ref ap);
            if (d1 <= 0f && d2 <= 0f)
            {
                result.m_closestPointOnSimplex = a;
                result.m_usedVertices.Set(0, true);
                result.SetBarycentricCoordinates(1, 0, 0, 0);
                return true;// a; // barycentric coordinates (1,0,0)
            }

            // Check if P in vertex region outside B
            IndexedVector3 bp;
            IndexedVector3.Subtract(out bp, ref p, ref b);
            float d3 = IndexedVector3.Dot(ref ab, ref bp);
            float d4 = IndexedVector3.Dot(ref ac, ref bp);
            if (d3 >= 0f && d4 <= d3)
            {
                result.m_closestPointOnSimplex = b;
                result.m_usedVertices.Set(1, true);
                result.SetBarycentricCoordinates(0, 1, 0, 0);

                return true; // b; // barycentric coordinates (0,1,0)
            }
            // Check if P in edge region of AB, if so return projection of P onto AB
            float vc = d1 * d4 - d3 * d2;
            if (vc <= 0f && d1 >= 0f && d3 <= 0f)
            {
                float v = d1 / (d1 - d3);
                result.m_closestPointOnSimplex = a + v * ab;
                result.m_usedVertices.Set(0, true);
                result.m_usedVertices.Set(1, true);
                result.SetBarycentricCoordinates(1 - v, v, 0, 0);
                return true;
                //return a + v * ab; // barycentric coordinates (1-v,v,0)
            }

            // Check if P in vertex region outside C
            IndexedVector3 cp;
            IndexedVector3.Subtract(out cp, ref p, ref c);
            float d5 = IndexedVector3.Dot(ref ab, ref cp);
            float d6 = IndexedVector3.Dot(ref ac, ref cp);

            if (d6 >= 0f && d5 <= d6)
            {
                result.m_closestPointOnSimplex = c;
                result.m_usedVertices.Set(2, true);
                result.SetBarycentricCoordinates(0, 0, 1, 0);
                return true;//c; // barycentric coordinates (0,0,1)
            }

            // Check if P in edge region of AC, if so return projection of P onto AC
            float vb = d5 * d2 - d1 * d6;
            if (vb <= 0f && d2 >= 0f && d6 <= 0f)
            {
                float w = d2 / (d2 - d6);
                result.m_closestPointOnSimplex = a + w * ac;
                result.m_usedVertices.Set(0, true);
                result.m_usedVertices.Set(2, true);
                result.SetBarycentricCoordinates(1 - w, 0, w, 0);
                return true;
                //return a + w * ac; // barycentric coordinates (1-w,0,w)
            }

            // Check if P in edge region of BC, if so return projection of P onto BC
            float va = d3 * d6 - d5 * d4;
            if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f)
            {
                float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));

                result.m_closestPointOnSimplex = b + w * (c - b);
                result.m_usedVertices.Set(1, true);
                result.m_usedVertices.Set(2, true);
                result.SetBarycentricCoordinates(0, 1 - w, w, 0);
                return true;
                // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
            }

            // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
            float denom = 1f / (va + vb + vc);
            float v1 = vb * denom;
            float w1 = vc * denom;

            result.m_closestPointOnSimplex = a + ab * v1 + ac * w1;
            result.m_usedVertices.Set(0, true);
            result.m_usedVertices.Set(1, true);
            result.m_usedVertices.Set(2, true);
            result.SetBarycentricCoordinates(1 - v1 - w1, v1, w1, 0);

            return true;
            //	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = float(1.0) - v - w

        }
        public bool ClosestPtPointTriangle(ref IndexedVector3 p, ref IndexedVector3 a, ref IndexedVector3 b, ref IndexedVector3 c, ref SubSimplexClosestResult result)
        {
            result.m_usedVertices.SetAll(false);

            // Check if P in vertex region outside A
            IndexedVector3 ab, ac, ap;

            IndexedVector3.Subtract(out ab, ref b, ref a);
            IndexedVector3.Subtract(out ac, ref c, ref a);
            IndexedVector3.Subtract(out ap, ref p, ref a);

            float d1 = IndexedVector3.Dot(ref ab, ref ap);
            float d2 = IndexedVector3.Dot(ref ac, ref ap);

            if (d1 <= 0f && d2 <= 0f)
            {
                result.m_closestPointOnSimplex = a;
                result.m_usedVertices.Set(0, true);
                result.SetBarycentricCoordinates(1, 0, 0, 0);
                return(true);// a; // barycentric coordinates (1,0,0)
            }

            // Check if P in vertex region outside B
            IndexedVector3 bp;

            IndexedVector3.Subtract(out bp, ref p, ref b);
            float d3 = IndexedVector3.Dot(ref ab, ref bp);
            float d4 = IndexedVector3.Dot(ref ac, ref bp);

            if (d3 >= 0f && d4 <= d3)
            {
                result.m_closestPointOnSimplex = b;
                result.m_usedVertices.Set(1, true);
                result.SetBarycentricCoordinates(0, 1, 0, 0);

                return(true); // b; // barycentric coordinates (0,1,0)
            }
            // Check if P in edge region of AB, if so return projection of P onto AB
            float vc = d1 * d4 - d3 * d2;

            if (vc <= 0f && d1 >= 0f && d3 <= 0f)
            {
                float v = d1 / (d1 - d3);
                result.m_closestPointOnSimplex = a + v * ab;
                result.m_usedVertices.Set(0, true);
                result.m_usedVertices.Set(1, true);
                result.SetBarycentricCoordinates(1 - v, v, 0, 0);
                return(true);
                //return a + v * ab; // barycentric coordinates (1-v,v,0)
            }

            // Check if P in vertex region outside C
            IndexedVector3 cp;

            IndexedVector3.Subtract(out cp, ref p, ref c);
            float d5 = IndexedVector3.Dot(ref ab, ref cp);
            float d6 = IndexedVector3.Dot(ref ac, ref cp);

            if (d6 >= 0f && d5 <= d6)
            {
                result.m_closestPointOnSimplex = c;
                result.m_usedVertices.Set(2, true);
                result.SetBarycentricCoordinates(0, 0, 1, 0);
                return(true);//c; // barycentric coordinates (0,0,1)
            }

            // Check if P in edge region of AC, if so return projection of P onto AC
            float vb = d5 * d2 - d1 * d6;

            if (vb <= 0f && d2 >= 0f && d6 <= 0f)
            {
                float w = d2 / (d2 - d6);
                result.m_closestPointOnSimplex = a + w * ac;
                result.m_usedVertices.Set(0, true);
                result.m_usedVertices.Set(2, true);
                result.SetBarycentricCoordinates(1 - w, 0, w, 0);
                return(true);
                //return a + w * ac; // barycentric coordinates (1-w,0,w)
            }

            // Check if P in edge region of BC, if so return projection of P onto BC
            float va = d3 * d6 - d5 * d4;

            if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f)
            {
                float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));

                result.m_closestPointOnSimplex = b + w * (c - b);
                result.m_usedVertices.Set(1, true);
                result.m_usedVertices.Set(2, true);
                result.SetBarycentricCoordinates(0, 1 - w, w, 0);
                return(true);
                // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
            }

            // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
            float denom = 1f / (va + vb + vc);
            float v1    = vb * denom;
            float w1    = vc * denom;

            result.m_closestPointOnSimplex = a + ab * v1 + ac * w1;
            result.m_usedVertices.Set(0, true);
            result.m_usedVertices.Set(1, true);
            result.m_usedVertices.Set(2, true);
            result.SetBarycentricCoordinates(1 - v1 - w1, v1, w1, 0);

            return(true);
            //	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = float(1.0) - v - w
        }
        public bool ClosestPtPointTetrahedron(ref IndexedVector3 p, ref IndexedVector3 a, ref IndexedVector3 b, ref IndexedVector3 c, ref IndexedVector3 d, ref SubSimplexClosestResult finalResult)
        {

            // Start ref assuming point inside all halfspaces, so closest to itself
            finalResult.m_closestPointOnSimplex = p;
            finalResult.m_usedVertices.SetAll(true);

            int pointOutsideABC = PointOutsideOfPlane(ref p, ref a, ref b, ref c, ref d);
            int pointOutsideACD = PointOutsideOfPlane(ref p, ref a, ref c, ref d, ref b);
            int pointOutsideADB = PointOutsideOfPlane(ref p, ref a, ref d, ref b, ref c);
            int pointOutsideBDC = PointOutsideOfPlane(ref p, ref b, ref d, ref c, ref a);

            if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
            {
                finalResult.m_degenerate = true;
                return false;
            }

            if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0)
            {
                return false;
            }

            SubSimplexClosestResult tempResult = BulletGlobals.SubSimplexClosestResultPool.Get();
            tempResult.Reset();

            float bestSqDist = float.MaxValue;
            // If point outside face abc then compute closest point on abc
            if (pointOutsideABC != 0)
            {
                ClosestPtPointTriangle(ref p, ref a, ref b, ref c, ref tempResult);
                IndexedVector3 q = tempResult.m_closestPointOnSimplex;

                float sqDist = (q - p).LengthSquared();
                // Update best closest point if (squared) distance is less than current best
                if (sqDist < bestSqDist)
                {
                    bestSqDist = sqDist;
                    finalResult.m_closestPointOnSimplex = q;
                    //convert result bitmask!
                    finalResult.m_usedVertices.SetAll(false);
                    finalResult.m_usedVertices.Set(0, tempResult.m_usedVertices.Get(0));
                    finalResult.m_usedVertices.Set(1, tempResult.m_usedVertices.Get(1));
                    finalResult.m_usedVertices.Set(2, tempResult.m_usedVertices.Get(2));
                    finalResult.SetBarycentricCoordinates(
                            tempResult.m_barycentricCoords.X,
                            tempResult.m_barycentricCoords.Y,
                            tempResult.m_barycentricCoords.Z,
                            0
                    );

                }
            }


            // Repeat test for face acd
            if (pointOutsideACD != 0)
            {
                ClosestPtPointTriangle(ref p, ref a, ref c, ref d, ref tempResult);
                IndexedVector3 q = tempResult.m_closestPointOnSimplex;

                float sqDist = (q - p).LengthSquared();
                // Update best closest point if (squared) distance is less than current best
                if (sqDist < bestSqDist)
                {
                    bestSqDist = sqDist;
                    finalResult.m_closestPointOnSimplex = q;
                    //convert result bitmask!
                    finalResult.m_usedVertices.SetAll(false);
                    finalResult.m_usedVertices.Set(0, tempResult.m_usedVertices.Get(0));
                    finalResult.m_usedVertices.Set(2, tempResult.m_usedVertices.Get(1));
                    finalResult.m_usedVertices.Set(3, tempResult.m_usedVertices.Get(2));
                    finalResult.SetBarycentricCoordinates(
                            tempResult.m_barycentricCoords.X,
                            0,
                            tempResult.m_barycentricCoords.Y,
                            tempResult.m_barycentricCoords.Z);
                }
            }
            // Repeat test for face adb


            if (pointOutsideADB != 0)
            {
                ClosestPtPointTriangle(ref p, ref a, ref d, ref b, ref tempResult);
                IndexedVector3 q = tempResult.m_closestPointOnSimplex;

                float sqDist = (q - p).LengthSquared();
                // Update best closest point if (squared) distance is less than current best
                if (sqDist < bestSqDist)
                {
                    bestSqDist = sqDist;
                    finalResult.m_closestPointOnSimplex = q;
                    //convert result bitmask!
                    finalResult.m_usedVertices.SetAll(false);
                    finalResult.m_usedVertices.Set(0, tempResult.m_usedVertices.Get(0));
                    finalResult.m_usedVertices.Set(1, tempResult.m_usedVertices.Get(2));
                    finalResult.m_usedVertices.Set(3, tempResult.m_usedVertices.Get(1));
                    finalResult.SetBarycentricCoordinates(
                            tempResult.m_barycentricCoords.X,
                            tempResult.m_barycentricCoords.Z,
                            0,
                            tempResult.m_barycentricCoords.Y);
                }
            }
            // Repeat test for face bdc


            if (pointOutsideBDC != 0)
            {
                ClosestPtPointTriangle(ref p, ref b, ref d, ref c, ref tempResult);
                IndexedVector3 q = tempResult.m_closestPointOnSimplex;

                float sqDist = (q - p).LengthSquared();
                // Update best closest point if (squared) distance is less than current best
                if (sqDist < bestSqDist)
                {
                    bestSqDist = sqDist;
                    finalResult.m_closestPointOnSimplex = q;
                    //convert result bitmask!
                    finalResult.m_usedVertices.SetAll(false);
                    finalResult.m_usedVertices.Set(1, tempResult.m_usedVertices.Get(0));
                    finalResult.m_usedVertices.Set(2, tempResult.m_usedVertices.Get(2));
                    finalResult.m_usedVertices.Set(3, tempResult.m_usedVertices.Get(1));
                    finalResult.SetBarycentricCoordinates(
                            0,
                            tempResult.m_barycentricCoords.X,
                            tempResult.m_barycentricCoords.Z,
                            tempResult.m_barycentricCoords.Y);
                }

            }

            BulletGlobals.SubSimplexClosestResultPool.Free(tempResult);

            //help! we ended up full !

            if (finalResult.m_usedVertices.Get(0) &&
                finalResult.m_usedVertices.Get(1) &&
                finalResult.m_usedVertices.Get(2) &&
                finalResult.m_usedVertices.Get(3))
            {
                return true;
            }

            return true;

        }
        public bool ClosestPtPointTetrahedron(ref IndexedVector3 p, ref IndexedVector3 a, ref IndexedVector3 b, ref IndexedVector3 c, ref IndexedVector3 d, ref SubSimplexClosestResult finalResult)
        {
            // Start ref assuming point inside all halfspaces, so closest to itself
            finalResult.m_closestPointOnSimplex = p;
            finalResult.m_usedVertices.SetAll(true);

            int pointOutsideABC = PointOutsideOfPlane(ref p, ref a, ref b, ref c, ref d);
            int pointOutsideACD = PointOutsideOfPlane(ref p, ref a, ref c, ref d, ref b);
            int pointOutsideADB = PointOutsideOfPlane(ref p, ref a, ref d, ref b, ref c);
            int pointOutsideBDC = PointOutsideOfPlane(ref p, ref b, ref d, ref c, ref a);

            if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
            {
                finalResult.m_degenerate = true;
                return(false);
            }

            if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0)
            {
                return(false);
            }

            SubSimplexClosestResult tempResult = BulletGlobals.SubSimplexClosestResultPool.Get();

            tempResult.Reset();

            float bestSqDist = float.MaxValue;

            // If point outside face abc then compute closest point on abc
            if (pointOutsideABC != 0)
            {
                ClosestPtPointTriangle(ref p, ref a, ref b, ref c, ref tempResult);
                IndexedVector3 q = tempResult.m_closestPointOnSimplex;

                float sqDist = (q - p).LengthSquared();
                // Update best closest point if (squared) distance is less than current best
                if (sqDist < bestSqDist)
                {
                    bestSqDist = sqDist;
                    finalResult.m_closestPointOnSimplex = q;
                    //convert result bitmask!
                    finalResult.m_usedVertices.SetAll(false);
                    finalResult.m_usedVertices.Set(0, tempResult.m_usedVertices.Get(0));
                    finalResult.m_usedVertices.Set(1, tempResult.m_usedVertices.Get(1));
                    finalResult.m_usedVertices.Set(2, tempResult.m_usedVertices.Get(2));
                    finalResult.SetBarycentricCoordinates(
                        tempResult.m_barycentricCoords.X,
                        tempResult.m_barycentricCoords.Y,
                        tempResult.m_barycentricCoords.Z,
                        0
                        );
                }
            }


            // Repeat test for face acd
            if (pointOutsideACD != 0)
            {
                ClosestPtPointTriangle(ref p, ref a, ref c, ref d, ref tempResult);
                IndexedVector3 q = tempResult.m_closestPointOnSimplex;

                float sqDist = (q - p).LengthSquared();
                // Update best closest point if (squared) distance is less than current best
                if (sqDist < bestSqDist)
                {
                    bestSqDist = sqDist;
                    finalResult.m_closestPointOnSimplex = q;
                    //convert result bitmask!
                    finalResult.m_usedVertices.SetAll(false);
                    finalResult.m_usedVertices.Set(0, tempResult.m_usedVertices.Get(0));
                    finalResult.m_usedVertices.Set(2, tempResult.m_usedVertices.Get(1));
                    finalResult.m_usedVertices.Set(3, tempResult.m_usedVertices.Get(2));
                    finalResult.SetBarycentricCoordinates(
                        tempResult.m_barycentricCoords.X,
                        0,
                        tempResult.m_barycentricCoords.Y,
                        tempResult.m_barycentricCoords.Z);
                }
            }
            // Repeat test for face adb


            if (pointOutsideADB != 0)
            {
                ClosestPtPointTriangle(ref p, ref a, ref d, ref b, ref tempResult);
                IndexedVector3 q = tempResult.m_closestPointOnSimplex;

                float sqDist = (q - p).LengthSquared();
                // Update best closest point if (squared) distance is less than current best
                if (sqDist < bestSqDist)
                {
                    bestSqDist = sqDist;
                    finalResult.m_closestPointOnSimplex = q;
                    //convert result bitmask!
                    finalResult.m_usedVertices.SetAll(false);
                    finalResult.m_usedVertices.Set(0, tempResult.m_usedVertices.Get(0));
                    finalResult.m_usedVertices.Set(1, tempResult.m_usedVertices.Get(2));
                    finalResult.m_usedVertices.Set(3, tempResult.m_usedVertices.Get(1));
                    finalResult.SetBarycentricCoordinates(
                        tempResult.m_barycentricCoords.X,
                        tempResult.m_barycentricCoords.Z,
                        0,
                        tempResult.m_barycentricCoords.Y);
                }
            }
            // Repeat test for face bdc


            if (pointOutsideBDC != 0)
            {
                ClosestPtPointTriangle(ref p, ref b, ref d, ref c, ref tempResult);
                IndexedVector3 q = tempResult.m_closestPointOnSimplex;

                float sqDist = (q - p).LengthSquared();
                // Update best closest point if (squared) distance is less than current best
                if (sqDist < bestSqDist)
                {
                    bestSqDist = sqDist;
                    finalResult.m_closestPointOnSimplex = q;
                    //convert result bitmask!
                    finalResult.m_usedVertices.SetAll(false);
                    finalResult.m_usedVertices.Set(1, tempResult.m_usedVertices.Get(0));
                    finalResult.m_usedVertices.Set(2, tempResult.m_usedVertices.Get(2));
                    finalResult.m_usedVertices.Set(3, tempResult.m_usedVertices.Get(1));
                    finalResult.SetBarycentricCoordinates(
                        0,
                        tempResult.m_barycentricCoords.X,
                        tempResult.m_barycentricCoords.Z,
                        tempResult.m_barycentricCoords.Y);
                }
            }

            BulletGlobals.SubSimplexClosestResultPool.Free(tempResult);

            //help! we ended up full !

            if (finalResult.m_usedVertices.Get(0) &&
                finalResult.m_usedVertices.Get(1) &&
                finalResult.m_usedVertices.Get(2) &&
                finalResult.m_usedVertices.Get(3))
            {
                return(true);
            }

            return(true);
        }