public void Initialize()
        {
            m_status = eStatus.Failed;
            m_normal = IndexedVector3.Zero;
            m_depth  = 0;
            m_nextsv = 0;
            m_result = new sSimplex();

            for (int i = 0; i < m_sv_store.Length; ++i)
            {
                m_sv_store[i] = new sSV();
            }

            for (int i = 0; i < m_fc_store.Length; ++i)
            {
                m_fc_store[i] = new sFace();
            }

            for (int i = 0; i < GjkEpaSolver2.EPA_MAX_FACES; ++i)
            {
                Append(m_stock, m_fc_store[GjkEpaSolver2.EPA_MAX_FACES - i - 1]);
            }
        }
        public void Initialise()
        {
            m_ray      = IndexedVector3.Zero;
            m_nfree    = 0;
            m_status   = GJKStatus.Failed;
            m_current  = 0;
            m_distance = 0f;
            for (int i = 0; i < m_simplices.Length; ++i)
            {
                if (m_simplices[i] == null)
                {
                    m_simplices[i] = new sSimplex();
                }
            }

            for (int i = 0; i < m_store.Length; ++i)
            {
                if (m_store[i] == null)
                {
                    m_store[i] = new sSV();
                }
            }
        }
        public eStatus Evaluate(GJK gjk, ref IndexedVector3 guess)
        {
            sSimplex simplex = gjk.m_simplex;

            if ((simplex.rank > 1) && gjk.EncloseOrigin())
            {
                /* Clean up				*/
                while (m_hull.Count > 0)
                {
                    sFace f = m_hull[0];
                    Remove(m_hull, f);
                    Append(m_stock, f);
                }

                m_status = eStatus.Valid;
                m_nextsv = 0;
                /* Orient simplex		*/
                if (GJK.Det(simplex.c[0].w - simplex.c[3].w,
                            simplex.c[1].w - simplex.c[3].w,
                            simplex.c[2].w - simplex.c[3].w) < 0)
                {
                    SwapSv(simplex.c, 0, 1);
                    SwapFloat(simplex.p, 0, 1);
                }
                /* Build initial hull	*/
                tetra[0] = NewFace(simplex.c[0], simplex.c[1], simplex.c[2], true);
                tetra[1] = NewFace(simplex.c[1], simplex.c[0], simplex.c[3], true);
                tetra[2] = NewFace(simplex.c[2], simplex.c[1], simplex.c[3], true);
                tetra[3] = NewFace(simplex.c[0], simplex.c[2], simplex.c[3], true);
                if (m_hull.Count == 4)
                {
                    sFace best       = FindBest();
                    sFace outer      = best;
                    uint  pass       = 0;
                    uint  iterations = 0;
                    Bind(tetra[0], 0, tetra[1], 0);
                    Bind(tetra[0], 1, tetra[2], 0);
                    Bind(tetra[0], 2, tetra[3], 0);
                    Bind(tetra[1], 1, tetra[3], 2);
                    Bind(tetra[1], 2, tetra[2], 1);
                    Bind(tetra[2], 2, tetra[3], 1);
                    m_status = eStatus.Valid;
                    for (; iterations < GjkEpaSolver2.EPA_MAX_ITERATIONS; ++iterations)
                    {
                        if (m_nextsv < GjkEpaSolver2.EPA_MAX_VERTICES)
                        {
                            sHorizon horizon = new sHorizon();
                            sSV      w       = m_sv_store[m_nextsv++];
                            bool     valid   = true;
                            best.pass = (uint)(++pass);
                            gjk.GetSupport(ref best.n, ref w);
                            float wdist = IndexedVector3.Dot(ref best.n, ref w.w) - best.d;
                            if (wdist > GjkEpaSolver2.EPA_ACCURACY)
                            {
                                for (int j = 0; (j < 3) && valid; ++j)
                                {
                                    valid &= Expand(pass, w,
                                                    best.f[j], best.e[j],
                                                    ref horizon);
                                }
                                if (valid && (horizon.nf >= 3))
                                {
                                    Bind(horizon.cf, 1, horizon.ff, 2);
                                    Remove(m_hull, best);
                                    Append(m_stock, best);
                                    best = FindBest();
                                    if (best.p >= outer.p)
                                    {
                                        outer = best;
                                    }
                                }
                                else
                                {
                                    m_status = eStatus.InvalidHull;
                                    break;
                                }
                            }
                            else
                            {
                                m_status = eStatus.AccuraryReached;
                                break;
                            }
                        }
                        else
                        {
                            m_status = eStatus.OutOfVertices;
                            break;
                        }
                    }
                    IndexedVector3 projection = outer.n * outer.d;
                    m_normal      = outer.n;
                    m_depth       = outer.d;
                    m_result.rank = 3;
                    m_result.c[0] = outer.c[0];
                    m_result.c[1] = outer.c[1];
                    m_result.c[2] = outer.c[2];
                    m_result.p[0] = IndexedVector3.Cross(outer.c[1].w - projection,
                                                         outer.c[2].w - projection).Length();
                    m_result.p[1] = IndexedVector3.Cross(outer.c[2].w - projection,
                                                         outer.c[0].w - projection).Length();
                    m_result.p[2] = IndexedVector3.Cross(outer.c[0].w - projection,
                                                         outer.c[1].w - projection).Length();
                    float sum = m_result.p[0] + m_result.p[1] + m_result.p[2];
                    m_result.p[0] /= sum;
                    m_result.p[1] /= sum;
                    m_result.p[2] /= sum;
                    return(m_status);
                }
            }
            /* Fallback		*/
            m_status = eStatus.FallBack;
            m_normal = -guess;
            float nl = m_normal.LengthSquared();

            if (nl > 0)
            {
                m_normal.Normalize();
            }
            else
            {
                m_normal = new IndexedVector3(1, 0, 0);
            }

            m_depth       = 0;
            m_result.rank = 1;
            m_result.c[0] = simplex.c[0];
            m_result.p[0] = 1;
            return(m_status);
        }
 public void AppendVertice(sSimplex simplex, ref IndexedVector3 v)
 {
     simplex.p[simplex.rank] = 0;
     simplex.c[simplex.rank] = m_free[--m_nfree];
     GetSupport(ref v, ref simplex.c[simplex.rank++]);
 }
 public void RemoveVertice(sSimplex simplex)
 {
     m_free[m_nfree++] = simplex.c[--simplex.rank];
 }
        public GJKStatus Evaluate(GjkEpaSolver2MinkowskiDiff shapearg, ref IndexedVector3 guess)
        {
            uint  iterations = 0;
            float sqdist     = 0f;
            float alpha      = 0f;
            uint  clastw     = 0;

            /* Initialize solver		*/
            m_free[0]  = m_store[0];
            m_free[1]  = m_store[1];
            m_free[2]  = m_store[2];
            m_free[3]  = m_store[3];
            m_nfree    = 4;
            m_current  = 0;
            m_status   = GJKStatus.Valid;
            m_shape    = shapearg;
            m_distance = 0f;
            /* Initialize simplex		*/
            m_simplices[0].rank = 0;
            m_ray = guess;
            float          sqrl = m_ray.LengthSquared();
            IndexedVector3 temp = sqrl > 0?-m_ray:new IndexedVector3(1, 0, 0);

            AppendVertice(m_simplices[0], ref temp);
            m_simplices[0].p[0] = 1;
            m_ray    = m_simplices[0].c[0].w;
            sqdist   = sqrl;
            lastw[0] = lastw[1] = lastw[2] = lastw[3] = m_ray;
            /* Loop						*/
            do
            {
                uint     next = 1 - m_current;
                sSimplex cs   = m_simplices[m_current];
                sSimplex ns   = m_simplices[next];
                /* Check zero							*/
                float rl = m_ray.Length();
                if (rl < GjkEpaSolver2.GJK_MIN_DISTANCE)
                {/* Touching or inside				*/
                    m_status = GJKStatus.Inside;
                    break;
                }
                /* Append new vertice in -'v' direction	*/
                IndexedVector3 temp2 = -m_ray;
                AppendVertice(cs, ref temp2);
                IndexedVector3 w     = cs.c[cs.rank - 1].w;
                bool           found = false;
                for (int i = 0; i < 4; ++i)
                {
                    if ((w - lastw[i]).LengthSquared() < GjkEpaSolver2.GJK_DUPLICATED_EPS)
                    {
                        found = true;
                        break;
                    }
                }
                if (found)
                {/* Return old simplex				*/
                    RemoveVertice(m_simplices[m_current]);
                    break;
                }
                else
                {/* Update lastw					*/
                    lastw[clastw = (clastw + 1) & 3] = w;
                }
                /* Check for termination				*/
                float omega = IndexedVector3.Dot(ref m_ray, ref w) / rl;
                alpha = Math.Max(omega, alpha);
                if (((rl - alpha) - (GjkEpaSolver2.GJK_ACCURARY * rl)) <= 0)
                {/* Return old simplex				*/
                    RemoveVertice(m_simplices[m_current]);
                    break;
                }

                /* Reduce simplex						*/
                IndexedVector4 weights = new IndexedVector4();
                uint           mask    = 0;
                switch (cs.rank)
                {
                case 2:
                {
                    sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref weights, ref mask);
                    break;
                }

                case 3:
                {
                    sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref weights, ref mask);
                    break;
                }

                case 4:
                {
                    sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref cs.c[3].w, ref weights, ref mask);
                    break;
                }
                }
                if (sqdist >= 0)
                {/* Valid	*/
                    ns.rank   = 0;
                    m_ray     = IndexedVector3.Zero;
                    m_current = next;
                    for (uint i = 0, ni = cs.rank; i < ni; ++i)
                    {
                        if ((mask & (1 << (int)i)) != 0)
                        {
                            ns.c[ns.rank] = cs.c[i];
                            float weight = weights[(int)i];
                            ns.p[ns.rank++] = weight;
                            m_ray          += cs.c[i].w * weight;
                        }
                        else
                        {
                            m_free[m_nfree++] = cs.c[i];
                        }
                    }
                    if (mask == 15)
                    {
                        m_status = GJKStatus.Inside;
                    }
                }
                else
                {/* Return old simplex				*/
                    RemoveVertice(m_simplices[m_current]);
                    break;
                }
                m_status = ((++iterations) < GjkEpaSolver2.GJK_MAX_ITERATIONS) ? m_status : GJKStatus.Failed;
            } while(m_status == GJKStatus.Valid);

            m_simplex = m_simplices[m_current];

            switch (m_status)
            {
            case    GJKStatus.Valid:
            {
                m_distance = m_ray.Length();
                break;
            }

            case    GJKStatus.Inside:
            {
                m_distance = 0;
                break;
            }
            }

#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK)
            {
                BulletGlobals.g_streamWriter.WriteLine(String.Format("gjk eval dist[{0}]", m_distance));
            }
#endif

            return(m_status);
        }