some work on compound collision shapes (not finished yet)

Bullet/CollisionDispatch/CollisionDispatcher.cpp

@@ -21,6 +21,7 @@ subject to the following restrictions:
 #include "CollisionDispatch/ConvexConvexAlgorithm.h"
 #include "CollisionDispatch/EmptyCollisionAlgorithm.h"
 #include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h"
+#include "CollisionDispatch/CompoundCollisionAlgorithm.h"
 #include "CollisionShapes/CollisionShape.h"
 #include "CollisionDispatch/CollisionObject.h"
 #include <algorithm>
@@ -122,6 +123,17 @@ CollisionAlgorithm* CollisionDispatcher::InternalFindAlgorithm(BroadphaseProxy&
 		return new ConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0);
 	}
 
+	if (body0->m_collisionShape->IsCompound())
+	{
+		return new CompoundCollisionAlgorithm(ci,&proxy0,&proxy1);
+	} else
+	{
+		if (body1->m_collisionShape->IsCompound())
+		{
+			return new CompoundCollisionAlgorithm(ci,&proxy1,&proxy0);
+		}
+	}
+
 	//failed to find an algorithm
 	return new EmptyAlgorithm(ci);
 	

Bullet/CollisionDispatch/CollisionWorld.cpp

@@ -20,6 +20,7 @@ subject to the following restrictions:
 #include "CollisionShapes/SphereShape.h" //for raycasting
 #include "CollisionShapes/TriangleMeshShape.h" //for raycasting
 #include "NarrowPhaseCollision/RaycastCallback.h"
+#include "CollisionShapes/CompoundShape.h"
 
 #include "NarrowPhaseCollision/SubSimplexConvexCast.h"
 #include "BroadphaseCollision/BroadphaseInterface.h"
@@ -139,58 +140,27 @@ void	CollisionWorld::RemoveCollisionObject(CollisionObject* collisionObject)
 		}
 }
 
-
+void	RayTestSingle(const SimdTransform& rayFromTrans,const SimdTransform& rayToTrans,
-
+					  CollisionObject* collisionObject,
-void	CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback)
+					  const CollisionShape* collisionShape,
+					  const SimdTransform& colObjWorldTransform,
+					  CollisionWorld::RayResultCallback& resultCallback)
 {
-
 	
-	SimdTransform	rayFromTrans,rayToTrans;
-	rayFromTrans.setIdentity();
-	rayFromTrans.setOrigin(rayFromWorld);
-	rayToTrans.setIdentity();
-	
-	rayToTrans.setOrigin(rayToWorld);
-
-	//do culling based on aabb (rayFrom/rayTo)
-	SimdVector3 rayAabbMin = rayFromWorld;
-	SimdVector3 rayAabbMax = rayFromWorld;
-	rayAabbMin.setMin(rayToWorld);
-	rayAabbMax.setMax(rayToWorld);
-
 	SphereShape pointShape(0.0f);
 
-	/// brute force go over all objects. Once there is a broadphase, use that, or
+	if (collisionShape->IsConvex())
-	/// add a raycast against aabb first.
-	
-	std::vector<CollisionObject*>::iterator iter;
-	
-	for (iter=m_collisionObjects.begin();
-	!(iter==m_collisionObjects.end()); iter++)
-	{
-		
-		CollisionObject*	collisionObject= (*iter);
-
-		//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
-		SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax;
-		collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);
-
-		//check aabb overlap
-
-		if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
-		{
-			if (collisionObject->m_collisionShape->IsConvex())
 			{
 				ConvexCast::CastResult castResult;
 				castResult.m_fraction = 1.f;//??
 
-				ConvexShape* convexShape = (ConvexShape*) collisionObject->m_collisionShape;
+				ConvexShape* convexShape = (ConvexShape*) collisionShape;
 				VoronoiSimplexSolver	simplexSolver;
 				SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
 				//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
 				//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
 				
-				if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,collisionObject->m_worldTransform,collisionObject->m_worldTransform,castResult))
+				if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
 				{
 					//add hit
 					if (castResult.m_normal.length2() > 0.0001f)
@@ -217,12 +187,12 @@ void	CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3&
 			else
 			{
 				
-				if (collisionObject->m_collisionShape->IsConcave())
+				if (collisionShape->IsConcave())
 					{
 
-						TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionObject->m_collisionShape;
+						TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionShape;
 						
-						SimdTransform worldTocollisionObject = collisionObject->m_worldTransform.inverse();
+						SimdTransform worldTocollisionObject = colObjWorldTransform.inverse();
 
 						SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
 						SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
@@ -231,12 +201,12 @@ void	CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3&
 
 						struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback 
 						{
-							RayResultCallback* m_resultCallback;
+							CollisionWorld::RayResultCallback* m_resultCallback;
 							CollisionObject*	m_collisionObject;
 							TriangleMeshShape*	m_triangleMesh;
 
 							BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to,
-								RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape*	triangleMesh):
+								CollisionWorld::RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape*	triangleMesh):
 								TriangleRaycastCallback(from,to),
 									m_resultCallback(resultCallback),
 									m_collisionObject(collisionObject),
@@ -275,10 +245,75 @@ void	CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3&
 
 						triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
 											
-					}
+					} else
-					
+					{
+						//todo: use AABB tree or other BVH acceleration structure!
+						if (collisionShape->IsCompound())
+						{
+							const CompoundShape* compoundShape = static_cast<const CompoundShape*>(collisionShape);
+							int i=0;
+							for (i=0;i<compoundShape->GetNumChildShapes();i++)
+							{
+								SimdTransform childTrans = compoundShape->GetChildTransform(i);
+								const CollisionShape* childCollisionShape = compoundShape->GetChildShape(i);
+								SimdTransform childWorldTrans = colObjWorldTransform * childTrans;
+								RayTestSingle(rayFromTrans,rayToTrans,
+									collisionObject,
+									childCollisionShape,
+									childWorldTrans,
+									resultCallback);
 
+							}
+
+
+						}
+					}
 			}
