path: root/tests/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp

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

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

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

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

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

namespace gjkepa2_impl
{

	// Config

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

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


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

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

		MinkowskiDiff()
		{

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

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

	typedef	MinkowskiDiff	tShape;


	// GJK
	struct	GJK
	{
		/* Types		*/ 
		struct	sSV
		{
			btVector3	d,w;
		};
		struct	sSimplex
		{
			sSV*		c[4];
			btScalar	p[4];
			U			rank;
		};
		struct	eStatus	{ enum _ {
			Valid,
			Inside,
			Failed		};};
			/* Fields		*/ 
			tShape			m_shape;
			btVector3		m_ray;
			btScalar		m_distance;
			sSimplex		m_simplices[2];
			sSV				m_store[4];
			sSV*			m_free[4];
			U				m_nfree;
			U				m_current;
			sSimplex*		m_simplex;
			eStatus::_		m_status;
			/* Methods		*/ 
			GJK()
			{
				Initialize();
			}
			void				Initialize()
			{
				m_ray		=	btVector3(0,0,0);
				m_nfree		=	0;
				m_status	=	eStatus::Failed;
				m_current	=	0;
				m_distance	=	0;
			}
			eStatus::_			Evaluate(const tShape& shapearg,const btVector3& guess)
			{
				U			iterations=0;
				btScalar	sqdist=0;
				btScalar	alpha=0;
				btVector3	lastw[4];
				U			clastw=0;
				/* Initialize solver		*/ 
				m_free[0]			=	&m_store[0];
				m_free[1]			=	&m_store[1];
				m_free[2]			=	&m_store[2];
				m_free