Added faster and more robust support for btStaticPlaneShape

7 files changed, 220 insertions, 10 deletions

diff --git a/Demos/BasicDemo/BasicDemo.cpp b/Demos/BasicDemo/BasicDemo.cpp
index fcf8b5ff7..eb857f186 100644
--- a/Demos/BasicDemo/BasicDemo.cpp
+++ b/Demos/BasicDemo/BasicDemo.cpp
@@ -96,15 +96,16 @@ void	BasicDemo::initPhysics()
 	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)

 	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;

 	m_solver = sol;

-	

+

 

 	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver,m_collisionConfiguration);

 

 	m_dynamicsWorld->setGravity(btVector3(0,-10,0));

 

 	///create a few basic rigid bodies

-	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));

-

+//	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));

+	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);

+	

 	m_collisionShapes.push_back(groundShape);

 

 	btTransform groundTransform;

@@ -135,8 +136,8 @@ void	BasicDemo::initPhysics()
 		//create a few dynamic rigidbodies

 		// Re-using the same collision is better for memory usage and performance

 

-		//btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));

-		btCollisionShape* colShape = new btSphereShape(btScalar(1.));

+		btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));

+		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));

 		m_collisionShapes.push_back(colShape);

 

 		/// Create Dynamic Objects

diff --git a/Demos/BasicDemo/main.cpp b/Demos/BasicDemo/main.cpp
index 09c575885..f85e7dfb6 100644
--- a/Demos/BasicDemo/main.cpp
+++ b/Demos/BasicDemo/main.cpp
@@ -15,14 +15,17 @@ subject to the following restrictions:
 
 #include "BasicDemo.h"
 #include "GlutStuff.h"
+#include "GLDebugDrawer.h"
+#include "btBulletDynamicsCommon.h"
 
-//#define CHECK_MEMORY_LEAKS 1
 
 int main(int argc,char** argv)
 {
+	GLDebugDrawer	gDebugDrawer;
 
 	BasicDemo ccdDemo;
 	ccdDemo.initPhysics();
+	ccdDemo.getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
 
 
 #ifdef CHECK_MEMORY_LEAKS
diff --git a/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
new file mode 100644
index 000000000..9b39433dd
--- /dev/null
+++ b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
@@ -0,0 +1,109 @@
+/*

+Bullet Continuous Collision Detection and Physics Library

+Copyright (c) 2003-2006 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.

+*/

+

+#include "btConvexPlaneCollisionAlgorithm.h"

+

+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"

+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"

+#include "BulletCollision/CollisionShapes/btConvexShape.h"

+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"

+

+//#include <stdio.h>

+

+btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)

+: btCollisionAlgorithm(ci),

+m_ownManifold(false),

+m_manifoldPtr(mf),

+m_isSwapped(isSwapped)

+{

+	btCollisionObject* convexObj = m_isSwapped? col1 : col0;

+	btCollisionObject* planeObj = m_isSwapped? col0 : col1;

+	

+	if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj))

+	{

+		m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj);

+		m_ownManifold = true;

+	}

+}

+

+

+btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()

+{

+	if (m_ownManifold)

+	{

+		if (m_manifoldPtr)

+			m_dispatcher->releaseManifold(m_manifoldPtr);

+	}

+}

+

+

+

+void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)

+{

+	(void)dispatchInfo;

+	(void)resultOut;

+	if (!m_manifoldPtr)

+		return;

+

+	btCollisionObject* convexObj = m_isSwapped? body1 : body0;

+	btCollisionObject* planeObj = m_isSwapped? body0: body1;

+

+	btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape();

+	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();

+

+	bool hasCollision = false;

+	const btVector3& planeNormal = planeShape->getPlaneNormal();

+	const btScalar& planeConstant = planeShape->getPlaneConstant();

+	btTransform planeInConvex;

+	planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform();

+	btTransform convexInPlaneTrans;

+	convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform();

+	//btVector3 vtx = convexShape->localGetSupportingVertexWithoutMargin(planeNormal);

+	btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);

+	btVector3 vtxInPlane = convexInPlaneTrans(vtx);

+	//btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant) - convexShape->getMargin();

+	btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);

+

+	btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;

+	btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected;

+

+	hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();

+	resultOut->setPersistentManifold(m_manifoldPtr);

+	if (hasCollision)

+	{

+		/// report a contact. internally this will be kept persistent, and contact reduction is done

+		btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal;

+		btVector3 pOnB = vtxInPlaneWorld;

+		resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);