+}
+
+void	CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback)
+{
+
+	
+	SimdTransform	rayFromTrans,rayToTrans;
+	rayFromTrans.setIdentity();
+	rayFromTrans.setOrigin(rayFromWorld);
+	rayToTrans.setIdentity();
+	
+	rayToTrans.setOrigin(rayToWorld);
+
+	//do culling based on aabb (rayFrom/rayTo)
+	SimdVector3 rayAabbMin = rayFromWorld;
+	SimdVector3 rayAabbMax = rayFromWorld;
+	rayAabbMin.setMin(rayToWorld);
+	rayAabbMax.setMax(rayToWorld);
+
+
+	/// brute force go over all objects. Once there is a broadphase, use that, or
+	/// add a raycast against aabb first.
+	
+	std::vector<CollisionObject*>::iterator iter;
+	
+	for (iter=m_collisionObjects.begin();
+	!(iter==m_collisionObjects.end()); iter++)
+	{
+		
+		CollisionObject*	collisionObject= (*iter);
+
+		//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+		SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+		collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);
+
+		//check aabb overlap
+
+		if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
+		{
+			RayTestSingle(rayFromTrans,rayToTrans,
+				collisionObject,
+					 collisionObject->m_collisionShape,
+					  collisionObject->m_worldTransform,
+					  resultCallback);
+			
 		}
 	}
 

Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp

@@ -14,19 +14,70 @@ subject to the following restrictions:
 */
 
 #include "CollisionDispatch/CompoundCollisionAlgorithm.h"
+#include "CollisionDispatch/CollisionObject.h"
+#include "CollisionShapes/CompoundShape.h"
 
 
 CompoundCollisionAlgorithm::CompoundCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
+:m_dispatcher(ci.m_dispatcher),
+m_compoundProxy(*proxy0),
+m_otherProxy(*proxy1)
 {
+	CollisionObject* colObj = static_cast<CollisionObject*>(m_compoundProxy.m_clientObject);
+	assert (colObj->m_collisionShape->IsCompound());
+	
+	CompoundShape* compoundShape = static_cast<CompoundShape*>(colObj->m_collisionShape);
+	int numChildren = compoundShape->GetNumChildShapes();
+	m_childProxies.resize( numChildren );
+	int i;
+	for (i=0;i<numChildren;i++)
+	{
+		m_childProxies[i] = BroadphaseProxy(*proxy0);
+	}
+
+	m_childCollisionAlgorithms.resize(numChildren);
+	for (i=0;i<numChildren;i++)
+	{
+		CollisionShape* childShape = compoundShape->GetChildShape(i);
+		CollisionObject* colObj = static_cast<CollisionObject*>(m_childProxies[i].m_clientObject);
+		CollisionShape* orgShape = colObj->m_collisionShape;
+		colObj->m_collisionShape = childShape;
+		m_childCollisionAlgorithms[i] = m_dispatcher->FindAlgorithm(m_childProxies[i],m_otherProxy);
+		colObj->m_collisionShape =orgShape;
+	}
 }
 
+
 CompoundCollisionAlgorithm::~CompoundCollisionAlgorithm()
 {
+	int numChildren = m_childCollisionAlgorithms.size();
+	int i;
+	for (i=0;i<numChildren;i++)
+	{
+		delete m_childCollisionAlgorithms[i];
+	}
 }
 
-void CompoundCollisionAlgorithm::ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo)
+void CompoundCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo)
 {
+	CollisionObject* colObj = static_cast<CollisionObject*>(m_compoundProxy.m_clientObject);
+	assert (colObj->m_collisionShape->IsCompound());
 	
+	CompoundShape* compoundShape = static_cast<CompoundShape*>(colObj->m_collisionShape);
+
+	int numChildren = m_childCollisionAlgorithms.size();
+	int i;
+	for (i=0;i<numChildren;i++)
+	{
+		//temporarily exchange parent CollisionShape with childShape, and recurse
+		CollisionShape* childShape = compoundShape->GetChildShape(i);
+		CollisionObject* colObj = static_cast<CollisionObject*>(m_childProxies[i].m_clientObject);
+		CollisionShape* orgShape = colObj->m_collisionShape;
+		colObj->m_collisionShape = childShape;
+		m_childCollisionAlgorithms[i]->ProcessCollision(&m_childProxies[i],&m_otherProxy,dispatchInfo);
+		//revert back
+		colObj->m_collisionShape =orgShape;
+	}
 }
 
 float	CompoundCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo)

