text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2014 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.
*/

/*
Initial GJK-EPA collision solver by Nathanael Presson, 2008
Improvements and refactoring by Erwin Coumans, 2008-2014
*/
#ifndef BT_GJK_EPA3_H
#define BT_GJK_EPA3_H

#include "LinearMath/btTransform.h"
#include "btGjkCollisionDescription.h"

struct	btGjkEpaSolver3
{
struct	sResults
	{
	enum eStatus
		{
		Separated,		/* Shapes doesnt penetrate												*/
		Penetrating,	/* Shapes are penetrating												*/
		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/
		EPA_Failed		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/
		}		status;
	btVector3	witnesses[2];
	btVector3	normal;
	btScalar	distance;
	};

};

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

    // 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
    template <typename btConvexTemplate>
    struct	MinkowskiDiff
    {
        const btConvexTemplate* m_convexAPtr;
        const btConvexTemplate* m_convexBPtr;

        btMatrix3x3				m_toshape1;
        btTransform				m_toshape0;

        bool					m_enableMargin;

        MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b)
        :m_convexAPtr(&a),
        m_convexBPtr(&b)
        {
        }

        void					EnableMargin(bool enable)
        {
            m_enableMargin = enable;
        }
        inline btVector3		Support0(const btVector3& d) const
        {
            return m_convexAPtr->getLocalSupportWithMargin(d);
        }
        inline btVector3		Support1(const btVector3& d) const
        {
            return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(m_toshape1*d);
        }

        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));
        }
    };

enum	eGjkStatus
{
    eGjkValid,
    eGjkInside,
    eGjkFailed
};

    // GJK
    template <typename btConvexTemplate>
    struct	GJK
    {
        /* Types		*/
        struct	sSV
        {
            btVector3	d,w;
        };
        struct	sSimplex
        {
            sSV*		c[4];
            btScalar	p[4];
            U			rank;
        };

        /* Fields		*/

        MinkowskiDiff<btConvexTemplate>			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;
        eGjkStatus      m_status;
        /* Methods		*/

        GJK(const btConvexTemplate& a, const btConvexTemplate& b)
        :m_shape(a,b)
        {
            Initialize();
        }
        void				Initialize()
        {
            m_ray		=	btVector3(0,0,0);
            m_nfree		=	0;
            m_status	=	eGjkFailed;
            m_current	=	0;
            m_distance	=	0;
        }
        eGjkStatus			Evaluate(const MinkowskiDiff<btConvexTemplate>& 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			=	eGjkValid;
            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=eGjkInside;
                    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=eGjkInside;
                }
                else
                {/* Return old simplex				*/
                    removevertice(m_simplices[m_current]);
                    break;
                }
                m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eGjkFailed;
            } while(m_status==eGjkValid);
            m_simplex=&m_simplices[m_current];
            switch(m_status)
            {
                case	eGjkValid:		m_distance=m_ray.length();break;
                case	eGjkInside:	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);
        }
    };

enum	eEpaStatus
{
    eEpaValid,
    eEpaTouching,
    eEpaDegenerated,
    eEpaNonConvex,
    eEpaInvalidHull,
    eEpaOutOfFaces,
    eEpaOutOfVertices,
    eEpaAccuraryReached,
    eEpaFallBack,
    eEpaFailed
};

    // EPA
template <typename btConvexTemplate>
    struct	EPA
    {
        /* Types		*/

        struct	sFace
        {
            btVector3	n;
            btScalar	d;
            typename GJK<btConvexTemplate>::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)	{}
        };

        /* Fields		*/
        eEpaStatus		m_status;
        typename GJK<btConvexTemplate>::sSimplex	m_result;
        btVector3		m_normal;
        btScalar		m_depth;
        typename GJK<btConvexTemplate>::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	=	eEpaFailed;
            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]);
            }
        }
        eEpaStatus			Evaluate(GJK<btConvexTemplate>& gjk,const btVector3& guess)
        {
            typename GJK<btConvexTemplate>::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	=	eEpaValid;
                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=eEpaValid;
                    for(;iterations<EPA_MAX_ITERATIONS;++iterations)
                    {
                        if(m_nextsv<EPA_MAX_VERTICES)
                        {
                            sHorizon		horizon;
                            typename GJK<btConvexTemplate>::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=eEpaInvalidHull;break; }
                            } else { m_status=eEpaAccuraryReached;break; }
                        } else { m_status=eEpaOutOfVertices;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	=	eEpaFallBack;
            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, typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::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(typename GJK<btConvexTemplate>::sSV* a,typename GJK<btConvexTemplate>::sSV* b,typename GJK<btConvexTemplate>::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=eEpaNonConvex;
                }
                else
                    m_status=eEpaDegenerated;

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

            }
            m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces;
            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,typename GJK<btConvexTemplate>::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);
        }

    };

    template <typename btConvexTemplate>
    static void	Initialize(	const btConvexTemplate& a, const btConvexTemplate& b,
                           btGjkEpaSolver3::sResults& results,
                           MinkowskiDiff<btConvexTemplate>& shape)
    {
        /* Results		*/
        results.witnesses[0]	=
        results.witnesses[1]	=	btVector3(0,0,0);
        results.status			=	btGjkEpaSolver3::sResults::Separated;
        /* Shape		*/

        shape.m_toshape1		=	b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis());
        shape.m_toshape0		=	a.getWorldTransform().inverseTimes(b.getWorldTransform());

    }

//
// Api
//

//
template <typename btConvexTemplate>
bool		btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b,
                                      const btVector3& guess,
                                      btGjkEpaSolver3::sResults& results)
{
    MinkowskiDiff<btConvexTemplate>			shape(a,b);
    Initialize(a,b,results,shape);
    GJK<btConvexTemplate>				gjk(a,b);
    eGjkStatus	gjk_status=gjk.Evaluate(shape,guess);
    if(gjk_status==eGjkValid)
    {
        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]	=	a.getWorldTransform()*w0;
        results.witnesses[1]	=	a.getWorldTransform()*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==eGjkInside?
        btGjkEpaSolver3::sResults::Penetrating	:
        btGjkEpaSolver3::sResults::GJK_Failed	;
        return(false);
    }
}

