btGjkEpa2.cpp

Go to the documentation of this file.

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the
use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software in a
product, an acknowledgment in the product documentation would be appreciated
but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

/*
GJK-EPA collision solver by Nathanael Presson, 2008
*/
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "btGjkEpa2.h"

#if defined(DEBUG) || defined (_DEBUG)
#include <stdio.h> //for debug printf
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
#endif //__SPU__
#endif

namespace gjkepa2_impl
{

        // Config

        /* GJK  */ 
#define GJK_MAX_ITERATIONS      128
#define GJK_ACCURARY            ((btScalar)0.0001)
#define GJK_MIN_DISTANCE        ((btScalar)0.0001)
#define GJK_DUPLICATED_EPS      ((btScalar)0.0001)
#define GJK_SIMPLEX2_EPS        ((btScalar)0.0)
#define GJK_SIMPLEX3_EPS        ((btScalar)0.0)
#define GJK_SIMPLEX4_EPS        ((btScalar)0.0)

        /* EPA  */ 
#define EPA_MAX_VERTICES        64
#define EPA_MAX_FACES           (EPA_MAX_VERTICES*2)
#define EPA_MAX_ITERATIONS      255
#define EPA_ACCURACY            ((btScalar)0.0001)
#define EPA_FALLBACK            (10*EPA_ACCURACY)
#define EPA_PLANE_EPS           ((btScalar)0.00001)
#define EPA_INSIDE_EPS          ((btScalar)0.01)


        // Shorthands
        typedef unsigned int    U;
        typedef unsigned char   U1;

        // MinkowskiDiff
        struct  MinkowskiDiff
        {
                const btConvexShape*    m_shapes[2];
                btMatrix3x3                             m_toshape1;
                btTransform                             m_toshape0;
#ifdef __SPU__
                bool                                    m_enableMargin;
#else
                btVector3                               (btConvexShape::*Ls)(const btVector3&) const;
#endif//__SPU__
                

                MinkowskiDiff()
                {

                }
#ifdef __SPU__
                        void                                    EnableMargin(bool enable)
                {
                        m_enableMargin = enable;
                }       
                inline btVector3                Support0(const btVector3& d) const
                {
                        if (m_enableMargin)
                        {
                                return m_shapes[0]->localGetSupportVertexNonVirtual(d);
                        } else
                        {
                                return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
                        }
                }
                inline btVector3                Support1(const btVector3& d) const
                {
                        if (m_enableMargin)
                        {
                                return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d));
                        } else
                        {
                                return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d));
                        }
                }
#else
                void                                    EnableMargin(bool enable)
                {
                        if(enable)
                                Ls=&btConvexShape::localGetSupportVertexNonVirtual;
                        else
                                Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
                }       
                inline btVector3                Support0(const btVector3& d) const
                {
                        return(((m_shapes[0])->*(Ls))(d));
                }
                inline btVector3                Support1(const btVector3& d) const
                {
                        return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
                }