Bullet/CollisionDispatch/CompoundCollisionAlgorithm.h

@@ -23,14 +23,19 @@ subject to the following restrictions:
 #include "NarrowPhaseCollision/PersistentManifold.h"
 class Dispatcher;
 #include "BroadphaseCollision/BroadphaseProxy.h"
-
+#include <vector>
 
 
 /// CompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
 /// Place holder, not fully implemented yet
 class CompoundCollisionAlgorithm  : public CollisionAlgorithm
 {
+	BroadphaseProxy	m_compoundProxy;
+	BroadphaseProxy	m_otherProxy;
+	std::vector<BroadphaseProxy> m_childProxies;
+	std::vector<CollisionAlgorithm*> m_childCollisionAlgorithms;
 
+	Dispatcher*		m_dispatcher;
 	BroadphaseProxy m_compound;
 
 	BroadphaseProxy m_other;

Bullet/CollisionShapes/CompoundShape.cpp

@@ -20,6 +20,8 @@ subject to the following restrictions:
 
 
 CompoundShape::CompoundShape()
+:m_localAabbMin(1e30f,1e30f,1e30f),
+m_localAabbMax(-1e30f,-1e30f,-1e30f)
 {
 }
 
@@ -28,21 +30,66 @@ CompoundShape::~CompoundShape()
 {
 }
 
+void	CompoundShape::AddChildShape(const SimdTransform& localTransform,CollisionShape* shape)
+{
+	m_childTransforms.push_back(localTransform);
+	m_childShapes.push_back(shape);
+
+	//extend the local aabbMin/aabbMax
+	SimdVector3 localAabbMin,localAabbMax;
+	shape->GetAabb(localTransform,localAabbMin,localAabbMax);
+	for (int i=0;i<3;i++)
+	{
+		if (m_localAabbMin[i] > localAabbMin[i])
+		{
+			m_localAabbMin[i] = localAabbMin[i];
+		}
+		if (m_localAabbMax[i] < localAabbMax[i])
+		{
+			m_localAabbMax[i] = localAabbMax[i];
+		}
+
+	}
+}
+
+
 
 	///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
-void CompoundShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const
+void CompoundShape::GetAabb(const SimdTransform& trans,SimdVector3& aabbMin,SimdVector3& aabbMax) const
 {
-	SimdVector3 margin(GetMargin(),GetMargin(),GetMargin());
+	SimdVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin);
+	SimdVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin);
+	
+	SimdMatrix3x3 abs_b = trans.getBasis().absolute();  
 