template <typename btConvexTemplate>
bool	btGjkEpaSolver3_Penetration(const btConvexTemplate& a,
                                     const btConvexTemplate& b,
                                     const btVector3& guess,
                                     btGjkEpaSolver3::sResults& results)
{
    MinkowskiDiff<btConvexTemplate>			shape(a,b);
    Initialize(a,b,results,shape);
    GJK<btConvexTemplate>				gjk(a,b);
    eGjkStatus	gjk_status=gjk.Evaluate(shape,-guess);
    switch(gjk_status)
    {
        case	eGjkInside:
        {
            EPA<btConvexTemplate>				epa;
            eEpaStatus	epa_status=epa.Evaluate(gjk,-guess);
            if(epa_status!=eEpaFailed)
            {
                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			=	btGjkEpaSolver3::sResults::Penetrating;
                results.witnesses[0]	=	a.getWorldTransform()*w0;
                results.witnesses[1]	=	a.getWorldTransform()*(w0-epa.m_normal*epa.m_depth);
                results.normal			=	-epa.m_normal;
                results.distance		=	-epa.m_depth;
                return(true);
            } else results.status=btGjkEpaSolver3::sResults::EPA_Failed;
        }
            break;
        case	eGjkFailed:
            results.status=btGjkEpaSolver3::sResults::GJK_Failed;
            break;
        default:
        {
        }
    }
    return(false);
}

#if 0
int	btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo)
{
    btGjkEpaSolver3::sResults results;
    btVector3 guess = colDesc.m_firstDir;

    bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB,
                                            colDesc.m_transformA,colDesc.m_transformB,
                                            colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB,
                                            guess,
                                            results);
    if (res)
    {
        if ((results.status==btGjkEpaSolver3::sResults::Penetrating) || results.status==GJK::eStatus::Inside)
        {
            //normal could be 'swapped'

            distInfo->m_distance = results.distance;
            distInfo->m_normalBtoA = results.normal;
            btVector3 tmpNormalInB = results.witnesses[1]-results.witnesses[0];
            btScalar lenSqr = tmpNormalInB.length2();
            if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
            {
                tmpNormalInB = results.normal;
                lenSqr = results.normal.length2();
            }

            if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
            {
                tmpNormalInB /= btSqrt(lenSqr);
                btScalar distance2 = -(results.witnesses[0]-results.witnesses[1]).length();
                //only replace valid penetrations when the result is deeper (check)
                //if ((distance2 < results.distance))
                {
                    distInfo->m_distance = distance2;
                    distInfo->m_pointOnA= results.witnesses[0];
                    distInfo->m_pointOnB= results.witnesses[1];
                    distInfo->m_normalBtoA= tmpNormalInB;
                    return 0;
                }
            }
        }

    }

    return -1;
}
#endif

template <typename btConvexTemplate, typename btDistanceInfoTemplate>
int	btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b,
                         const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo)
{
    btGjkEpaSolver3::sResults results;
    btVector3 guess = colDesc.m_firstDir;

    bool isSeparated = btGjkEpaSolver3_Distance(	a,b,
                                                 guess,
                                                 results);
    if (isSeparated)
    {
        distInfo->m_distance = results.distance;
        distInfo->m_pointOnA= results.witnesses[0];
        distInfo->m_pointOnB= results.witnesses[1];
        distInfo->m_normalBtoA= results.normal;
        return 0;
    }

    return -1;
}