#endif //__SPU__

                inline btVector3                Support(const btVector3& d) const
                {
                        return(Support0(d)-Support1(-d));
                }
                btVector3                               Support(const btVector3& d,U index) const
                {
                        if(index)
                                return(Support1(d));
                        else
                                return(Support0(d));
                }
        };

        typedef MinkowskiDiff   tShape;


        // GJK
        struct  GJK
        {
                /* Types                */ 
                struct  sSV
                {
                        btVector3       d,w;
                };
                struct  sSimplex
                {
                        sSV*            c[4];
                        btScalar        p[4];
                        U                       rank;
                };
                struct  eStatus { enum _ {
                        Valid,
                        Inside,
                        Failed          };};
                        /* Fields               */ 
                        tShape                  m_shape;
                        btVector3               m_ray;
                        btScalar                m_distance;
                        sSimplex                m_simplices[2];
                        sSV                             m_store[4];
                        sSV*                    m_free[4];
                        U                               m_nfree;
                        U                               m_current;
                        sSimplex*               m_simplex;
                        eStatus::_              m_status;
                        /* Methods              */ 
                        GJK()
                        {
                                Initialize();
                        }
                        void                            Initialize()
                        {
                                m_ray           =       btVector3(0,0,0);
                                m_nfree         =       0;
                                m_status        =       eStatus::Failed;
                                m_current       =       0;
                                m_distance      =       0;
                        }
                        eStatus::_                      Evaluate(const tShape& shapearg,const btVector3& guess)
                        {
                                U                       iterations=0;
                                btScalar        sqdist=0;
                                btScalar        alpha=0;
                                btVector3       lastw[4];
                                U                       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                        =       eStatus::Valid;
                                m_shape                         =       shapearg;
                                m_distance                      =       0;
                                /* Initialize simplex           */ 
                                m_simplices[0].rank     =       0;
                                m_ray                           =       guess;
                                const btScalar  sqrl=   m_ray.length2();
                                appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
                                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      {
                                        const U         next=1-m_current;
                                        sSimplex&       cs=m_simplices[m_current];
                                        sSimplex&       ns=m_simplices[next];
                                        /* Check zero                                                   */ 
                                        const btScalar  rl=m_ray.length();
                                        if(rl<GJK_MIN_DISTANCE)
                                        {/* Touching or inside                          */ 
                                                m_status=eStatus::Inside;
                                                break;
                                        }
                                        /* Append new vertice in -'v' direction */ 
                                        appendvertice(cs,-m_ray);
                                        const btVector3&        w=cs.c[cs.rank-1]->w;
                                        bool                            found=false;
                                        for(U i=0;i<4;++i)
                                        {
                                                if((w-lastw[i]).length2()<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                                */ 
                                        const btScalar  omega=btDot(m_ray,w)/rl;
                                        alpha=btMax(omega,alpha);
                                        if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
                                        {/* Return old simplex                          */ 
                                                removevertice(m_simplices[m_current]);
                                                break;
                                        }               
                                        /* Reduce simplex                                               */ 
                                        btScalar        weights[4];
                                        U                       mask=0;
                                        switch(cs.rank)
                                        {
                                        case    2:      sqdist=projectorigin(   cs.c[0]->w,
                                                                        cs.c[1]->w,
                                                                        weights,mask);break;
                                        case    3:      sqdist=projectorigin(   cs.c[0]->w,
                                                                        cs.c[1]->w,
                                                                        cs.c[2]->w,
                                                                        weights,mask);break;
                                        case    4:      sqdist=projectorigin(   cs.c[0]->w,
                                                                        cs.c[1]->w,
                                                                        cs.c[2]->w,
                                                                        cs.c[3]->w,
                                                                        weights,mask);break;
                                        }
                                        if(sqdist>=0)
                                        {/* Valid       */ 
                                                ns.rank         =       0;
                                                m_ray           =       btVector3(0,0,0);
                                                m_current       =       next;
                                                for(U i=0,ni=cs.rank;i<ni;++i)
                                                {
                                                        if(mask&(1<<i))
                                                        {
                                                                ns.c[ns.rank]           =       cs.c[i];
                                                                ns.p[ns.rank++]         =       weights[i];
                                                                m_ray                           +=      cs.c[i]->w*weights[i];
                                                        }
                                                        else
                                                        {
                                                                m_free[m_nfree++]       =       cs.c[i];
                                                        }
                                                }
                                                if(mask==15) m_status=eStatus::Inside;
                                        }
                                        else
                                        {/* Return old simplex                          */ 
                                                removevertice(m_simplices[m_current]);
                                                break;
                                        }
                                        m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
                                } while(m_status==eStatus::Valid);
                                m_simplex=&m_simplices[m_current];
                                switch(m_status)
                                {
                                case    eStatus::Valid:         m_distance=m_ray.length();break;
                                case    eStatus::Inside:        m_distance=0;break;
                                default:
                                        {
                                        }
                                }       
                                return(m_status);
                        }
                        bool                                    EncloseOrigin()
                        {
                                switch(m_simplex->rank)
                                {
                                case    1:
                                        {
                                                for(U i=0;i<3;++i)
                                                {
                                                        btVector3               axis=btVector3(0,0,0);
                                                        axis[i]=1;
                                                        appendvertice(*m_simplex, axis);
                                                        if(EncloseOrigin())     return(true);
                                                        removevertice(*m_simplex);
                                                        appendvertice(*m_simplex,-axis);
                                                        if(EncloseOrigin())     return(true);
                                                        removevertice(*m_simplex);
                                                }
                                        }
                                        break;
                                case    2:
                                        {
                                                const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
                                                for(U i=0;i<3;++i)
                                                {
                                                        btVector3               axis=btVector3(0,0,0);
                                                        axis[i]=1;
                                                        const btVector3 p=btCross(d,axis);
                                                        if(p.length2()>0)
                                                        {
                                                                appendvertice(*m_simplex, p);
                                                                if(EncloseOrigin())     return(true);
                                                                removevertice(*m_simplex);
                                                                appendvertice(*m_simplex,-p);
                                                                if(EncloseOrigin())     return(true);
                                                                removevertice(*m_simplex);
                                                        }
                                                }
                                        }
                                        break;
                                case    3:
                                        {
                                                const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
                                                        m_simplex->c[2]->w-m_simplex->c[0]->w);
                                                if(n.length2()>0)
                                                {
                                                        appendvertice(*m_simplex,n);
                                                        if(EncloseOrigin())     return(true);
                                                        removevertice(*m_simplex);
                                                        appendvertice(*m_simplex,-n);
                                                        if(EncloseOrigin())     return(true);
                                                        removevertice(*m_simplex);
                                                }
                                        }
                                        break;
                                case    4:
                                        {
                                                if(btFabs(det(  m_simplex->c[0]->w-m_simplex->c[3]->w,
                                                        m_simplex->c[1]->w-m_simplex->c[3]->w,
                                                        m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
                                                        return(true);
                                        }
                                        break;
                                }
                                return(false);
                        }
                        /* Internals    */ 
                        void                            getsupport(const btVector3& d,sSV& sv) const
                        {
                                sv.d    =       d/d.length();
                                sv.w    =       m_shape.Support(sv.d);
                        }
                        void                            removevertice(sSimplex& simplex)
                        {
                                m_free[m_nfree++]=simplex.c[--simplex.rank];
                        }
                        void                            appendvertice(sSimplex& simplex,const btVector3& v)
                        {
                                simplex.p[simplex.rank]=0;
                                simplex.c[simplex.rank]=m_free[--m_nfree];
                                getsupport(v,*simplex.c[simplex.rank++]);
                        }
                        static btScalar         det(const btVector3& a,const btVector3& b,const btVector3& c)
                        {
                                return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
                                        a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
                                        a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
                        }
                        static btScalar         projectorigin(  const btVector3& a,
                                const btVector3& b,
                                btScalar* w,U& m)
                        {
                                const btVector3 d=b-a;
                                const btScalar  l=d.length2();
                                if(l>GJK_SIMPLEX2_EPS)
                                {
                                        const btScalar  t(l>0?-btDot(a,d)/l:0);
                                        if(t>=1)                { w[0]=0;w[1]=1;m=2;return(b.length2()); }
                                        else if(t<=0)   { w[0]=1;w[1]=0;m=1;return(a.length2()); }
                                        else                    { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
                                }
                                return(-1);
                        }
                        static btScalar         projectorigin(  const btVector3& a,
                                const btVector3& b,
                                const btVector3& c,
                                btScalar* w,U& m)
                        {
                                static const U          imd3[]={1,2,0};
                                const btVector3*        vt[]={&a,&b,&c};
                                const btVector3         dl[]={a-b,b-c,c-a};
                                const btVector3         n=btCross(dl[0],dl[1]);
                                const btScalar          l=n.length2();
                                if(l>GJK_SIMPLEX3_EPS)
                                {
                                        btScalar        mindist=-1;
                                        btScalar        subw[2]={0.f,0.f};
                                        U                       subm(0);
                                        for(U i=0;i<3;++i)
                                        {
                                                if(btDot(*vt[i],btCross(dl[i],n))>0)
                                                {
                                                        const U                 j=imd3[i];
                                                        const btScalar  subd(projectorigin(*vt[i],*vt[j],subw,subm));
                                                        if((mindist<0)||(subd<mindist))
                                                        {
                                                                mindist         =       subd;
                                                                m                       =       static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
                                                                w[i]            =       subw[0];
                                                                w[j]            =       subw[1];
                                                                w[imd3[j]]      =       0;                              
                                                        }
                                                }
                                        }
                                        if(mindist<0)
                                        {
                                                const btScalar  d=btDot(a,n);   
                                                const btScalar  s=btSqrt(l);
                                                const btVector3 p=n*(d/l);
                                                mindist =       p.length2();
                                                m               =       7;
                                                w[0]    =       (btCross(dl[1],b-p)).length()/s;
                                                w[1]    =       (btCross(dl[2],c-p)).length()/s;
                                                w[2]    =       1-(w[0]+w[1]);
                                        }
                                        return(mindist);
                                }
                                return(-1);
                        }
                        static btScalar         projectorigin(  const btVector3& a,
                                const btVector3& b,
                                const btVector3& c,
                                const btVector3& d,
                                btScalar* w,U& m)
                        {
                                static const U          imd3[]={1,2,0};
                                const btVector3*        vt[]={&a,&b,&c,&d};
                                const btVector3         dl[]={a-d,b-d,c-d};
                                const btScalar          vl=det(dl[0],dl[1],dl[2]);
                                const bool                      ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
                                if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
                                {
                                        btScalar        mindist=-1;
                                        btScalar        subw[3]={0.f,0.f,0.f};
                                        U                       subm(0);
                                        for(U i=0;i<3;++i)
                                        {
                                                const U                 j=imd3[i];
                                                const btScalar  s=vl*btDot(d,btCross(dl[i],dl[j]));
                                                if(s>0)
                                                {
                                                        const btScalar  subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
                                                        if((mindist<0)||(subd<mindist))
                                                        {
                                                                mindist         =       subd;
                                                                m                       =       static_cast<U>((subm&1?1<<i:0)+
                                                                        (subm&2?1<<j:0)+
                                                                        (subm&4?8:0));
                                                                w[i]            =       subw[0];
                                                                w[j]            =       subw[1];
                                                                w[imd3[j]]      =       0;
                                                                w[3]            =       subw[2];
                                                        }
                                                }
                                        }
                                        if(mindist<0)
                                        {
                                                mindist =       0;
                                                m               =       15;
                                                w[0]    =       det(c,b,d)/vl;
                                                w[1]    =       det(a,c,d)/vl;
                                                w[2]    =       det(b,a,d)/vl;
                                                w[3]    =       1-(w[0]+w[1]+w[2]);
                                        }
                                        return(mindist);
                                }
                                return(-1);
                        }
        };

        // EPA
        struct  EPA
        {
                /* Types                */ 
                typedef GJK::sSV        sSV;
                struct  sFace
                {
                        btVector3       n;
                        btScalar        d;
                        sSV*            c[3];
                        sFace*          f[3];
                        sFace*          l[2];
                        U1                      e[3];
                        U1                      pass;
                };
                struct  sList
                {
                        sFace*          root;
                        U                       count;
                        sList() : root(0),count(0)      {}
                };
                struct  sHorizon
                {
                        sFace*          cf;
                        sFace*          ff;
                        U                       nf;
                        sHorizon() : cf(0),ff(0),nf(0)  {}
                };
                struct  eStatus { enum _ {
                        Valid,
                        Touching,
                        Degenerated,
                        NonConvex,
                        InvalidHull,            
                        OutOfFaces,
                        OutOfVertices,
                        AccuraryReached,
                        FallBack,
                        Failed          };};
                        /* Fields               */ 
                        eStatus::_              m_status;
                        GJK::sSimplex   m_result;
                        btVector3               m_normal;
                        btScalar                m_depth;
                        sSV                             m_sv_store[EPA_MAX_VERTICES];
                        sFace                   m_fc_store[EPA_MAX_FACES];
                        U                               m_nextsv;
                        sList                   m_hull;
                        sList                   m_stock;
                        /* Methods              */ 
                        EPA()
                        {
                                Initialize();   
                        }


                        static inline void              bind(sFace* fa,U ea,sFace* fb,U eb)
                        {
                                fa->e[ea]=(U1)eb;fa->f[ea]=fb;
                                fb->e[eb]=(U1)ea;fb->f[eb]=fa;
                        }
                        static inline void              append(sList& list,sFace* face)
                        {
                                face->l[0]      =       0;
                                face->l[1]      =       list.root;
                                if(list.root) list.root->l[0]=face;
                                list.root       =       face;
                                ++list.count;
                        }
                        static inline void              remove(sList& list,sFace* face)
                        {
                                if(face->l[1]) face->l[1]->l[0]=face->l[0];
                                if(face->l[0]) face->l[0]->l[1]=face->l[1];
                                if(face==list.root) list.root=face->l[1];
                                --list.count;
                        }


                        void                            Initialize()
                        {
                                m_status        =       eStatus::Failed;
                                m_normal        =       btVector3(0,0,0);
                                m_depth         =       0;
                                m_nextsv        =       0;
                                for(U i=0;i<EPA_MAX_FACES;++i)
                                {
                                        append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
                                }
                        }
                        eStatus::_                      Evaluate(GJK& gjk,const btVector3& guess)
                        {
                                GJK::sSimplex&  simplex=*gjk.m_simplex;
                                if((simplex.rank>1)&&gjk.EncloseOrigin())
                                {

                                        /* Clean up                             */ 
                                        while(m_hull.root)
                                        {
                                                sFace*  f = m_hull.root;
                                                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)
                                        {
                                                btSwap(simplex.c[0],simplex.c[1]);
                                                btSwap(simplex.p[0],simplex.p[1]);
                                        }
                                        /* Build initial hull   */ 
                                        sFace*  tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
                                                newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
                                                newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
                                                newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
                                        if(m_hull.count==4)
                                        {
                                                sFace*          best=findbest();
                                                sFace           outer=*best;
                                                U                       pass=0;
                                                U                       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<EPA_MAX_ITERATIONS;++iterations)
                                                {
                                                        if(m_nextsv<EPA_MAX_VERTICES)
                                                        {       
                                                                sHorizon                horizon;
                                                                sSV*                    w=&m_sv_store[m_nextsv++];
                                                                bool                    valid=true;                                     
                                                                best->pass      =       (U1)(++pass);
                                                                gjk.getsupport(best->n,*w);
                                                                const btScalar  wdist=btDot(best->n,w->w)-best->d;
                                                                if(wdist>EPA_ACCURACY)
                                                                {
                                                                        for(U j=0;(j<3)&&valid;++j)
                                                                        {
                                                                                valid&=expand(  pass,w,
                                                                                        best->f[j],best->e[j],
                                                                                        horizon);
                                                                        }
                                                                        if(valid&&(horizon.nf>=3))
                                                                        {
                                                                                bind(horizon.cf,1,horizon.ff,2);
                                                                                remove(m_hull,best);
                                                                                append(m_stock,best);
                                                                                best=findbest();
                                                                                outer=*best;
                                                                        } else { m_status=eStatus::InvalidHull;break; }
                                                                } else { m_status=eStatus::AccuraryReached;break; }
                                                        } else { m_status=eStatus::OutOfVertices;break; }
                                                }
                                                const btVector3 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]   =       btCross(        outer.c[1]->w-projection,
                                                        outer.c[2]->w-projection).length();
                                                m_result.p[1]   =       btCross(        outer.c[2]->w-projection,
                                                        outer.c[0]->w-projection).length();
                                                m_result.p[2]   =       btCross(        outer.c[0]->w-projection,
                                                        outer.c[1]->w-projection).length();
                                                const btScalar  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;
                                const btScalar  nl=m_normal.length();
                                if(nl>0)
                                        m_normal        =       m_normal/nl;
                                else
                                        m_normal        =       btVector3(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);
                        }
                        bool getedgedist(sFace* face, sSV* a, sSV* b, btScalar& dist)
                        {
                                const btVector3 ba = b->w - a->w;
                                const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane
                                const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required

                                if(a_dot_nab < 0)
                                {
                                        // Outside of edge a->b

                                        const btScalar ba_l2 = ba.length2();
                                        const btScalar a_dot_ba = btDot(a->w, ba);
                                        const btScalar b_dot_ba = btDot(b->w, ba);

                                        if(a_dot_ba > 0)
                                        {
                                                // Pick distance vertex a
                                                dist = a->w.length();
                                        }
                                        else if(b_dot_ba < 0)
                                        {
                                                // Pick distance vertex b
                                                dist = b->w.length();
                                        }
                                        else
                                        {
                                                // Pick distance to edge a->b
                                                const btScalar a_dot_b = btDot(a->w, b->w);
                                                dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
                                        }

                                        return true;
                                }

                                return false;
                        }
                        sFace*                          newface(sSV* a,sSV* b,sSV* c,bool forced)
                        {
                                if(m_stock.root)
                                {
                                        sFace*  face=m_stock.root;
                                        remove(m_stock,face);
                                        append(m_hull,face);
                                        face->pass      =       0;
                                        face->c[0]      =       a;
                                        face->c[1]      =       b;
                                        face->c[2]      =       c;
                                        face->n         =       btCross(b->w-a->w,c->w-a->w);
                                        const btScalar  l=face->n.length();
                                        const bool              v=l>EPA_ACCURACY;

                                        if(v)
                                        {
                                                if(!(getedgedist(face, a, b, face->d) ||
                                                         getedgedist(face, b, c, face->d) ||
                                                         getedgedist(face, c, a, face->d)))
                                                {
                                                        // Origin projects to the interior of the triangle
                                                        // Use distance to triangle plane
                                                        face->d = btDot(a->w, face->n) / l;
                                                }

                                                face->n /= l;
                                                if(forced || (face->d >= -EPA_PLANE_EPS))
                                                {
                                                        return face;
                                                }
                                                else
                                                        m_status=eStatus::NonConvex;
                                        }
                                        else
                                                m_status=eStatus::Degenerated;

                                        remove(m_hull, face);
                                        append(m_stock, face);
                                        return 0;

                                }
                                m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
                                return 0;
                        }
                        sFace*                          findbest()
                        {
                                sFace*          minf=m_hull.root;
                                btScalar        mind=minf->d*minf->d;
                                for(sFace* f=minf->l[1];f;f=f->l[1])
                                {
                                        const btScalar  sqd=f->d*f->d;
                                        if(sqd<mind)
                                        {
                                                minf=f;
                                                mind=sqd;
                                        }
                                }
                                return(minf);
                        }
                        bool                            expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
                        {
                                static const U  i1m3[]={1,2,0};
                                static const U  i2m3[]={2,0,1};
                                if(f->pass!=pass)
                                {
                                        const U e1=i1m3[e];
                                        if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
                                        {
                                                sFace*  nf=newface(f->c[e1],f->c[e],w,false);
                                                if(nf)
                                                {
                                                        bind(nf,0,f,e);
                                                        if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
                                                        horizon.cf=nf;
                                                        ++horizon.nf;
                                                        return(true);
                                                }
                                        }
                                        else
                                        {
                                                const U e2=i2m3[e];
                                                f->pass         =       (U1)pass;
                                                if(     expand(pass,w,f->f[e1],f->e[e1],horizon)&&
                                                        expand(pass,w,f->f[e2],f->e[e2],horizon))
                                                {
                                                        remove(m_hull,f);
                                                        append(m_stock,f);
                                                        return(true);
                                                }
                                        }
                                }
                                return(false);
                        }

        };

        //
        static void     Initialize(     const btConvexShape* shape0,const btTransform& wtrs0,
                const btConvexShape* shape1,const btTransform& wtrs1,
                btGjkEpaSolver2::sResults& results,
                tShape& shape,
                bool withmargins)
        {
                /* Results              */ 
                results.witnesses[0]    =
                        results.witnesses[1]    =       btVector3(0,0,0);
                results.status                  =       btGjkEpaSolver2::sResults::Separated;
                /* Shape                */ 
                shape.m_shapes[0]               =       shape0;
                shape.m_shapes[1]               =       shape1;
                shape.m_toshape1                =       wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
                shape.m_toshape0                =       wtrs0.inverseTimes(wtrs1);
                shape.EnableMargin(withmargins);
        }

}

//
// Api
//

using namespace gjkepa2_impl;

//
int                     btGjkEpaSolver2::StackSizeRequirement()
{
        return(sizeof(GJK)+sizeof(EPA));
}

//
bool            btGjkEpaSolver2::Distance(      const btConvexShape*    shape0,
                                                                          const btTransform&            wtrs0,
                                                                          const btConvexShape*  shape1,
                                                                          const btTransform&            wtrs1,
                                                                          const btVector3&              guess,
                                                                          sResults&                             results)
{
        tShape                  shape;
        Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
        GJK                             gjk;
        GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
        if(gjk_status==GJK::eStatus::Valid)
        {
                btVector3       w0=btVector3(0,0,0);
                btVector3       w1=btVector3(0,0,0);
                for(U i=0;i<gjk.m_simplex->rank;++i)
                {
                        const btScalar  p=gjk.m_simplex->p[i];
                        w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
                        w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
                }
                results.witnesses[0]    =       wtrs0*w0;
                results.witnesses[1]    =       wtrs0*w1;
                results.normal                  =       w0-w1;
                results.distance                =       results.normal.length();
                results.normal                  /=      results.distance>GJK_MIN_DISTANCE?results.distance:1;
                return(true);
        }
        else
        {
                results.status  =       gjk_status==GJK::eStatus::Inside?
                        sResults::Penetrating   :
                sResults::GJK_Failed    ;
                return(false);
        }
}

//
bool    btGjkEpaSolver2::Penetration(   const btConvexShape*    shape0,
                                                                         const btTransform&             wtrs0,
                                                                         const btConvexShape*   shape1,
                                                                         const btTransform&             wtrs1,
                                                                         const btVector3&               guess,
                                                                         sResults&                              results,
                                                                         bool                                   usemargins)
{
        tShape                  shape;
        Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
        GJK                             gjk;    
        GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
        switch(gjk_status)
        {
        case    GJK::eStatus::Inside:
                {
                        EPA                             epa;
                        EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
                        if(epa_status!=EPA::eStatus::Failed)
                        {
                                btVector3       w0=btVector3(0,0,0);
                                for(U i=0;i<epa.m_result.rank;++i)
                                {
                                        w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
                                }
                                results.status                  =       sResults::Penetrating;
                                results.witnesses[0]    =       wtrs0*w0;
                                results.witnesses[1]    =       wtrs0*(w0-epa.m_normal*epa.m_depth);
                                results.normal                  =       -epa.m_normal;
                                results.distance                =       -epa.m_depth;
                                return(true);
                        } else results.status=sResults::EPA_Failed;
                }
                break;
        case    GJK::eStatus::Failed:
                results.status=sResults::GJK_Failed;
                break;
                default:
                                        {
                                        }
        }
        return(false);
}

#ifndef __SPU__
//
btScalar        btGjkEpaSolver2::SignedDistance(const btVector3& position,
                                                                                        btScalar margin,
                                                                                        const btConvexShape* shape0,
                                                                                        const btTransform& wtrs0,
                                                                                        sResults& results)
{
        tShape                  shape;
        btSphereShape   shape1(margin);
        btTransform             wtrs1(btQuaternion(0,0,0,1),position);
        Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
        GJK                             gjk;    
        GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
        if(gjk_status==GJK::eStatus::Valid)
        {
                btVector3       w0=btVector3(0,0,0);
                btVector3       w1=btVector3(0,0,0);
                for(U i=0;i<gjk.m_simplex->rank;++i)
                {
                        const btScalar  p=gjk.m_simplex->p[i];
                        w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
                        w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
                }
                results.witnesses[0]    =       wtrs0*w0;
                results.witnesses[1]    =       wtrs0*w1;
                const btVector3 delta=  results.witnesses[1]-
                        results.witnesses[0];
                const btScalar  margin= shape0->getMarginNonVirtual()+
                        shape1.getMarginNonVirtual();
                const btScalar  length= delta.length(); 
                results.normal                  =       delta/length;
                results.witnesses[0]    +=      results.normal*margin;
                return(length-margin);
        }
        else
        {
                if(gjk_status==GJK::eStatus::Inside)
                {
                        if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
                        {
                                const btVector3 delta=  results.witnesses[0]-
                                        results.witnesses[1];
                                const btScalar  length= delta.length();
                                if (length >= SIMD_EPSILON)
                                        results.normal  =       delta/length;                   
                                return(-length);
                        }
                }       
        }
        return(SIMD_INFINITY);
}

//
bool    btGjkEpaSolver2::SignedDistance(const btConvexShape*    shape0,
                                                                                const btTransform&              wtrs0,
                                                                                const btConvexShape*    shape1,
                                                                                const btTransform&              wtrs1,
                                                                                const btVector3&                guess,
                                                                                sResults&                               results)
{
        if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
                return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
        else
                return(true);
}
#endif //__SPU__

/* Symbols cleanup              */ 

#undef GJK_MAX_ITERATIONS
#undef GJK_ACCURARY
#undef GJK_MIN_DISTANCE
#undef GJK_DUPLICATED_EPS
#undef GJK_SIMPLEX2_EPS
#undef GJK_SIMPLEX3_EPS
#undef GJK_SIMPLEX4_EPS

#undef EPA_MAX_VERTICES
#undef EPA_MAX_FACES
#undef EPA_MAX_ITERATIONS
#undef EPA_ACCURACY
#undef EPA_FALLBACK
#undef EPA_PLANE_EPS
#undef EPA_INSIDE_EPS