-	aabbMin = t.getOrigin() - margin;
+	SimdPoint3 center = trans(localCenter);
 
-	aabbMax = t.getOrigin() + margin;
+	SimdVector3 extent = SimdVector3(abs_b[0].dot(localHalfExtents),
+		   abs_b[1].dot(localHalfExtents),
+		  abs_b[2].dot(localHalfExtents));
+	extent += SimdVector3(GetMargin(),GetMargin(),GetMargin());
 
+	aabbMin = center - extent;
+	aabbMax = center + extent;
 }
 
 void	CompoundShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
 {
-	assert(0);
+	//approximation: take the inertia from the aabb for now
+	SimdTransform ident;
+	SimdVector3 aabbMin,aabbMax;
+	GetAabb(ident,aabbMin,aabbMax);
+	
+	SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f;
+	
+	SimdScalar lx=2.f*(halfExtents.x());
+	SimdScalar ly=2.f*(halfExtents.y());
+	SimdScalar lz=2.f*(halfExtents.z());
+
+	inertia[0] = mass/(12.0f) * (ly*ly + lz*lz);
+	inertia[1] = mass/(12.0f) * (lx*lx + lz*lz);
+	inertia[2] = mass/(12.0f) * (lx*lx + ly*ly);
+
 }
 
 	

Bullet/CollisionShapes/CompoundShape.h

@@ -28,14 +28,43 @@ subject to the following restrictions:
 
 /// CompoundShape allows to store multiple other CollisionShapes
 /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape.
-/// Place holder, not fully implemented yet
 class CompoundShape	: public CollisionShape
 {
+	std::vector<SimdTransform>		m_childTransforms;
+	std::vector<CollisionShape*>	m_childShapes;
+	SimdVector3						m_localAabbMin;
+	SimdVector3						m_localAabbMax;
+
+
 public:
 	CompoundShape();
 
 	virtual ~CompoundShape();
 
+	void	AddChildShape(const SimdTransform& localTransform,CollisionShape* shape);
+
+	int		GetNumChildShapes() const
+	{
+		return m_childShapes.size();
+	}
+
+	CollisionShape* GetChildShape(int index)
+	{
+		return m_childShapes[index];
+	}
+	const CollisionShape* GetChildShape(int index) const
+	{
+		return m_childShapes[index];
+	}
+
+	SimdTransform	GetChildTransform(int index)
+	{
+		return m_childTransforms[index];
+	}
+	const SimdTransform	GetChildTransform(int index) const
+	{
+		return m_childTransforms[index];
+	}
 
 	///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
 	void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const;

Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp

@@ -26,7 +26,7 @@ subject to the following restrictions:
 #include "CollisionShapes/SphereShape.h"
 #include "CollisionShapes/ConeShape.h"
 #include "CollisionShapes/StaticPlaneShape.h"
-
+#include "CollisionShapes/CompoundShape.h"
 #include "CollisionShapes/Simplex1to4Shape.h"
 #include "CollisionShapes/EmptyShape.h"
 
@@ -126,7 +126,7 @@ CollisionShape* shapePtr[numShapes] =
 #else
 	new BoxShape (SimdVector3(450,10,450)),
 #endif
-
+		
 		new BoxShape (SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)),
 		new SphereShape (CUBE_HALF_EXTENTS- 0.05f),
 
@@ -205,8 +205,13 @@ int main(int argc,char** argv)
 			shapeIndex[i] = 0;
 	}
 
+	CompoundShape* compoundShape = new CompoundShape();
+	//shapePtr[1] = compoundShape;
 
+	SimdTransform ident;
+	ident.setIdentity();
 