/* 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

#endif //BT_GJK_EPA3_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_MINKOWSKI_SUM_SHAPE_H
#define BT_MINKOWSKI_SUM_SHAPE_H

#include "btConvexInternalShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types

/// The btMinkowskiSumShape is only for advanced users. This shape represents implicit based minkowski sum of two convex implicit shapes.
ATTRIBUTE_ALIGNED16(class) btMinkowskiSumShape : public btConvexInternalShape
{

	btTransform	m_transA;
	btTransform	m_transB;
	const btConvexShape*	m_shapeA;
	const btConvexShape*	m_shapeB;

public:

BT_DECLARE_ALIGNED_ALLOCATOR();

	btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB);

	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;

	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;

	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;

	void	setTransformA(const btTransform&	transA) { m_transA = transA;}
	void	setTransformB(const btTransform&	transB) { m_transB = transB;}

	const btTransform& getTransformA()const  { return m_transA;}
	const btTransform& GetTransformB()const  { return m_transB;}

	virtual btScalar	getMargin() const;

	const btConvexShape*	getShapeA() const { return m_shapeA;}
	const btConvexShape*	getShapeB() const { return m_shapeB;}

	virtual const char*	getName()const
	{
		return "MinkowskiSum";
	}
};

#endif //BT_MINKOWSKI_SUM_SHAPE_H

repo_name: JoshParnell/libphx

repo_language: C

mime_type: text/x-c++


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_TRIANGLE_MESH_SHAPE_H
#define BT_TRIANGLE_MESH_SHAPE_H

#include "btConcaveShape.h"
#include "btStridingMeshInterface.h"

///The btTriangleMeshShape is an internal concave triangle mesh interface. Don't use this class directly, use btBvhTriangleMeshShape instead.
ATTRIBUTE_ALIGNED16(class) btTriangleMeshShape : public btConcaveShape
{
protected:
	btVector3	m_localAabbMin;
	btVector3	m_localAabbMax;
	btStridingMeshInterface* m_meshInterface;

	///btTriangleMeshShape constructor has been disabled/protected, so that users will not mistakenly use this class.
	///Don't use btTriangleMeshShape but use btBvhTriangleMeshShape instead!
	btTriangleMeshShape(btStridingMeshInterface* meshInterface);

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	virtual ~btTriangleMeshShape();

	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;

	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
	{
		btAssert(0);
		return localGetSupportingVertex(vec);
	}

	void	recalcLocalAabb();

	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;

	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;

	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;

	virtual void	setLocalScaling(const btVector3& scaling);
	virtual const btVector3& getLocalScaling() const;

	btStridingMeshInterface* getMeshInterface()
	{
		return m_meshInterface;
	}

	const btStridingMeshInterface* getMeshInterface() const
	{
		return m_meshInterface;
	}

	const btVector3& getLocalAabbMin() const
	{
		return m_localAabbMin;
	}
	const btVector3& getLocalAabbMax() const
	{
		return m_localAabbMax;
	}

	//debugging
	virtual const char*	getName()const {return "TRIANGLEMESH";}

};

#endif //BT_TRIANGLE_MESH_SHAPE_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_UNIFORM_SCALING_SHAPE_H
#define BT_UNIFORM_SCALING_SHAPE_H

#include "btConvexShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"

/// The btUniformScalingShape allows to re-use uniform scaled instances of
/// btConvexShape in a memory efficient way. Instead of using
/// btUniformScalingShape, it is better to use the non-uniform setLocalScaling
/// method on convex shapes that implement it.
ATTRIBUTE_ALIGNED16(class) btUniformScalingShape : public btConvexShape
{
	btConvexShape* m_childConvexShape;
	btScalar       m_uniformScalingFactor;

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	btUniformScalingShape(btConvexShape* convexChildShape, btScalar uniformScalingFactor);
	virtual ~btUniformScalingShape();

	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
	virtual void      batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
	virtual void      calculateLocalInertia(btScalar mass, btVector3 & inertia) const;

	btScalar             getUniformScalingFactor() const { return m_uniformScalingFactor; }
	btConvexShape*       getChildShape() { return m_childConvexShape; }
	const btConvexShape* getChildShape() const { return m_childConvexShape; }
	virtual const char*  getName() const { return "UniformScalingShape"; }

	/// getAabb's default implementation is brute force, expected derived classes
	/// to implement a fast dedicated version
	void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;

	virtual void             getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
	virtual void             setLocalScaling(const btVector3& scaling);
	virtual const btVector3& getLocalScaling() const;
	virtual void             setMargin(btScalar margin);
	virtual btScalar         getMargin() const;
	virtual int              getNumPreferredPenetrationDirections() const;
	virtual void             getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const;
};

#endif //BT_UNIFORM_SCALING_SHAPE_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_TRIANGLE_MESH_H
#define BT_TRIANGLE_MESH_H

#include "btTriangleIndexVertexArray.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btAlignedObjectArray.h"

///The btTriangleMesh class is a convenience class derived from btTriangleIndexVertexArray, that provides storage for a concave triangle mesh. It can be used as data for the btBvhTriangleMeshShape.
///It allows either 32bit or 16bit indices, and 4 (x-y-z-w) or 3 (x-y-z) component vertices.
///If you want to share triangle/index data between graphics mesh and collision mesh (btBvhTriangleMeshShape), you can directly use btTriangleIndexVertexArray or derive your own class from btStridingMeshInterface.
///Performance of btTriangleMesh and btTriangleIndexVertexArray used in a btBvhTriangleMeshShape is the same.
class btTriangleMesh : public btTriangleIndexVertexArray
{
	btAlignedObjectArray<btVector3>	m_4componentVertices;
	btAlignedObjectArray<btScalar>	m_3componentVertices;

	btAlignedObjectArray<unsigned int>		m_32bitIndices;
	btAlignedObjectArray<unsigned short int>		m_16bitIndices;
	bool	m_use32bitIndices;
	bool	m_use4componentVertices;

	public:
		btScalar	m_weldingThreshold;

		btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true);

		bool	getUse32bitIndices() const
		{
			return m_use32bitIndices;
		}

		bool	getUse4componentVertices() const
		{
			return m_use4componentVertices;
		}
		///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes.
		///In general it is better to directly use btTriangleIndexVertexArray instead.
		void	addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2, bool removeDuplicateVertices=false);

		///Add a triangle using its indices. Make sure the indices are pointing within the vertices array, so add the vertices first (and to be sure, avoid removal of duplicate vertices)
		void	addTriangleIndices(int index1, int index2, int index3 );

		int getNumTriangles() const;

		virtual void	preallocateVertices(int numverts);
		virtual void	preallocateIndices(int numindices);

		///findOrAddVertex is an internal method, use addTriangle instead
		int		findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices);
		///addIndex is an internal method, use addTriangle instead
		void	addIndex(int index);

};

#endif //BT_TRIANGLE_MESH_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_CONVEX_SHAPE_INTERFACE1
#define BT_CONVEX_SHAPE_INTERFACE1

#include "btCollisionShape.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btMatrix3x3.h"
#include "btCollisionMargin.h"
#include "LinearMath/btAlignedAllocator.h"

#define MAX_PREFERRED_PENETRATION_DIRECTIONS 10

/// The btConvexShape is an abstract shape interface, implemented by all convex
/// shapes such as btBoxShape, btConvexHullShape etc. It describes general
/// convex shapes using the localGetSupportingVertex interface, used by
/// collision detectors such as btGjkPairDetector.
ATTRIBUTE_ALIGNED16(class) btConvexShape : public btCollisionShape
{
public:
	BT_DECLARE_ALIGNED_ALLOCATOR();
	btConvexShape ();
	virtual ~btConvexShape();
	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const = 0;

#ifndef __SPU__
	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const = 0;
#endif //#ifndef __SPU__

	btVector3    localGetSupportVertexWithoutMarginNonVirtual(const btVector3& vec) const;
	btVector3    localGetSupportVertexNonVirtual(const btVector3& vec) const;
	btScalar     getMarginNonVirtual() const;
	void         getAabbNonVirtual(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;

	virtual void project(
	    const btTransform& trans,
	    const btVector3&   dir,
	    btScalar&          minProj,
	    btScalar&          maxProj,
	    btVector3&         witnesPtMin,
	    btVector3&         witnesPtMax) const;

	/// notice that the vectors should be unit length
	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(
	    const btVector3* vectors,
	    btVector3*       supportVerticesOut,
	    int              numVectors) const = 0;

	/// getAabb's default implementation is brute force, expected derived classes
	/// to implement a fast dedicated version
	void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0;

	virtual void             getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0;
	virtual void             setLocalScaling(const btVector3& scaling)                                       = 0;
	virtual const btVector3& getLocalScaling() const                                                         = 0;
	virtual void             setMargin(btScalar margin)                                                      = 0;
	virtual btScalar         getMargin() const                                                               = 0;
	virtual int              getNumPreferredPenetrationDirections() const                                    = 0;
	virtual void             getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const = 0;
};

#endif //BT_CONVEX_SHAPE_INTERFACE1


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_CONVEX_2D_SHAPE_H
#define BT_CONVEX_2D_SHAPE_H

#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types

///The btConvex2dShape allows to use arbitrary convex shapes as 2d convex shapes, with the Z component assumed to be 0.
///For 2d boxes, the btBox2dShape is recommended.
ATTRIBUTE_ALIGNED16(class) btConvex2dShape : public btConvexShape
{
	btConvexShape*	m_childConvexShape;

	public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	btConvex2dShape(	btConvexShape* convexChildShape);

	virtual ~btConvex2dShape();

	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;

	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;

	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;

	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;

	btConvexShape*	getChildShape()
	{
		return m_childConvexShape;
	}

	const btConvexShape*	getChildShape() const
	{
		return m_childConvexShape;
	}

	virtual const char*	getName()const
	{
		return "Convex2dShape";
	}

	///////////////////////////

	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;

	virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;

	virtual void	setLocalScaling(const btVector3& scaling) ;
	virtual const btVector3& getLocalScaling() const ;

	virtual void	setMargin(btScalar margin);
	virtual btScalar	getMargin() const;

	virtual int		getNumPreferredPenetrationDirections() const;

	virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const;

};

#endif //BT_CONVEX_2D_SHAPE_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_CONVEX_INTERNAL_SHAPE_H
#define BT_CONVEX_INTERNAL_SHAPE_H

#include "btConvexShape.h"
#include "LinearMath/btAabbUtil2.h"

///The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
///The btConvexInternalShape uses a default collision margin set to CONVEX_DISTANCE_MARGIN.
///This collision margin used by Gjk and some other algorithms, see also btCollisionMargin.h
///Note that when creating small shapes (derived from btConvexInternalShape),
///you need to make sure to set a smaller collision margin, using the 'setMargin' API
///There is a automatic mechanism 'setSafeMargin' used by btBoxShape and btCylinderShape
ATTRIBUTE_ALIGNED16(class) btConvexInternalShape : public btConvexShape
{

	protected:

	//local scaling. collisionMargin is not scaled !
	btVector3	m_localScaling;

	btVector3	m_implicitShapeDimensions;

	btScalar	m_collisionMargin;

	btScalar	m_padding;

	btConvexInternalShape();

public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	virtual ~btConvexInternalShape()
	{

	}

	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;

	const btVector3& getImplicitShapeDimensions() const
	{
		return m_implicitShapeDimensions;
	}

	///warning: use setImplicitShapeDimensions with care
	///changing a collision shape while the body is in the world is not recommended,
	///it is best to remove the body from the world, then make the change, and re-add it
	///alternatively flush the contact points, see documentation for 'cleanProxyFromPairs'
	void	setImplicitShapeDimensions(const btVector3& dimensions)
	{
		m_implicitShapeDimensions = dimensions;
	}

	void	setSafeMargin(btScalar minDimension, btScalar defaultMarginMultiplier = 0.1f)
	{
		btScalar safeMargin = defaultMarginMultiplier*minDimension;
		if (safeMargin < getMargin())
		{
			setMargin(safeMargin);
		}
	}
	void	setSafeMargin(const btVector3& halfExtents, btScalar defaultMarginMultiplier = 0.1f)
	{
		//see http://code.google.com/p/bullet/issues/detail?id=349
		//this margin check could could be added to other collision shapes too,
		//or add some assert/warning somewhere
		btScalar minDimension=halfExtents[halfExtents.minAxis()];
		setSafeMargin(minDimension, defaultMarginMultiplier);
	}

	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
	{
		getAabbSlow(t,aabbMin,aabbMax);
	}

	virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;

	virtual void	setLocalScaling(const btVector3& scaling);
	virtual const btVector3& getLocalScaling() const
	{
		return m_localScaling;
	}

	const btVector3& getLocalScalingNV() const
	{
		return m_localScaling;
	}

	virtual void	setMargin(btScalar margin)
	{
		m_collisionMargin = margin;
	}
	virtual btScalar	getMargin() const
	{
		return m_collisionMargin;
	}

	btScalar	getMarginNV() const
	{
		return m_collisionMargin;
	}

	virtual int		getNumPreferredPenetrationDirections() const
	{
		return 0;
	}

	virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
	{
		(void)penetrationVector;
		(void)index;
		btAssert(0);
	}

	virtual	int	calculateSerializeBufferSize() const;

	///fills the dataBuffer and returns the struct name (and 0 on failure)
	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;

};

///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct	btConvexInternalShapeData
{
	btCollisionShapeData	m_collisionShapeData;

	btVector3FloatData	m_localScaling;

	btVector3FloatData	m_implicitShapeDimensions;

	float			m_collisionMargin;

	int	m_padding;

};

SIMD_FORCE_INLINE	int	btConvexInternalShape::calculateSerializeBufferSize() const
{
	return sizeof(btConvexInternalShapeData);
}

///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE	const char*	btConvexInternalShape::serialize(void* dataBuffer, btSerializer* serializer) const
{
	btConvexInternalShapeData* shapeData = (btConvexInternalShapeData*) dataBuffer;
	btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);

	m_implicitShapeDimensions.serializeFloat(shapeData->m_implicitShapeDimensions);
	m_localScaling.serializeFloat(shapeData->m_localScaling);
	shapeData->m_collisionMargin = float(m_collisionMargin);

	// Fill padding with zeros to appease msan.
	shapeData->m_padding = 0;

	return "btConvexInternalShapeData";
}

///btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive bounding box calculations
class btConvexInternalAabbCachingShape : public btConvexInternalShape
{
	btVector3	m_localAabbMin;
	btVector3	m_localAabbMax;
	bool		m_isLocalAabbValid;

protected:

	btConvexInternalAabbCachingShape();

	void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax)
	{
		m_isLocalAabbValid = true;
		m_localAabbMin = aabbMin;
		m_localAabbMax = aabbMax;
	}

	inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const
	{
		btAssert(m_isLocalAabbValid);
		aabbMin = m_localAabbMin;
		aabbMax = m_localAabbMax;
	}

	inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const
	{

		//lazy evaluation of local aabb
		btAssert(m_isLocalAabbValid);
		btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax);
	}

public:

	virtual void	setLocalScaling(const btVector3& scaling);

	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;

	void	recalcLocalAabb();

};

#endif //BT_CONVEX_INTERNAL_SHAPE_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_OBB_BOX_MINKOWSKI_H
#define BT_OBB_BOX_MINKOWSKI_H

#include "btPolyhedralConvexShape.h"
#include "btCollisionMargin.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btMinMax.h"

/// The btBoxShape is a box primitive around the origin, its sides axis aligned
/// with length specified by half extents, in local shape coordinates. When used
/// as part of a btCollisionObject or btRigidBody it will be an oriented box in
/// world space.
ATTRIBUTE_ALIGNED16(class) btBoxShape: public btPolyhedralConvexShape
{
public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	btVector3 getHalfExtentsWithMargin() const
	{
		btVector3 halfExtents = getHalfExtentsWithoutMargin();
		btVector3 margin(getMargin(),getMargin(),getMargin());
		halfExtents += margin;
		return halfExtents;
	}

	/// scaling is included, margin is not
	const btVector3& getHalfExtentsWithoutMargin() const { return m_implicitShapeDimensions; }

	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const
	{
		btVector3 halfExtents = getHalfExtentsWithoutMargin();
		btVector3 margin(getMargin(),getMargin(),getMargin());
		halfExtents += margin;

		return btVector3(
			btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
			btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
			btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
	}

	SIMD_FORCE_INLINE  btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
	{
		const btVector3& halfExtents = getHalfExtentsWithoutMargin();

		return btVector3(
			btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
			btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
			btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
	}

	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
	{
		const btVector3& halfExtents = getHalfExtentsWithoutMargin();

		for (int i = 0; i < numVectors; i++)
		{
			const btVector3& vec = vectors[i];
			supportVerticesOut[i].setValue(
				btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
				btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
				btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
		}
	}

	btBoxShape(const btVector3& boxHalfExtents);

	virtual void setMargin(btScalar collisionMargin)
	{
		//correct the m_implicitShapeDimensions for the margin
		btVector3 oldMargin(getMargin(), getMargin(), getMargin());
		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;

		btConvexInternalShape::setMargin(collisionMargin);
		btVector3 newMargin(getMargin(), getMargin(), getMargin());
		m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
	}

	virtual void setLocalScaling(const btVector3& scaling)
	{
		btVector3 oldMargin(getMargin(), getMargin(), getMargin());
		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
		btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;

		btConvexInternalShape::setLocalScaling(scaling);

		m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
	}

	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;

	virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const
	{
		//this plane might not be aligned...
		btVector4 plane ;
		getPlaneEquation(plane,i);
		planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ());
		planeSupport = localGetSupportingVertex(-planeNormal);
	}

	virtual int getNumPlanes() const { return 6; }
	virtual int	getNumVertices() const { return 8; }
	virtual int getNumEdges() const { return 12; }

	virtual void getVertex(int i,btVector3& vtx) const
	{
		btVector3 halfExtents = getHalfExtentsWithMargin();

		vtx = btVector3(
				halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1),
				halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1),
				halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2));
	}

	virtual void	getPlaneEquation(btVector4& plane,int i) const
	{
		btVector3 halfExtents = getHalfExtentsWithoutMargin();

		switch (i)
		{
			case 0: plane.setValue(btScalar(1.), btScalar(0.), btScalar(0.), -halfExtents.x()); break;
			case 1: plane.setValue(btScalar(-1.), btScalar(0.), btScalar(0.), -halfExtents.x()); break;
			case 2: plane.setValue(btScalar(0.), btScalar(1.), btScalar(0.), -halfExtents.y()); break;
			case 3: plane.setValue(btScalar(0.), btScalar(-1.), btScalar(0.), -halfExtents.y()); break;
			case 4: plane.setValue(btScalar(0.), btScalar(0.), btScalar(1.), -halfExtents.z()); break;
			case 5: plane.setValue(btScalar(0.), btScalar(0.), btScalar(-1.), -halfExtents.z()); break;
			default: btAssert(0);
		}
	}

	virtual void getEdge(int i,btVector3& pa,btVector3& pb) const
	{
		int edgeVert0 = 0;
		int edgeVert1 = 0;

		switch (i)
		{
		case 0:  edgeVert0 = 0; edgeVert1 = 1; break;
		case 1:  edgeVert0 = 0; edgeVert1 = 2; break;
		case 2:  edgeVert0 = 1; edgeVert1 = 3; break;
		case 3:  edgeVert0 = 2; edgeVert1 = 3; break;
		case 4:  edgeVert0 = 0; edgeVert1 = 4; break;
		case 5:  edgeVert0 = 1; edgeVert1 = 5; break;
		case 6:  edgeVert0 = 2; edgeVert1 = 6; break;
		case 7:  edgeVert0 = 3; edgeVert1 = 7; break;
		case 8:  edgeVert0 = 4; edgeVert1 = 5; break;
		case 9:  edgeVert0 = 4; edgeVert1 = 6; break;
		case 10: edgeVert0 = 5; edgeVert1 = 7; break;
		case 11: edgeVert0 = 6; edgeVert1 = 7; break;
		default: btAssert(0);
		}

		getVertex(edgeVert0, pa);
		getVertex(edgeVert1, pb);
	}

	virtual bool isInside(const btVector3& pt, btScalar tolerance) const
	{
		btVector3 halfExtents = getHalfExtentsWithoutMargin();

		bool result = (pt.x() <= (halfExtents.x() + tolerance)) &&
		              (pt.x() >= (-halfExtents.x() - tolerance)) &&
		              (pt.y() <= (halfExtents.y() + tolerance)) &&
		              (pt.y() >= (-halfExtents.y() - tolerance)) &&
		              (pt.z() <= (halfExtents.z() + tolerance)) &&
		              (pt.z() >= (-halfExtents.z() - tolerance));

		return result;
	}

	/// debugging
	virtual const char* getName() const { return "Box"; }

	virtual int getNumPreferredPenetrationDirections() const { return 6; }

	virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
	{
		switch (index)
		{
		case 0: penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); break;
		case 1: penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); break;
		case 2: penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); break;
		case 3: penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); break;
		case 4: penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); break;
		case 5: penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); break;
		default: btAssert(0);
		}
	}
};

#endif //BT_OBB_BOX_MINKOWSKI_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_CONVEX_POINT_CLOUD_SHAPE_H
#define BT_CONVEX_POINT_CLOUD_SHAPE_H

#include "btPolyhedralConvexShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
#include "LinearMath/btAlignedObjectArray.h"

///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices.
ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape
{
	btVector3* m_unscaledPoints;
	int m_numPoints;

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	btConvexPointCloudShape()
	{
		m_localScaling.setValue(1.f,1.f,1.f);
		m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
		m_unscaledPoints = 0;
		m_numPoints = 0;
	}

	btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true)
	{
		m_localScaling = localScaling;
		m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
		m_unscaledPoints = points;
		m_numPoints = numPoints;

		if (computeAabb)
			recalcLocalAabb();
	}

	void setPoints (btVector3* points, int numPoints, bool computeAabb = true,const btVector3& localScaling=btVector3(1.f,1.f,1.f))
	{
		m_unscaledPoints = points;
		m_numPoints = numPoints;
		m_localScaling = localScaling;

		if (computeAabb)
			recalcLocalAabb();
	}

	SIMD_FORCE_INLINE	btVector3* getUnscaledPoints()
	{
		return m_unscaledPoints;
	}

	SIMD_FORCE_INLINE	const btVector3* getUnscaledPoints() const
	{
		return m_unscaledPoints;
	}

	SIMD_FORCE_INLINE	int getNumPoints() const
	{
		return m_numPoints;
	}

	SIMD_FORCE_INLINE	btVector3	getScaledPoint( int index) const
	{
		return m_unscaledPoints[index] * m_localScaling;
	}

#ifndef __SPU__
	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
#endif

	//debugging
	virtual const char*	getName()const {return "ConvexPointCloud";}

	virtual int	getNumVertices() const;
	virtual int getNumEdges() const;
	virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
	virtual void getVertex(int i,btVector3& vtx) const;
	virtual int	getNumPlanes() const;
	virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const;
	virtual	bool isInside(const btVector3& pt,btScalar tolerance) const;

	///in case we receive negative scaling
	virtual void	setLocalScaling(const btVector3& scaling);
};

#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_BVH_TRIANGLE_MESH_SHAPE_H
#define BT_BVH_TRIANGLE_MESH_SHAPE_H

#include "btTriangleMeshShape.h"
#include "btOptimizedBvh.h"
#include "LinearMath/btAlignedAllocator.h"
#include "btTriangleInfoMap.h"

///The btBvhTriangleMeshShape is a static-triangle mesh shape, it can only be used for fixed/non-moving objects.
///If you required moving concave triangle meshes, it is recommended to perform convex decomposition
///using HACD, see Bullet/Demos/ConvexDecompositionDemo.
///Alternatively, you can use btGimpactMeshShape for moving concave triangle meshes.
///btBvhTriangleMeshShape has several optimizations, such as bounding volume hierarchy and
///cache friendly traversal for PlayStation 3 Cell SPU.
///It is recommended to enable useQuantizedAabbCompression for better memory usage.
///It takes a triangle mesh as input, for example a btTriangleMesh or btTriangleIndexVertexArray. The btBvhTriangleMeshShape class allows for triangle mesh deformations by a refit or partialRefit method.
///Instead of building the bounding volume hierarchy acceleration structure, it is also possible to serialize (save) and deserialize (load) the structure from disk.
///See Demos\ConcaveDemo\ConcavePhysicsDemo.cpp for an example.
ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape
{

	btOptimizedBvh*	m_bvh;
	btTriangleInfoMap*	m_triangleInfoMap;

	bool m_useQuantizedAabbCompression;
	bool m_ownsBvh;
#ifdef __clang__
	bool m_pad[11] __attribute__((unused));////need padding due to alignment
#else
	bool m_pad[11];////need padding due to alignment
#endif

public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true);

	///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb
	btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true);

	virtual ~btBvhTriangleMeshShape();

	bool getOwnsBvh () const
	{
		return m_ownsBvh;
	}

	void performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget);
	void performConvexcast (btTriangleCallback* callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax);

	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;

	void	refitTree(const btVector3& aabbMin,const btVector3& aabbMax);

	///for a fast incremental refit of parts of the tree. Note: the entire AABB of the tree will become more conservative, it never shrinks
	void	partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax);

	//debugging
	virtual const char*	getName()const {return "BVHTRIANGLEMESH";}

	virtual void	setLocalScaling(const btVector3& scaling);

	btOptimizedBvh*	getOptimizedBvh()
	{
		return m_bvh;
	}

	void	setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1));

	void    buildOptimizedBvh();

	bool	usesQuantizedAabbCompression() const
	{
		return	m_useQuantizedAabbCompression;
	}

	void	setTriangleInfoMap(btTriangleInfoMap* triangleInfoMap)
	{
		m_triangleInfoMap = triangleInfoMap;
	}

	const btTriangleInfoMap*	getTriangleInfoMap() const
	{
		return m_triangleInfoMap;
	}

	btTriangleInfoMap*	getTriangleInfoMap()
	{
		return m_triangleInfoMap;
	}

	virtual	int	calculateSerializeBufferSize() const;

	///fills the dataBuffer and returns the struct name (and 0 on failure)
	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;

	virtual void	serializeSingleBvh(btSerializer* serializer) const;

	virtual void	serializeSingleTriangleInfoMap(btSerializer* serializer) const;

};

///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct	btTriangleMeshShapeData
{
	btCollisionShapeData	m_collisionShapeData;

	btStridingMeshInterfaceData m_meshInterface;

	btQuantizedBvhFloatData		*m_quantizedFloatBvh;
	btQuantizedBvhDoubleData	*m_quantizedDoubleBvh;

	btTriangleInfoMapData	*m_triangleInfoMap;

	float	m_collisionMargin;

	char m_pad3[4];

};

SIMD_FORCE_INLINE	int	btBvhTriangleMeshShape::calculateSerializeBufferSize() const
{
	return sizeof(btTriangleMeshShapeData);
}

#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

///This file was created by Alex Silverman

#ifndef BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H
#define BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H

#include "btTriangleIndexVertexArray.h"

ATTRIBUTE_ALIGNED16( struct)	btMaterialProperties
{
    ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution
    int m_numMaterials;
    const unsigned char * m_materialBase;
    int m_materialStride;
    PHY_ScalarType m_materialType;
    ///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're
    ///                           padding the structure, it can be reproduced at no real cost
    ///m_triangleMaterials =====> 1 integer value makes up one entry
    ///                           eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5
    int m_numTriangles;
    const unsigned char * m_triangleMaterialsBase;
    int m_triangleMaterialStride;
    ///m_triangleType <========== Automatically set in addMaterialProperties
    PHY_ScalarType m_triangleType;
};

typedef btAlignedObjectArray<btMaterialProperties>	MaterialArray;

///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray
///The addition of a material array allows for the utilization of the partID and
///triangleIndex that are returned in the ContactAddedCallback.  As with
///TriangleIndexVertexArray, no duplicate is made of the material data, so it
///is the users responsibility to maintain the array during the lifetime of the
///TriangleIndexVertexMaterialArray.
ATTRIBUTE_ALIGNED16(class) btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray
{
protected:
    MaterialArray       m_materials;

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

    btTriangleIndexVertexMaterialArray()
	{
	}

    btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,
        int numVertices,btScalar* vertexBase,int vertexStride,
        int numMaterials, unsigned char* materialBase, int materialStride,
        int* triangleMaterialsBase, int materialIndexStride);

    virtual ~btTriangleIndexVertexMaterialArray() {}

    void	addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER)
    {
        m_materials.push_back(mat);
        m_materials[m_materials.size()-1].m_triangleType = triangleType;
    }

    virtual void getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
        unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType ,int subpart = 0);

    virtual void getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
        const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0);

}
;

#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_MULTI_SPHERE_MINKOWSKI_H
#define BT_MULTI_SPHERE_MINKOWSKI_H

#include "btConvexInternalShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btAabbUtil2.h"

///The btMultiSphereShape represents the convex hull of a collection of spheres. You can create special capsules or other smooth volumes.
///It is possible to animate the spheres for deformation, but call 'recalcLocalAabb' after changing any sphere position/radius
ATTRIBUTE_ALIGNED16(class) btMultiSphereShape : public btConvexInternalAabbCachingShape
{

	btAlignedObjectArray<btVector3> m_localPositionArray;
	btAlignedObjectArray<btScalar>  m_radiArray;

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres);

	///CollisionShape Interface
	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;

	/// btConvexShape Interface
	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;

	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;

	int	getSphereCount() const
	{
		return m_localPositionArray.size();
	}

	const btVector3&	getSpherePosition(int index) const
	{
		return m_localPositionArray[index];
	}

	btScalar	getSphereRadius(int index) const
	{
		return m_radiArray[index];
	}

	virtual const char*	getName()const
	{
		return "MultiSphere";
	}

	virtual	int	calculateSerializeBufferSize() const;

	///fills the dataBuffer and returns the struct name (and 0 on failure)
	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;

};

struct	btPositionAndRadius
{
	btVector3FloatData	m_pos;
	float		m_radius;
};

struct	btMultiSphereShapeData
{
	btConvexInternalShapeData	m_convexInternalShapeData;

	btPositionAndRadius	*m_localPositionArrayPtr;
	int				m_localPositionArraySize;
	char	m_padding[4];
};

SIMD_FORCE_INLINE	int	btMultiSphereShape::calculateSerializeBufferSize() const
{
	return sizeof(btMultiSphereShapeData);
}

#endif //BT_MULTI_SPHERE_MINKOWSKI_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_CONVEX_HULL_SHAPE_H
#define BT_CONVEX_HULL_SHAPE_H

#include "btPolyhedralConvexShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "LinearMath/btAlignedObjectArray.h"

/// The btConvexHullShape implements an implicit convex hull of an array of
/// vertices. Bullet provides a general and fast collision detector for convex
/// shapes based on GJK and EPA using localGetSupportingVertex.
ATTRIBUTE_ALIGNED16(class)
btConvexHullShape : public btPolyhedralConvexAabbCachingShape
{
	btAlignedObjectArray<btVector3> m_unscaledPoints;

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	/// this constructor optionally takes in a pointer to points. Each point is
	/// assumed to be 3 consecutive btScalar (x,y,z), the striding defines the
	/// number of bytes between each point, in memory. It is easier to not pass
	/// any points in the constructor, and just add one point at a time, using
	/// addPoint. btConvexHullShape make an internal copy of the points.
	btConvexHullShape(const btScalar* points = 0, int numPoints = 0, int stride = sizeof(btVector3));

	void addPoint(const btVector3& point, bool recalculateLocalAabb = true);

	btVector3*       getUnscaledPoints() { return &m_unscaledPoints[0]; }
	const btVector3* getUnscaledPoints() const { return &m_unscaledPoints[0]; }

	/// getPoints is obsolete, please use getUnscaledPoints
	const btVector3* getPoints() const { return getUnscaledPoints(); }

	void optimizeConvexHull();

	SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const { return m_unscaledPoints[i] * m_localScaling; }
	SIMD_FORCE_INLINE int       getNumPoints() const { return m_unscaledPoints.size(); }

	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;

	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(
	    const btVector3* vectors,
	    btVector3*       supportVerticesOut,
	    int              numVectors) const;

	virtual void project(
	    const btTransform& trans,
	    const btVector3&   dir,
	    btScalar&          minProj,
	    btScalar&          maxProj,
	    btVector3&         witnesPtMin,
	    btVector3&         witnesPtMax) const;

	//debugging
	virtual const char* getName() const { return "Convex"; }

	virtual int  getNumVertices() const;
	virtual int  getNumEdges() const;
	virtual void getEdge(int i, btVector3& pa, btVector3& pb) const;
	virtual void getVertex(int i, btVector3& vtx) const;
	virtual int  getNumPlanes() const;
	virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const;
	virtual bool isInside(const btVector3& pt, btScalar tolerance) const;

	/// in case we receive negative scaling
	virtual void setLocalScaling(const btVector3& scaling);

	virtual int calculateSerializeBufferSize() const;

	/// fills the dataBuffer and returns the struct name (and 0 on failure)
	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};

/// do not change those serialization structures, it requires an updated
/// sBulletDNAstr/sBulletDNAstr64
struct btConvexHullShapeData
{
	btConvexInternalShapeData m_convexInternalShapeData;

	btVector3FloatData*  m_unscaledPointsFloatPtr;
	btVector3DoubleData* m_unscaledPointsDoublePtr;

	int  m_numUnscaledPoints;
	char m_padding3[4];
};

SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const
{
	return sizeof(btConvexHullShapeData);
}

#endif //BT_CONVEX_HULL_SHAPE_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_OBB_TRIANGLE_MINKOWSKI_H
#define BT_OBB_TRIANGLE_MINKOWSKI_H

#include "btConvexShape.h"
#include "btBoxShape.h"

ATTRIBUTE_ALIGNED16(class) btTriangleShape : public btPolyhedralConvexShape
{

public:

BT_DECLARE_ALIGNED_ALLOCATOR();

	btVector3	m_vertices1[3];

	virtual int getNumVertices() const
	{
		return 3;
	}

	btVector3& getVertexPtr(int index)
	{
		return m_vertices1[index];
	}

	const btVector3& getVertexPtr(int index) const
	{
		return m_vertices1[index];
	}
	virtual void getVertex(int index,btVector3& vert) const
	{
		vert = m_vertices1[index];
	}

	virtual int getNumEdges() const
	{
		return 3;
	}

	virtual void getEdge(int i,btVector3& pa,btVector3& pb) const
	{
		getVertex(i,pa);
		getVertex((i+1)%3,pb);
	}

	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const
	{
//		btAssert(0);
		getAabbSlow(t,aabbMin,aabbMax);
	}

	btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir)const
	{
        btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]);
	  	return m_vertices1[dots.maxAxis()];

	}

	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
	{
		for (int i=0;i<numVectors;i++)
		{
			const btVector3& dir = vectors[i];
            btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]);
  			supportVerticesOut[i] = m_vertices1[dots.maxAxis()];
		}

	}

	btTriangleShape() : btPolyhedralConvexShape ()
    {
		m_shapeType = TRIANGLE_SHAPE_PROXYTYPE;
	}

	btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) : btPolyhedralConvexShape ()
    {
		m_shapeType = TRIANGLE_SHAPE_PROXYTYPE;
        m_vertices1[0] = p0;
        m_vertices1[1] = p1;
        m_vertices1[2] = p2;
    }

	virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const
	{
		getPlaneEquation(i,planeNormal,planeSupport);
	}

	virtual int	getNumPlanes() const
	{
		return 1;
	}

	void calcNormal(btVector3& normal) const
	{
		normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]);
		normal.normalize();
	}

	virtual void getPlaneEquation(int i, btVector3& planeNormal,btVector3& planeSupport) const
	{
		(void)i;
		calcNormal(planeNormal);
		planeSupport = m_vertices1[0];
	}

	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const
	{
		(void)mass;
		btAssert(0);
		inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
	}

		virtual	bool isInside(const btVector3& pt,btScalar tolerance) const
	{
		btVector3 normal;
		calcNormal(normal);
		//distance to plane
		btScalar dist = pt.dot(normal);
		btScalar planeconst = m_vertices1[0].dot(normal);
		dist -= planeconst;
		if (dist >= -tolerance && dist <= tolerance)
		{
			//inside check on edge-planes
			int i;
			for (i=0;i<3;i++)
			{
				btVector3 pa,pb;
				getEdge(i,pa,pb);
				btVector3 edge = pb-pa;
				btVector3 edgeNormal = edge.cross(normal);
				edgeNormal.normalize();
				btScalar dist = pt.dot( edgeNormal);
				btScalar edgeConst = pa.dot(edgeNormal);
				dist -= edgeConst;
				if (dist < -tolerance)
					return false;
			}

			return true;
		}

		return false;
	}
		//debugging
		virtual const char*	getName()const
		{
			return "Triangle";
		}

		virtual int		getNumPreferredPenetrationDirections() const
		{
			return 2;
		}

		virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
		{
			calcNormal(penetrationVector);
			if (index)
				penetrationVector *= btScalar(-1.);
		}

};

#endif //BT_OBB_TRIANGLE_MINKOWSKI_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_COLLISION_MARGIN_H
#define BT_COLLISION_MARGIN_H

///The CONVEX_DISTANCE_MARGIN is a default collision margin for convex collision shapes derived from btConvexInternalShape.
///This collision margin is used by Gjk and some other algorithms
///Note that when creating small objects, you need to make sure to set a smaller collision margin, using the 'setMargin' API
#define CONVEX_DISTANCE_MARGIN btScalar(0.04)// btScalar(0.1)//;//btScalar(0.01)

#endif //BT_COLLISION_MARGIN_H