+	}

+	if (m_ownManifold)

+	{

+		if (m_manifoldPtr->getNumContacts())

+		{

+			resultOut->refreshContactPoints();

+		}

+	}

+}

+

+btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)

+{

+	(void)resultOut;

+	(void)dispatchInfo;

+	(void)col0;

+	(void)col1;

+

+	//not yet

+	return btScalar(1.);

+}

diff --git a/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
new file mode 100644
index 000000000..b503ba40c
--- /dev/null
+++ b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
@@ -0,0 +1,63 @@
+/*

+Bullet Continuous Collision Detection and Physics Library

+Copyright (c) 2003-2006 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.

+*/

+

+#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H

+#define CONVEX_PLANE_COLLISION_ALGORITHM_H

+

+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"

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

+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"

+class btPersistentManifold;

+#include "btCollisionDispatcher.h"

+

+#include "LinearMath/btVector3.h"

+

+/// btSphereBoxCollisionAlgorithm  provides sphere-box collision detection.

+/// Other features are frame-coherency (persistent data) and collision response.

+class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm

+{

+	bool	m_ownManifold;

+	btPersistentManifold*	m_manifoldPtr;

+	bool	m_isSwapped;

+	

+public:

+

+	btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);

+

+	virtual ~btConvexPlaneCollisionAlgorithm();

+

+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);

+

+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);

+

+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc

+	{

+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)

+		{

+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm));

+			if (!m_swapped)

+			{

+				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false);

+			} else

+			{

+				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true);

+			}

+		}

+	};

+

+};

+

+#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H

+

diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
index 661270b9b..83286c37a 100644
--- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
+++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
@@ -20,6 +20,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h"
+
 #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h"
@@ -58,7 +60,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc*	s
 	m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc;
 	mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16);
 	m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc;
-
+	
 	mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16);
 	m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc;
 	mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
@@ -72,7 +74,13 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc*	s
 	m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
 	m_triangleSphereCF->m_swapped = true;
 
-
+	//convex versus plane
+	mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
+	m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
+	m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
+	m_planeConvexCF->m_swapped = true;
+	
 	///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
 	int maxSize = sizeof(btConvexConvexAlgorithm);
 	int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
@@ -167,6 +175,11 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
 	m_triangleSphereCF->~btCollisionAlgorithmCreateFunc();
 	btAlignedFree( m_triangleSphereCF);
 
+	m_convexPlaneCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_convexPlaneCF);
+	m_planeConvexCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_planeConvexCF);
+
 	m_simplexSolver->~btVoronoiSimplexSolver();
 	btAlignedFree(m_simplexSolver);
 	m_pdSolver->~btGjkEpaPenetrationDepthSolver();
@@ -178,7 +191,8 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
 
 btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
 {
-	
+
+
 
 	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
 	{
@@ -205,6 +219,15 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
 		return	m_triangleSphereCF;
 	}
 	
+	if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE))
+	{
+		return m_convexPlaneCF;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE))
+	{
+		return m_planeConvexCF;
+	}
 
 	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
 	{
diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
index 2e99f1db1..14475ad14 100644
--- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
+++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
@@ -54,7 +54,9 @@ class	btDefaultCollisionConfiguration : public btCollisionConfiguration
 	btCollisionAlgorithmCreateFunc* m_boxSphereCF;
 	btCollisionAlgorithmCreateFunc*	m_sphereTriangleCF;
 	btCollisionAlgorithmCreateFunc*	m_triangleSphereCF;
-
+	btCollisionAlgorithmCreateFunc*	m_planeConvexCF;
+	btCollisionAlgorithmCreateFunc*	m_convexPlaneCF;
+	
 public:
 
 	btDefaultCollisionConfiguration(btStackAlloc*	stackAlloc=0,btPoolAllocator*	persistentManifoldPool=0,btPoolAllocator*	collisionAlgorithmPool=0);
diff --git a/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h
index 0cbce3abd..42b8cf113 100644
--- a/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h
+++ b/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h
@@ -51,6 +51,15 @@ public:
 	virtual void	setLocalScaling(const btVector3& scaling);
 	virtual const btVector3& getLocalScaling() const;
 	
+	const btVector3&	getPlaneNormal() const
+	{
+		return	m_planeNormal;
+	}
+
+	const btScalar&	getPlaneConstant() const
+	{
+		return	m_planeConstant;
+	}
 
 	//debugging
 	virtual const char*	getName()const {return "STATICPLANE";}