+	compoundShape->AddChildShape(ident,new BoxShape (SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)));
 
 	for (i=0;i<numObjects;i++)
 	{
@@ -498,8 +503,17 @@ void renderme()
 		char	extraDebug[125];
 		sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId);
 		physObjects[i]->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug);
+
+		float vec[16];
+		SimdTransform ident;
+		ident.setIdentity();
+		ident.getOpenGLMatrix(vec);
+		glPushMatrix(); 
+	  glLoadMatrixf(vec);
+
 		GL_ShapeDrawer::DrawOpenGL(m,physObjects[i]->GetRigidBody()->GetCollisionShape(),wireColor,getDebugMode());
 
+		glPopMatrix(); 
 		///this block is just experimental code to show some internal issues with replacing shapes on the fly.
 		if (getDebugMode()!=0 && (i>0))
 		{

Demos/OpenGL/GL_ShapeDrawer.cpp

@@ -34,6 +34,7 @@ subject to the following restrictions:
 #include "CollisionShapes/ConeShape.h"
 #include "CollisionShapes/CylinderShape.h"
 #include "CollisionShapes/Simplex1to4Shape.h"
+#include "CollisionShapes/CompoundShape.h"
 
 #include "CollisionShapes/ConvexTriangleMeshShape.h"
 
@@ -105,225 +106,242 @@ public:
 
 void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const SimdVector3& color,int	debugMode)
 {
+
+	
 	glPushMatrix(); 
-    glLoadMatrixf(m);
+    glMultMatrixf(m);
 
-    //DrawCoordSystem();
+	if (shape->GetShapeType() == COMPOUND_SHAPE_PROXYTYPE)
-    
-	glPushMatrix();
-	glEnable(GL_COLOR_MATERIAL);
-	glColor3f(color.x(),color.y(), color.z());
-
-	glRasterPos3f(0.0,  0.0,  0.0);
-
-	bool useWireframeFallback = true;
-
-	if (!(debugMode & IDebugDraw::DBG_DrawWireframe))
 	{
-		switch (shape->GetShapeType())
+		const CompoundShape* compoundShape = static_cast<const CompoundShape*>(shape);
+		for (int i=0;i<compoundShape->GetNumChildShapes();i++)
 		{
-		case BOX_SHAPE_PROXYTYPE:
+			SimdTransform childTrans = compoundShape->GetChildTransform(i);
-			{
+			const CollisionShape* colShape = compoundShape->GetChildShape(i);
-				const BoxShape* boxShape = static_cast<const BoxShape*>(shape);
+			float childMat[16];
-				SimdVector3 halfExtent = boxShape->GetHalfExtents();
+			childTrans.getOpenGLMatrix(childMat);
-				glScaled(2*halfExtent[0], 2*halfExtent[1], 2*halfExtent[2]);
+			DrawOpenGL(childMat,colShape,color,debugMode);
-				glutSolidCube(1.0);
+		}
-				useWireframeFallback = false;
-				break;
-			}
-		case TRIANGLE_SHAPE_PROXYTYPE:
-		case TETRAHEDRAL_SHAPE_PROXYTYPE:
-			{
-				const BU_Simplex1to4* tetra = static_cast<const BU_Simplex1to4*>(shape);
-				//todo:	
-				useWireframeFallback = false;
-				break;
-			}
-		case CONVEX_HULL_SHAPE_PROXYTYPE:
-			break;
-		case SPHERE_SHAPE_PROXYTYPE:
-			{
-				const SphereShape* sphereShape = static_cast<const SphereShape*>(shape);
-				float radius = sphereShape->GetMargin();//radius doesn't include the margin, so draw with margin
-				glutSolidSphere(radius,10,10);
-				useWireframeFallback = false;
-				break;
-			}
-		case MULTI_SPHERE_SHAPE_PROXYTYPE:
-		case CONE_SHAPE_PROXYTYPE:
-			{
-				const ConeShape* coneShape = static_cast<const ConeShape*>(shape);
-				float radius = coneShape->GetRadius();//+coneShape->GetMargin();
-				float height = coneShape->GetHeight();//+coneShape->GetMargin();
-				//glRotatef(-90.0, 1.0, 0.0, 0.0);
-				glTranslatef(0.0, 0.0, -0.5*height);
-				glutSolidCone(radius,height,10,10);
-				useWireframeFallback = false;
-				break;
 
-			}
+		return;
-		case CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE:
+	} else
-			{
+	{
-				useWireframeFallback = false;
+		//DrawCoordSystem();
-				break;
+	    
-			}
+		glPushMatrix();
+		glEnable(GL_COLOR_MATERIAL);
+		glColor3f(color.x(),color.y(), color.z());
 
-		case CONVEX_SHAPE_PROXYTYPE:
+		glRasterPos3f(0.0,  0.0,  0.0);
-		case CYLINDER_SHAPE_PROXYTYPE:
+
+		bool useWireframeFallback = true;
+
+		if (!(debugMode & IDebugDraw::DBG_DrawWireframe))
+		{
+			switch (shape->GetShapeType())
 			{
-				const CylinderShape* cylinder = static_cast<const CylinderShape*>(shape);
+			case BOX_SHAPE_PROXYTYPE:
-				int upAxis = cylinder->GetUpAxis();
-				
-				GLUquadricObj *quadObj = gluNewQuadric();
-				
-				glPushMatrix();
-				switch (upAxis)
 				{
-				case 0:
+					const BoxShape* boxShape = static_cast<const BoxShape*>(shape);
-					glRotatef(-90.0, 0.0, 1.0, 0.0);
+					SimdVector3 halfExtent = boxShape->GetHalfExtents();
-					glTranslatef(0.0, 0.0, -1.0);
+					glScaled(2*halfExtent[0], 2*halfExtent[1], 2*halfExtent[2]);
+					glutSolidCube(1.0);
+					useWireframeFallback = false;
 					break;
-				case 1:
+				}
-					glRotatef(-90.0, 1.0, 0.0, 0.0);
+			case TRIANGLE_SHAPE_PROXYTYPE:
-					glTranslatef(0.0, 0.0, -1.0);
+			case TETRAHEDRAL_SHAPE_PROXYTYPE:
+				{
+					const BU_Simplex1to4* tetra = static_cast<const BU_Simplex1to4*>(shape);
+					//todo:	
+					useWireframeFallback = false;
 					break;
-				case 2:
+				}
+			case CONVEX_HULL_SHAPE_PROXYTYPE:
+				break;
+			case SPHERE_SHAPE_PROXYTYPE:
+				{
+					const SphereShape* sphereShape = static_cast<const SphereShape*>(shape);
+					float radius = sphereShape->GetMargin();//radius doesn't include the margin, so draw with margin
+					glutSolidSphere(radius,10,10);
+					useWireframeFallback = false;
+					break;
+				}
+			case MULTI_SPHERE_SHAPE_PROXYTYPE:
+			case CONE_SHAPE_PROXYTYPE:
+				{
+					const ConeShape* coneShape = static_cast<const ConeShape*>(shape);
+					float radius = coneShape->GetRadius();//+coneShape->GetMargin();
+					float height = coneShape->GetHeight();//+coneShape->GetMargin();
+					//glRotatef(-90.0, 1.0, 0.0, 0.0);
+					glTranslatef(0.0, 0.0, -0.5*height);
+					glutSolidCone(radius,height,10,10);
+					useWireframeFallback = false;
+					break;
+
+				}
+			case CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE:
+				{
+					useWireframeFallback = false;
+					break;
+				}
+
+			case CONVEX_SHAPE_PROXYTYPE:
+			case CYLINDER_SHAPE_PROXYTYPE:
+				{
+					const CylinderShape* cylinder = static_cast<const CylinderShape*>(shape);
+					int upAxis = cylinder->GetUpAxis();
 					
-					glTranslatef(0.0, 0.0, -1.0);
+					GLUquadricObj *quadObj = gluNewQuadric();
-					break;
+					
-				default:
+					glPushMatrix();
+					switch (upAxis)
 					{
-						assert(0);
+					case 0:
+						glRotatef(-90.0, 0.0, 1.0, 0.0);
+						glTranslatef(0.0, 0.0, -1.0);
+						break;
+					case 1:
+						glRotatef(-90.0, 1.0, 0.0, 0.0);
+						glTranslatef(0.0, 0.0, -1.0);
+						break;
+					case 2:
+						
+						glTranslatef(0.0, 0.0, -1.0);
+						break;
+					default:
+						{
+							assert(0);
+						}
+
+					}
+					
+					//The gluCylinder subroutine draws a cylinder that is oriented along the z axis. 
+					//The base of the cylinder is placed at z = 0; the top of the cylinder is placed at z=height. 
+					//Like a sphere, the cylinder is subdivided around the z axis into slices and along the z axis into stacks.
+					
+					gluQuadricDrawStyle(quadObj, (GLenum)GLU_FILL);
+					gluQuadricNormals(quadObj, (GLenum)GLU_SMOOTH);
+					float radius = cylinder->GetHalfExtents().getX();
+					float height = 2.f*cylinder->GetHalfExtents().getY();
+					gluCylinder(quadObj, radius, radius, height, 15, 10);
+					glPopMatrix();
+					glEndList();
+
+					break;
+				}
+			default:
+				{
+				}
+
+			};
+
+		}
+		
+
+		
+
+		if (useWireframeFallback)
+		{
+			/// for polyhedral shapes
+			if (shape->IsPolyhedral())
+			{
+				PolyhedralConvexShape* polyshape = (PolyhedralConvexShape*) shape;
+				
+				
+				glBegin(GL_LINES);
+
+
+				int i;
+				for (i=0;i<polyshape->GetNumEdges();i++)
+				{
+					SimdPoint3 a,b;
+					polyshape->GetEdge(i,a,b);
+
+					glVertex3f(a.getX(),a.getY(),a.getZ());
+					glVertex3f(b.getX(),b.getY(),b.getZ());
+
+
+				}
+				glEnd();
+
+				
+				if (debugMode==IDebugDraw::DBG_DrawFeaturesText)
+				{
+					BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->GetExtraDebugInfo());
+
+					glColor3f(1.f, 1.f, 1.f);
+					for (i=0;i<polyshape->GetNumVertices();i++)
+					{
+						SimdPoint3 vtx;
+						polyshape->GetVertex(i,vtx);
+						glRasterPos3f(vtx.x(),  vtx.y(),  vtx.z());
+						char buf[12];
+						sprintf(buf," %d",i);
+						BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
 					}
 
+					for (i=0;i<polyshape->GetNumPlanes();i++)
+					{
+						SimdVector3 normal;
+						SimdPoint3 vtx;
+						polyshape->GetPlane(normal,vtx,i);
+						SimdScalar d = vtx.dot(normal);
+
+						glRasterPos3f(normal.x()*d,  normal.y()*d, normal.z()*d);
+						char buf[12];
+						sprintf(buf," plane %d",i);
+						BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+						
+					}
 				}
+
 				
-				//The gluCylinder subroutine draws a cylinder that is oriented along the z axis. 
-				//The base of the cylinder is placed at z = 0; the top of the cylinder is placed at z=height. 
-				//Like a sphere, the cylinder is subdivided around the z axis into slices and along the z axis into stacks.
-				
-				gluQuadricDrawStyle(quadObj, (GLenum)GLU_FILL);
-				gluQuadricNormals(quadObj, (GLenum)GLU_SMOOTH);
-				float radius = cylinder->GetHalfExtents().getX();
-				float height = 2.f*cylinder->GetHalfExtents().getY();
-				gluCylinder(quadObj, radius, radius, height, 15, 10);
-				glPopMatrix();
-				glEndList();
-
-				break;
 			}
-		default:
-			{
-			}
-
-		};
-
-	}
-	
-
-	
-
-	if (useWireframeFallback)
-	{
-		/// for polyhedral shapes
-		if (shape->IsPolyhedral())
-		{
-			PolyhedralConvexShape* polyshape = (PolyhedralConvexShape*) shape;
-			
-			
-			glBegin(GL_LINES);
-
-
-			int i;
-			for (i=0;i<polyshape->GetNumEdges();i++)
-			{
-				SimdPoint3 a,b;
-				polyshape->GetEdge(i,a,b);
-
-				glVertex3f(a.getX(),a.getY(),a.getZ());
-				glVertex3f(b.getX(),b.getY(),b.getZ());
-
-
-			}
-			glEnd();
-
-			
-			if (debugMode==IDebugDraw::DBG_DrawFeaturesText)
-			{
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->GetExtraDebugInfo());
-
-				glColor3f(1.f, 1.f, 1.f);
-				for (i=0;i<polyshape->GetNumVertices();i++)
-				{
-					SimdPoint3 vtx;
-					polyshape->GetVertex(i,vtx);
-					glRasterPos3f(vtx.x(),  vtx.y(),  vtx.z());
-					char buf[12];
-					sprintf(buf," %d",i);
-					BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				}
-
-				for (i=0;i<polyshape->GetNumPlanes();i++)
-				{
-					SimdVector3 normal;
-					SimdPoint3 vtx;
-					polyshape->GetPlane(normal,vtx,i);
-					SimdScalar d = vtx.dot(normal);
-
-					glRasterPos3f(normal.x()*d,  normal.y()*d, normal.z()*d);
-					char buf[12];
-					sprintf(buf," plane %d",i);
-					BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-					
-				}
-			}
-
-			
 		}
-	}
 
-	if (shape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+		if (shape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
-	{
+		{
-		TriangleMeshShape* concaveMesh = (TriangleMeshShape*) shape;
+			TriangleMeshShape* concaveMesh = (TriangleMeshShape*) shape;
-		//SimdVector3 aabbMax(1e30f,1e30f,1e30f);
+			//SimdVector3 aabbMax(1e30f,1e30f,1e30f);
-		//SimdVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);
+			//SimdVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);
+			
+			extern float eye[3];
+			SimdVector3 aabbMax(eye[0]+100,eye[1]+100,eye[2]+100);//1e30f,1e30f,1e30f);
+			SimdVector3 aabbMin(eye[0]-100,eye[1]-100,eye[2]-100);//1e30f,1e30f,1e30f);
+
+			GlDrawcallback drawCallback;
+
+			concaveMesh->ProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+
+
+		}
+
+		if (shape->GetShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
+		{
+			ConvexTriangleMeshShape* convexMesh = (ConvexTriangleMeshShape*) shape;
+			
+			extern float eye[3];
+			SimdVector3 aabbMax(eye[0]+100,eye[1]+100,eye[2]+100);
+			SimdVector3 aabbMin(eye[0]-100,eye[1]-100,eye[2]-100);
+			TriangleGlDrawcallback drawCallback;
+			convexMesh->GetStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+
+		}
 		
-		extern float eye[3];
+		glDisable(GL_DEPTH_BUFFER_BIT);
-		SimdVector3 aabbMax(eye[0]+100,eye[1]+100,eye[2]+100);//1e30f,1e30f,1e30f);
+		if (debugMode==IDebugDraw::DBG_DrawText)
-		SimdVector3 aabbMin(eye[0]-100,eye[1]-100,eye[2]-100);//1e30f,1e30f,1e30f);
+		{
-
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->GetName());
-		GlDrawcallback drawCallback;
+		}
-
-		concaveMesh->ProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
-
 
+		if (debugMode==IDebugDraw::DBG_DrawFeaturesText)
+		{
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->GetExtraDebugInfo());
+		}
+		glEnable(GL_DEPTH_BUFFER_BIT);
+		glPopMatrix();
 	}
-
-	if (shape->GetShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
-	{
-		ConvexTriangleMeshShape* convexMesh = (ConvexTriangleMeshShape*) shape;
-		
-		extern float eye[3];
-		SimdVector3 aabbMax(eye[0]+100,eye[1]+100,eye[2]+100);
-		SimdVector3 aabbMin(eye[0]-100,eye[1]-100,eye[2]-100);
-		TriangleGlDrawcallback drawCallback;
-		convexMesh->GetStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
-
-	}
-	
-	glDisable(GL_DEPTH_BUFFER_BIT);
-	if (debugMode==IDebugDraw::DBG_DrawText)
-	{
-		BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->GetName());
-	}
-
-	if (debugMode==IDebugDraw::DBG_DrawFeaturesText)
-	{
-		BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->GetExtraDebugInfo());
-	}
-	glEnable(GL_DEPTH_BUFFER_BIT);
-    
-	glPopMatrix();
     glPopMatrix();
 	
 }