diff --git a/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp b/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp
index 584d2eba8..fc5ffbdb4 100644
--- a/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp
+++ b/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp
@@ -17,6 +17,7 @@ subject to the following restrictions:
 ///
 /// CollisionInterfaceDemo shows high level usage of the Collision Detection.
 ///
+#define TEST_NOT_ADDING_OBJECTS_TO_WORLD
 
 #include "GL_Simplex1to4.h"
 
@@ -57,7 +58,7 @@ int main(int argc,char** argv)
 void	CollisionInterfaceDemo::initPhysics()
 {
 			
-	//m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
+	m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
 	
 	btMatrix3x3 basisA;
 	basisA.setIdentity();
@@ -69,7 +70,10 @@ void	CollisionInterfaceDemo::initPhysics()
 	objects[1].getWorldTransform().setBasis(basisB);
 
 	btBoxShape* boxA = new btBoxShape(btVector3(1,1,1));
+	boxA->setMargin(0.f);
+
 	btBoxShape* boxB = new btBoxShape(btVector3(0.5,0.5,0.5));
+	boxB->setMargin(0.f);
 	//ConvexHullShape	hullA(points0,3);
 	//hullA.setLocalScaling(btVector3(3,3,3));
 	//ConvexHullShape	hullB(points1,4);
@@ -89,9 +93,13 @@ void	CollisionInterfaceDemo::initPhysics()
 	//SimpleBroadphase*	broadphase = new btSimpleBroadphase;
 
 	collisionWorld = new btCollisionWorld(dispatcher,broadphase,collisionConfiguration);
+	collisionWorld->setDebugDrawer(&debugDrawer);
 		
+#ifdef TEST_NOT_ADDING_OBJECTS_TO_WORLD
 	collisionWorld->addCollisionObject(&objects[0]);
 	collisionWorld->addCollisionObject(&objects[1]);
+#endif //TEST_NOT_ADDING_OBJECTS_TO_WORLD
+
 
 }
 
@@ -115,14 +123,33 @@ void CollisionInterfaceDemo::displayCallback(void) {
     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
 	glDisable(GL_LIGHTING);
 
+		btScalar m[16];
+	
+	btVector3	worldBoundsMin,worldBoundsMax;
+	collisionWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);
+
+
+	int i;
+	for (i=0;i<numObjects;i++)
+	{
+		
+		objects[i].getWorldTransform().getOpenGLMatrix( m );
+		m_shapeDrawer->drawOpenGL(m,objects[i].getCollisionShape(),btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
+	}
+
 	collisionWorld->getDispatchInfo().m_debugDraw = &debugDrawer;
 	
 	if (collisionWorld)
 		collisionWorld->performDiscreteCollisionDetection();
-	
-	int i;
 
-	///one way to draw all the contact points is iterating over contact manifolds / points:
+	
+	
+
+
+#ifndef TEST_NOT_ADDING_OBJECTS_TO_WORLD
+	
+	collisionWorld->debugDrawWorld();
+	///one way to draw all the contact points is iterating over contact manifolds in the dispatcher:
 
 	int numManifolds = collisionWorld->getDispatcher()->getNumManifolds();
 	for (i=0;i<numManifolds;i++)
@@ -137,7 +164,7 @@ void CollisionInterfaceDemo::displayCallback(void) {
 			btManifoldPoint& pt = contactManifold->getContactPoint(j);
 
 			glBegin(GL_LINES);
-			glColor3f(1, 0, 1);
+			glColor3f(0, 0, 0);
 			
 			btVector3 ptA = pt.getPositionWorldOnA();
 			btVector3 ptB = pt.getPositionWorldOnB();
@@ -150,33 +177,88 @@ void CollisionInterfaceDemo::displayCallback(void) {
 		//you can un-comment out this line, and then all points are removed
 		//contactManifold->clearManifold();	
 	}
+#else
+
+	glDisable(GL_TEXTURE_2D);
+	for (i=0;i<numObjects;i++)
+	{
+		collisionWorld->debugDrawObject(objects[i].getWorldTransform(),objects[i].getCollisionShape(), btVector3(1,1,0));
+	}
+
+	//another way is to directly query the dispatcher for both objects. The objects don't need to be inserted into the world
+
+	btCollisionAlgorithm* algo = collisionWorld->getDispatcher()->findAlgorithm(&objects[0],&objects[1]);
+	btManifoldResult contactPointResult(&objects[0],&objects[1]);
+	algo->processCollision(&objects[0],&objects[1],collisionWorld->getDispatchInfo(),&contactPointResult);
+	
+	btManifoldArray manifoldArray;
+	algo->getAllContactManifolds(manifoldArray);
+
+	int numManifolds = manifoldArray.size();
+	for (i=0;i<numManifolds;i++)
+	{
+		btPersistentManifold* contactManifold = manifoldArray[i];
+		btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+		btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+	
+		glDisable(GL_DEPTH_TEST);
+		int numContacts = contactManifold->getNumContacts();
+		bool swap = obA == &objects[0];
+
+		for (int j=0;j<numContacts;j++)
+		{
+			btManifoldPoint& pt = contactManifold->getContactPoint(j);
+		
+			glBegin(GL_LINES);
+			glColor3f(0, 0, 0);
+
+			btVector3 ptA = swap ?pt.getPositionWorldOnA():pt.getPositionWorldOnB();
+			btVector3 ptB = swap ? pt.getPositionWorldOnB():pt.getPositionWorldOnA();
+
+			glVertex3d(ptA.x(),ptA.y(),ptA.z());
+			glVertex3d(ptB.x(),ptB.y(),ptB.z());
+			glEnd();
+		}
+
+		//you can un-comment out this line, and then all points are removed
+		//contactManifold->clearManifold();	
+	}
+
+#endif
+	
+
+
+
 
 	//GL_ShapeDrawer::drawCoordSystem();
 
-	btScalar m[16];
-	
-	btVector3	worldBoundsMin,worldBoundsMax;
-	collisionWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);
+
+	btQuaternion qA = objects[0].getWorldTransform().getRotation();
+	btQuaternion qB = objects[1].getWorldTransform().getRotation();
 
 
-	for (i=0;i<numObjects;i++)
+	if (!m_idle)
 	{
+
 		
-		objects[i].getWorldTransform().getOpenGLMatrix( m );
-		m_shapeDrawer->drawOpenGL(m,objects[i].getCollisionShape(),btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
+		btScalar timeInSeconds = getDeltaTimeMicroseconds()/1000.f;
 
+		btQuaternion orn;
+
+		objects[0].getWorldTransform().getBasis().getEulerYPR(yaw,pitch,roll);
+		pitch += 0.00005f*timeInSeconds;
+		yaw += 0.0001f*timeInSeconds;
+		objects[0].getWorldTransform().getBasis().setEulerYPR(yaw,pitch,roll);
+
+		orn.setEuler(yaw,pitch,roll);
+		objects[1].getWorldTransform().setOrigin(objects[1].getWorldTransform().getOrigin()+btVector3(0,-0.00001*timeInSeconds,0));
+
+		//objects[0].getWorldTransform().setRotation(orn);
+
+		
+		
 	}
 
-
-	btQuaternion orn;
-	orn.setEuler(yaw,pitch,roll);
-	objects[1].getWorldTransform().setOrigin(objects[1].getWorldTransform().getOrigin()+btVector3(0,-0.01,0));
-
-	objects[0].getWorldTransform().setRotation(orn);
-
-	pitch += 0.005f;
-	yaw += 0.01f;
-
 	glFlush();
     glutSwapBuffers();
 }
@@ -184,7 +266,14 @@ void CollisionInterfaceDemo::displayCallback(void) {
 void CollisionInterfaceDemo::clientResetScene()
 {
 	objects[0].getWorldTransform().setOrigin(btVector3(0.0f,3.f,0.f));
-	objects[1].getWorldTransform().setOrigin(btVector3(0.0f,9.f,0.f));
+	
+	btQuaternion rotA(0.739,-0.204,0.587,0.257);
+	rotA.normalize();
+
+	objects[0].getWorldTransform().setRotation(rotA);
+
+	objects[1].getWorldTransform().setOrigin(btVector3(0.0f,4.248f,0.f));
+	
 }
 
 
diff --git a/Demos/OpenGL/GL_ShapeDrawer.cpp b/Demos/OpenGL/GL_ShapeDrawer.cpp
index 13a636203..262dd9e37 100644
--- a/Demos/OpenGL/GL_ShapeDrawer.cpp
+++ b/Demos/OpenGL/GL_ShapeDrawer.cpp
@@ -601,17 +601,25 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 						7,2,3,
 						7,6,2};
 
-					static btVector3 vertices[8]={	btVector3(1,1,1),btVector3(-1,1,1),	btVector3(1,-1,1),	btVector3(-1,-1,1),	btVector3(1,1,-1),	btVector3(-1,1,-1),	btVector3(1,-1,-1),	btVector3(-1,-1,-1)};
+					 btVector3 vertices[8]={	
+						btVector3(halfExtent[0],halfExtent[1],halfExtent[2]),
+						btVector3(-halfExtent[0],halfExtent[1],halfExtent[2]),
+						btVector3(halfExtent[0],-halfExtent[1],halfExtent[2]),	
+						btVector3(-halfExtent[0],-halfExtent[1],halfExtent[2]),	
+						btVector3(halfExtent[0],halfExtent[1],-halfExtent[2]),
+						btVector3(-halfExtent[0],halfExtent[1],-halfExtent[2]),	
+						btVector3(halfExtent[0],-halfExtent[1],-halfExtent[2]),	
+						btVector3(-halfExtent[0],-halfExtent[1],-halfExtent[2])};
+#if 1
 					glBegin (GL_TRIANGLES);
 					int si=36;
 					for (int i=0;i<si;i+=3)
 					{
-						btVector3 v1 = vertices[indices[i]]*halfExtent;
-						btVector3 v2 = vertices[indices[i+1]]*halfExtent;
-						btVector3 v3 = vertices[indices[i+2]]*halfExtent;
+						const btVector3& v1 = vertices[indices[i]];;
+						const btVector3& v2 = vertices[indices[i+1]];
+						const btVector3& v3 = vertices[indices[i+2]];
 						btVector3 normal = (v3-v1).cross(v2-v1);
 						normal.normalize ();
-
 						glNormal3f(normal.getX(),normal.getY(),normal.getZ());
 						glVertex3f (v1.x(), v1.y(), v1.z());
 						glVertex3f (v2.x(), v2.y(), v2.z());
@@ -619,6 +627,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 						
 					}
 					glEnd();
+#endif
 
 					useWireframeFallback = false;
 					break;
@@ -812,8 +821,6 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 			btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
 
 			{
-				glRasterPos3f(0.0,  0.0,  0.0);
-				//btDrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->getExtraDebugInfo());
 
 				glColor3f(1.f, 1.f, 1.f);
 				int i;
@@ -821,7 +828,6 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 				{
 					btVector3 vtx;
 					polyshape->getVertex(i,vtx);
-					glRasterPos3f(vtx.x(),  vtx.y(),  vtx.z());
 					char buf[12];
 					sprintf(buf," %d",i);
 					//btDrawString(BMF_GetFont(BMF_kHelvetica10),buf);
@@ -834,7 +840,6 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 					polyshape->getPlane(normal,vtx,i);
 					btScalar d = vtx.dot(normal);
 
-					glRasterPos3f(normal.x()*d,  normal.y()*d, normal.z()*d);
 					char buf[12];
 					sprintf(buf," plane %d",i);
 					//btDrawString(BMF_GetFont(BMF_kHelvetica10),buf);
@@ -878,21 +883,6 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 
 
 
-glDisable(GL_DEPTH_TEST);
-glRasterPos3f(0,0,0);//mvtx.x(),  vtx.y(),  vtx.z());
-if (debugMode&btIDebugDraw::DBG_DrawText)
-{
-	GLDebugDrawString(0,0,shape->getName());
-}
-
-if (debugMode& btIDebugDraw::DBG_DrawFeaturesText)
-{
-	//btDrawString(BMF_GetFont(BMF_kHelvetica10),shape->getExtraDebugInfo());
-}
-glEnable(GL_DEPTH_TEST);
-
-//	glPopMatrix();	
-if(m_textureenabled) glDisable(GL_TEXTURE_2D);
 	}
 	glPopMatrix();
 
diff --git a/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
index 8600dbfbf..bcd42ca16 100644
--- a/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
+++ b/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
@@ -1,4 +1,3 @@
-
 /*
  * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
  * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
@@ -424,6 +423,7 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
 		output.addContactPoint(-normal,pointInWorld,-*depth);
 #else
 		output.addContactPoint(-normal,pb,-*depth);
+
 #endif //
 		*return_code = code;
 	}
@@ -593,21 +593,30 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
   if (maxc < 1) maxc = 1;
 
   if (cnum <= maxc) {
+
+	  if (code<4) 
+	  {
     // we have less contacts than we need, so we use them all
-    for (j=0; j < cnum; j++) {
-
-		//AddContactPoint...
-
-		//dContactGeom *con = CONTACT(contact,skip*j);
-      //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
-      //con->depth = dep[j];
-
+    for (j=0; j < cnum; j++) 
+	{
 		btVector3 pointInWorld;
 		for (i=0; i<3; i++) 
 			pointInWorld[i] = point[j*3+i] + pa[i];
 		output.addContactPoint(-normal,pointInWorld,-dep[j]);
 
     }
+	  } else
+	  {
+		  // we have less contacts than we need, so we use them all
+		for (j=0; j < cnum; j++) 
+		{
+			btVector3 pointInWorld;
+			for (i=0; i<3; i++) 
+				pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
+				//pointInWorld[i] = point[j*3+i] + pa[i];
+			output.addContactPoint(-normal,pointInWorld,-dep[j]);
+		}
+	  }
   }
   else {
     // we have more contacts than are wanted, some of them must be culled.
@@ -632,7 +641,13 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
 		btVector3 posInWorld;
 		for (i=0; i<3; i++) 
 			posInWorld[i] = point[iret[j]*3+i] + pa[i];
-		output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
+		if (code<4) 
+	   {
+			output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
+		} else
+		{
+			output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]);
+		}
     }
     cnum = maxc;
   }
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index c5d5646ea..f1b82923f 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -42,6 +42,24 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
 
 
+///for debug drawing
+
+//for debug rendering
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btConeShape.h"
+#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+
+
+
 btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
 :m_dispatcher1(dispatcher),
 m_broadphasePairCache(pairCache),
@@ -92,27 +110,27 @@ void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
 	btAssert(collisionObject);
 
 	//check that the object isn't already added
-		btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
+	btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
 
-		m_collisionObjects.push_back(collisionObject);
+	m_collisionObjects.push_back(collisionObject);
 
-		//calculate new AABB
-		btTransform trans = collisionObject->getWorldTransform();
+	//calculate new AABB
+	btTransform trans = collisionObject->getWorldTransform();
 
-		btVector3	minAabb;
-		btVector3	maxAabb;
-		collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
+	btVector3	minAabb;
+	btVector3	maxAabb;
+	collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
 
-		int type = collisionObject->getCollisionShape()->getShapeType();
-		collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
-			minAabb,
-			maxAabb,
-			type,
-			collisionObject,
-			collisionFilterGroup,
-			collisionFilterMask,
-			m_dispatcher1,0
-			))	;
+	int type = collisionObject->getCollisionShape()->getShapeType();
+	collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
+		minAabb,
+		maxAabb,
+		type,
+		collisionObject,
+		collisionFilterGroup,
+		collisionFilterMask,
+		m_dispatcher1,0
+		))	;
 
 
 
@@ -228,10 +246,10 @@ void	btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
 
 
 void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
-					  btCollisionObject* collisionObject,
-					  const btCollisionShape* collisionShape,
-					  const btTransform& colObjWorldTransform,
-					  RayResultCallback& resultCallback)
+										btCollisionObject* collisionObject,
+										const btCollisionShape* collisionShape,
+										const btTransform& colObjWorldTransform,
+										RayResultCallback& resultCallback)
 {
 	btSphereShape pointShape(btScalar(0.0));
 	pointShape.setMargin(0.f);
@@ -239,7 +257,7 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 
 	if (collisionShape->isConvex())
 	{
-//		BT_PROFILE("rayTestConvex");
+		//		BT_PROFILE("rayTestConvex");
 		btConvexCast::CastResult castResult;
 		castResult.m_fraction = resultCallback.m_closestHitFraction;
 
@@ -268,10 +286,10 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 					castResult.m_normal.normalize();
 					btCollisionWorld::LocalRayResult localRayResult
 						(
-							collisionObject,
-							0,
-							castResult.m_normal,
-							castResult.m_fraction
+						collisionObject,
+						0,
+						castResult.m_normal,
+						castResult.m_fraction
 						);
 
 					bool normalInWorldSpace = true;
@@ -283,7 +301,7 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 	} else {
 		if (collisionShape->isConcave())
 		{
-//			BT_PROFILE("rayTestConcave");
+			//			BT_PROFILE("rayTestConcave");
 			if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
 			{
 				///optimized version for btBvhTriangleMeshShape
@@ -299,18 +317,18 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 					btCollisionObject*	m_collisionObject;
 					btTriangleMeshShape*	m_triangleMesh;
 
-               btTransform m_colObjWorldTransform;
+					btTransform m_colObjWorldTransform;
 
 					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
 						btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh,const btTransform& colObjWorldTransform):
-                  //@BP Mod
-						btTriangleRaycastCallback(from,to, resultCallback->m_flags),
-							m_resultCallback(resultCallback),
-							m_collisionObject(collisionObject),
-							m_triangleMesh(triangleMesh),
-                     m_colObjWorldTransform(colObjWorldTransform)
-						{
-						}
+					//@BP Mod
+					btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+						m_resultCallback(resultCallback),
+						m_collisionObject(collisionObject),
+						m_triangleMesh(triangleMesh),
+						m_colObjWorldTransform(colObjWorldTransform)
+					{
+					}
 
 
 					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
@@ -319,10 +337,10 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 						shapeInfo.m_shapePart = partId;
 						shapeInfo.m_triangleIndex = triangleIndex;
 
-                  btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
+						btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
 
 						btCollisionWorld::LocalRayResult rayResult
-						(m_collisionObject,
+							(m_collisionObject,
 							&shapeInfo,
 							hitNormalWorld,
 							hitFraction);
@@ -354,18 +372,18 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 					btCollisionObject*	m_collisionObject;
 					btConcaveShape*	m_triangleMesh;
 
-               btTransform m_colObjWorldTransform;
+					btTransform m_colObjWorldTransform;
 
 					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
 						btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*	triangleMesh, const btTransform& colObjWorldTransform):
-                  //@BP Mod
-                  btTriangleRaycastCallback(from,to, resultCallback->m_flags),
-							m_resultCallback(resultCallback),
-							m_collisionObject(collisionObject),
-							m_triangleMesh(triangleMesh),
-                     m_colObjWorldTransform(colObjWorldTransform)
-						{
-						}
+					//@BP Mod
+					btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+						m_resultCallback(resultCallback),
+						m_collisionObject(collisionObject),
+						m_triangleMesh(triangleMesh),
+						m_colObjWorldTransform(colObjWorldTransform)
+					{
+					}
 
 
 					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
@@ -374,10 +392,10 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 						shapeInfo.m_shapePart = partId;
 						shapeInfo.m_triangleIndex = triangleIndex;
 
-                  btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
+						btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
 
 						btCollisionWorld::LocalRayResult rayResult
-						(m_collisionObject,
+							(m_collisionObject,
 							&shapeInfo,
 							hitNormalWorld,
 							hitFraction);
@@ -400,7 +418,7 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 				concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
 			}
 		} else {
-//			BT_PROFILE("rayTestCompound");
+			//			BT_PROFILE("rayTestCompound");
 			///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
 			if (collisionShape->isCompound())
 			{
@@ -428,10 +446,10 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 }
 
 void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
-					  btCollisionObject* collisionObject,
-					  const btCollisionShape* collisionShape,
-					  const btTransform& colObjWorldTransform,
-					  ConvexResultCallback& resultCallback, btScalar allowedPenetration)
+											btCollisionObject* collisionObject,
+											const btCollisionShape* collisionShape,
+											const btTransform& colObjWorldTransform,
+											ConvexResultCallback& resultCallback, btScalar allowedPenetration)
 {
 	if (collisionShape->isConvex())
 	{
@@ -443,15 +461,15 @@ void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
 		btConvexShape* convexShape = (btConvexShape*) collisionShape;
 		btVoronoiSimplexSolver	simplexSolver;
 		btGjkEpaPenetrationDepthSolver	gjkEpaPenetrationSolver;
-		
+
 		btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
 		//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
 		//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
 
 		btConvexCast* castPtr = &convexCaster1;
-	
-	
-		
+
+
+
 		if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
 		{
 			//add hit
@@ -461,13 +479,13 @@ void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
 				{
 					castResult.m_normal.normalize();
 					btCollisionWorld::LocalConvexResult localConvexResult
-								(
-									collisionObject,
-									0,
-									castResult.m_normal,
-									castResult.m_hitPoint,
-									castResult.m_fraction
-								);
+						(
+						collisionObject,
+						0,
+						castResult.m_normal,
+						castResult.m_hitPoint,
+						castResult.m_fraction
+						);
 
 					bool normalInWorldSpace = true;
 					resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
@@ -497,12 +515,12 @@ void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
 
 					BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
 						btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh, const btTransform& triangleToWorld):
-						btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
-							m_resultCallback(resultCallback),
-							m_collisionObject(collisionObject),
-							m_triangleMesh(triangleMesh)
-						{
-						}
+					btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
+						m_resultCallback(resultCallback),
+						m_collisionObject(collisionObject),
+						m_triangleMesh(triangleMesh)
+					{
+					}
 
 
 					virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
@@ -514,7 +532,7 @@ void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
 						{
 
 							btCollisionWorld::LocalConvexResult convexResult
-							(m_collisionObject,
+								(m_collisionObject,
 								&shapeInfo,
 								hitNormalLocal,
 								hitPointLocal,
@@ -554,12 +572,12 @@ void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
 
 					BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
 						btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*	triangleMesh, const btTransform& triangleToWorld):
-						btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
-							m_resultCallback(resultCallback),
-							m_collisionObject(collisionObject),
-							m_triangleMesh(triangleMesh)
-						{
-						}
+					btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
+						m_resultCallback(resultCallback),
+						m_collisionObject(collisionObject),
+						m_triangleMesh(triangleMesh)
+					{
+					}
 
 
 					virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
@@ -571,7 +589,7 @@ void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
 						{
 
 							btCollisionWorld::LocalConvexResult convexResult
-							(m_collisionObject,
+								(m_collisionObject,
 								&shapeInfo,
 								hitNormalLocal,
 								hitPointLocal,
@@ -641,10 +659,10 @@ struct btSingleRayCallback : public btBroadphaseRayCallback
 	btCollisionWorld::RayResultCallback&	m_resultCallback;
 
 	btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
-	:m_rayFromWorld(rayFromWorld),
-	m_rayToWorld(rayToWorld),
-	m_world(world),
-	m_resultCallback(resultCallback)
+		:m_rayFromWorld(rayFromWorld),
+		m_rayToWorld(rayToWorld),
+		m_world(world),
+		m_resultCallback(resultCallback)
 	{
 		m_rayFromTrans.setIdentity();
 		m_rayFromTrans.setOrigin(m_rayFromWorld);
@@ -666,7 +684,7 @@ struct btSingleRayCallback : public btBroadphaseRayCallback
 
 	}
 
-	
+
 
 	virtual bool	process(const btBroadphaseProxy* proxy)
 	{
@@ -697,9 +715,9 @@ struct btSingleRayCallback : public btBroadphaseRayCallback
 			{
 				m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
 					collisionObject,
-						collisionObject->getCollisionShape(),
-						collisionObject->getWorldTransform(),
-						m_resultCallback);
+					collisionObject->getCollisionShape(),
+					collisionObject->getWorldTransform(),
+					m_resultCallback);
 			}
 		}
 		return true;
@@ -771,13 +789,13 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback
 		if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
 			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
 			m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
-					collisionObject,
-						collisionObject->getCollisionShape(),
-						collisionObject->getWorldTransform(),
-						m_resultCallback,
-						m_allowedCcdPenetration);
+				collisionObject,
+				collisionObject->getCollisionShape(),
+				collisionObject->getWorldTransform(),
+				m_resultCallback,
+				m_allowedCcdPenetration);
 		}
-		
+
 		return true;
 	}
 };
@@ -792,7 +810,7 @@ void	btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT
 	/// and for each object with ray-aabb overlap, perform an exact ray test
 	/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
 
-	
+
 
 	btTransform	convexFromTrans,convexToTrans;
 	convexFromTrans = convexFromWorld;
@@ -835,12 +853,355 @@ void	btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT
 			{
 				objectQuerySingle(castShape, convexFromTrans,convexToTrans,
 					collisionObject,
-						collisionObject->getCollisionShape(),
-						collisionObject->getWorldTransform(),
-						resultCallback,
-						allowedCcdPenetration);
+					collisionObject->getCollisionShape(),
+					collisionObject->getWorldTransform(),
+					resultCallback,
+					allowedCcdPenetration);
 			}
 		}
 	}
 #endif //USE_BRUTEFORCE_RAYBROADPHASE
 }
+
+
+
+
+class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
+{
+	btIDebugDraw*	m_debugDrawer;
+	btVector3	m_color;
+	btTransform	m_worldTrans;
+
+public:
+
+	DebugDrawcallback(btIDebugDraw*	debugDrawer,const btTransform& worldTrans,const btVector3& color) :
+	  m_debugDrawer(debugDrawer),
+		  m_color(color),
+		  m_worldTrans(worldTrans)
+	  {
+	  }
+
+	  virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+	  {
+		  processTriangle(triangle,partId,triangleIndex);
+	  }
+
+	  virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
+	  {
+		  (void)partId;
+		  (void)triangleIndex;
+
+		  btVector3 wv0,wv1,wv2;
+		  wv0 = m_worldTrans*triangle[0];
+		  wv1 = m_worldTrans*triangle[1];
+		  wv2 = m_worldTrans*triangle[2];
+		  m_debugDrawer->drawLine(wv0,wv1,m_color);
+		  m_debugDrawer->drawLine(wv1,wv2,m_color);
+		  m_debugDrawer->drawLine(wv2,wv0,m_color);
+	  }
+};
+
+void btCollisionWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
+{
+	btVector3 start = transform.getOrigin();
+
+	const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
+	const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
+	const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
+
+	// XY 
+	getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
+	getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
+	getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
+	getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
+
+	// XZ
+	getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
+	getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
+	getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
+	getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
+
+	// YZ
+	getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
+	getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
+	getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
+	getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
+}
+
+void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
+{
+	// Draw a small simplex at the center of the object
+	{
+		btVector3 start = worldTransform.getOrigin();
+		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
+		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
+		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
+	}
+
+	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
+	{
+		const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
+		for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
+		{
+			btTransform childTrans = compoundShape->getChildTransform(i);
+			const btCollisionShape* colShape = compoundShape->getChildShape(i);
+			debugDrawObject(worldTransform*childTrans,colShape,color);
+		}
+
+	} else
+	{
+		switch (shape->getShapeType())
+		{
+
+		case SPHERE_SHAPE_PROXYTYPE:
+			{
+				const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
+				btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
+
+				debugDrawSphere(radius, worldTransform, color);
+				break;
+			}
+		case MULTI_SPHERE_SHAPE_PROXYTYPE:
+			{
+				const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
+
+				btTransform childTransform;
+				childTransform.setIdentity();
+
+				for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
+				{
+					childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
+					debugDrawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
+				}
+
+				break;
+			}
+		case CAPSULE_SHAPE_PROXYTYPE:
+			{
+				const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
+
+				btScalar radius = capsuleShape->getRadius();
+				btScalar halfHeight = capsuleShape->getHalfHeight();
+
+				int upAxis = capsuleShape->getUpAxis();
+
+
+				btVector3 capStart(0.f,0.f,0.f);
+				capStart[upAxis] = -halfHeight;
+
+				btVector3 capEnd(0.f,0.f,0.f);
+				capEnd[upAxis] = halfHeight;
+
+				// Draw the ends
+				{
+
+					btTransform childTransform = worldTransform;
+					childTransform.getOrigin() = worldTransform * capStart;
+					debugDrawSphere(radius, childTransform, color);
+				}
+
+				{
+					btTransform childTransform = worldTransform;
+					childTransform.getOrigin() = worldTransform * capEnd;
+					debugDrawSphere(radius, childTransform, color);
+				}
+
+				// Draw some additional lines
+				btVector3 start = worldTransform.getOrigin();
+
+
+				capStart[(upAxis+1)%3] = radius;
+				capEnd[(upAxis+1)%3] = radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+				capStart[(upAxis+1)%3] = -radius;
+				capEnd[(upAxis+1)%3] = -radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+
+				capStart[(upAxis+1)%3] = 0.f;
+				capEnd[(upAxis+1)%3] = 0.f;
+
+				capStart[(upAxis+2)%3] = radius;
+				capEnd[(upAxis+2)%3] = radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+				capStart[(upAxis+2)%3] = -radius;
+				capEnd[(upAxis+2)%3] = -radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+
+
+				break;
+			}
+		case CONE_SHAPE_PROXYTYPE:
+			{
+				const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
+				btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
+				btScalar height = coneShape->getHeight();//+coneShape->getMargin();
+				btVector3 start = worldTransform.getOrigin();
+
+				int upAxis= coneShape->getConeUpIndex();
+
+
+				btVector3	offsetHeight(0,0,0);
+				offsetHeight[upAxis] = height * btScalar(0.5);
+				btVector3	offsetRadius(0,0,0);
+				offsetRadius[(upAxis+1)%3] = radius;
+				btVector3	offset2Radius(0,0,0);
+				offset2Radius[(upAxis+2)%3] = radius;
+
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
+
+
+
+				break;
+
+			}
+		case CYLINDER_SHAPE_PROXYTYPE:
+			{
+				const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
+				int upAxis = cylinder->getUpAxis();
+				btScalar radius = cylinder->getRadius();
+				btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
+				btVector3 start = worldTransform.getOrigin();
+				btVector3	offsetHeight(0,0,0);
+				offsetHeight[upAxis] = halfHeight;
+				btVector3	offsetRadius(0,0,0);
+				offsetRadius[(upAxis+1)%3] = radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+				break;
+			}
+
+		case STATIC_PLANE_PROXYTYPE:
+			{
+				const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
+				btScalar planeConst = staticPlaneShape->getPlaneConstant();
+				const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
+				btVector3 planeOrigin = planeNormal * planeConst;
+				btVector3 vec0,vec1;
+				btPlaneSpace1(planeNormal,vec0,vec1);
+				btScalar vecLen = 100.f;
+				btVector3 pt0 = planeOrigin + vec0*vecLen;
+				btVector3 pt1 = planeOrigin - vec0*vecLen;
+				btVector3 pt2 = planeOrigin + vec1*vecLen;
+				btVector3 pt3 = planeOrigin - vec1*vecLen;
+				getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
+				getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
+				break;
+
+			}
+		default:
+			{
+
+				if (shape->isConcave())
+				{
+					btConcaveShape* concaveMesh = (btConcaveShape*) shape;
+
+					///@todo pass camera, for some culling? no -> we are not a graphics lib
+					btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+					btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+
+					DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+					concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
+
+				}
+
+				if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
+				{
+					btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
+					//todo: pass camera for some culling			
+					btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+					btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+					//DebugDrawcallback drawCallback;
+					DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+					convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+				}
+
+
+				/// for polyhedral shapes
+				if (shape->isPolyhedral())
+				{
+					btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
+
+					int i;
+					for (i=0;i<polyshape->getNumEdges();i++)
+					{
+						btVector3 a,b;
+						polyshape->getEdge(i,a,b);
+						btVector3 wa = worldTransform * a;
+						btVector3 wb = worldTransform * b;
+						getDebugDrawer()->drawLine(wa,wb,color);
+
+					}
+
+
+				}
+			}
+		}
+	}
+}
+
+
+void	btCollisionWorld::debugDrawWorld()
+{
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+	{
+		int numManifolds = getDispatcher()->getNumManifolds();
+		btVector3 color(0,0,0);
+		for (int i=0;i<numManifolds;i++)
+		{
+			btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
+			//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+			//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+
+			int numContacts = contactManifold->getNumContacts();
+			for (int j=0;j<numContacts;j++)
+			{
+				btManifoldPoint& cp = contactManifold->getContactPoint(j);
+				getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+			}
+		}
+	}
+
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
+	{
+		int i;
+
+		for (  i=0;i<m_collisionObjects.size();i++)
+		{
+			btCollisionObject* colObj = m_collisionObjects[i];
+			if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+			{
+				btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
+				switch(colObj->getActivationState())
+				{
+				case  ACTIVE_TAG:
+					color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
+				case ISLAND_SLEEPING:
+					color =  btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
+				case WANTS_DEACTIVATION:
+					color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
+				case DISABLE_DEACTIVATION:
+					color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
+				case DISABLE_SIMULATION:
+					color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
+				default:
+					{
+						color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
+					}
+				};
+
+				debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
+			}
+			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+			{
+				btVector3 minAabb,maxAabb;
+				btVector3 colorvec(1,0,0);
+				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+				m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
+			}
+
+		}
+	}
+}
\ No newline at end of file
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
index 983037e69..bddaccbed 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
@@ -150,6 +150,12 @@ public:
 		return m_debugDrawer;
 	}
 
+	virtual void	debugDrawWorld();
+
+	virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
+
+	void	debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
+
 
 	///LocalShapeInfo gives extra information for complex shapes
 	///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart
diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index e97300c52..e114f783d 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -35,20 +35,8 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
 
-//for debug rendering
-#include "BulletCollision/CollisionShapes/btBoxShape.h"
-#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
-#include "BulletCollision/CollisionShapes/btCompoundShape.h"
-#include "BulletCollision/CollisionShapes/btConeShape.h"
-#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
-#include "BulletCollision/CollisionShapes/btCylinderShape.h"
-#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
-#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
-#include "BulletCollision/CollisionShapes/btSphereShape.h"
-#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
-#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
-#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
 #include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
 
 
 #include "BulletDynamics/Dynamics/btActionInterface.h"
@@ -127,24 +115,8 @@ void	btDiscreteDynamicsWorld::debugDrawWorld()
 {
 	BT_PROFILE("debugDrawWorld");
 
-	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
-	{
-		int numManifolds = getDispatcher()->getNumManifolds();
-		btVector3 color(0,0,0);
-		for (int i=0;i<numManifolds;i++)
-		{
-			btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
-			//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
-			//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+	btCollisionWorld::debugDrawWorld();
 
-			int numContacts = contactManifold->getNumContacts();
-			for (int j=0;j<numContacts;j++)
-			{
-				btManifoldPoint& cp = contactManifold->getContactPoint(j);
-				getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
-			}
-		}
-	}
 	bool drawConstraints = false;
 	if (getDebugDrawer())
 	{
@@ -169,42 +141,6 @@ void	btDiscreteDynamicsWorld::debugDrawWorld()
 	{
 		int i;
 
-		for (  i=0;i<m_collisionObjects.size();i++)
-		{
-			btCollisionObject* colObj = m_collisionObjects[i];
-			if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
-			{
-				btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
-				switch(colObj->getActivationState())
-				{
-				case  ACTIVE_TAG:
-					color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
-				case ISLAND_SLEEPING:
-					color =  btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
-				case WANTS_DEACTIVATION:
-					color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
-				case DISABLE_DEACTIVATION:
-					color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
-				case DISABLE_SIMULATION:
-					color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
-				default:
-					{
-						color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
-					}
-				};
-
-				debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
-			}
-			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
-			{
-				btVector3 minAabb,maxAabb;
-				btVector3 colorvec(1,0,0);
-				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
-				m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
-			}
-
-		}
-	
 		if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
 		{
 			for (i=0;i<m_actions.size();i++)
@@ -929,283 +865,6 @@ void	btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
 
 	
 
-class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
-{
-	btIDebugDraw*	m_debugDrawer;
-	btVector3	m_color;
-	btTransform	m_worldTrans;
-
-public:
-
-	DebugDrawcallback(btIDebugDraw*	debugDrawer,const btTransform& worldTrans,const btVector3& color) :
-                m_debugDrawer(debugDrawer),
-		m_color(color),
-		m_worldTrans(worldTrans)
-	{
-	}
-
-	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
-	{
-		processTriangle(triangle,partId,triangleIndex);
-	}
-
-	virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
-	{
-		(void)partId;
-		(void)triangleIndex;
-
-		btVector3 wv0,wv1,wv2;
-		wv0 = m_worldTrans*triangle[0];
-		wv1 = m_worldTrans*triangle[1];
-		wv2 = m_worldTrans*triangle[2];
-		m_debugDrawer->drawLine(wv0,wv1,m_color);
-		m_debugDrawer->drawLine(wv1,wv2,m_color);
-		m_debugDrawer->drawLine(wv2,wv0,m_color);
-	}
-};
-
-void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
-{
-	btVector3 start = transform.getOrigin();
-
-	const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
-	const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
-	const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
-
-	// XY 
-	getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
-	getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
-	getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
-	getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
-
-	// XZ
-	getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
-	getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
-	getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
-	getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
-
-	// YZ
-	getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
-	getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
-	getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
-	getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
-}
-
-void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
-{
-	// Draw a small simplex at the center of the object
-	{
-		btVector3 start = worldTransform.getOrigin();
-		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
-		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
-		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
-	}
-
-	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
-	{
-		const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
-		for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
-		{
-			btTransform childTrans = compoundShape->getChildTransform(i);
-			const btCollisionShape* colShape = compoundShape->getChildShape(i);
-			debugDrawObject(worldTransform*childTrans,colShape,color);
-		}
-
-	} else
-	{
-		switch (shape->getShapeType())
-		{
-
-		case SPHERE_SHAPE_PROXYTYPE:
-			{
-				const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
-				btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
-				
-				debugDrawSphere(radius, worldTransform, color);
-				break;
-			}
-		case MULTI_SPHERE_SHAPE_PROXYTYPE:
-			{
-				const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
-
-				btTransform childTransform;
-				childTransform.setIdentity();
-
-				for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
-				{
-					childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
-					debugDrawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
-				}
-
-				break;
-			}
-		case CAPSULE_SHAPE_PROXYTYPE:
-			{
-				const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
-
-				btScalar radius = capsuleShape->getRadius();
-				btScalar halfHeight = capsuleShape->getHalfHeight();
-				
-				int upAxis = capsuleShape->getUpAxis();
-
-				
-				btVector3 capStart(0.f,0.f,0.f);
-				capStart[upAxis] = -halfHeight;
-
-				btVector3 capEnd(0.f,0.f,0.f);
-				capEnd[upAxis] = halfHeight;
-
-				// Draw the ends
-				{
-					
-					btTransform childTransform = worldTransform;
-					childTransform.getOrigin() = worldTransform * capStart;
-					debugDrawSphere(radius, childTransform, color);
-				}
-
-				{
-					btTransform childTransform = worldTransform;
-					childTransform.getOrigin() = worldTransform * capEnd;
-					debugDrawSphere(radius, childTransform, color);
-				}
-
-				// Draw some additional lines
-				btVector3 start = worldTransform.getOrigin();
-
-				
-				capStart[(upAxis+1)%3] = radius;
-				capEnd[(upAxis+1)%3] = radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-				capStart[(upAxis+1)%3] = -radius;
-				capEnd[(upAxis+1)%3] = -radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-
-				capStart[(upAxis+1)%3] = 0.f;
-				capEnd[(upAxis+1)%3] = 0.f;
-
-				capStart[(upAxis+2)%3] = radius;
-				capEnd[(upAxis+2)%3] = radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-				capStart[(upAxis+2)%3] = -radius;
-				capEnd[(upAxis+2)%3] = -radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-
-				
-				break;
-			}
-		case CONE_SHAPE_PROXYTYPE:
-			{
-				const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
-				btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
-				btScalar height = coneShape->getHeight();//+coneShape->getMargin();
-				btVector3 start = worldTransform.getOrigin();
-
-				int upAxis= coneShape->getConeUpIndex();
-				
-
-				btVector3	offsetHeight(0,0,0);
-				offsetHeight[upAxis] = height * btScalar(0.5);
-				btVector3	offsetRadius(0,0,0);
-				offsetRadius[(upAxis+1)%3] = radius;
-				btVector3	offset2Radius(0,0,0);
-				offset2Radius[(upAxis+2)%3] = radius;
-
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
-
-
-
-				break;
-
-			}
-		case CYLINDER_SHAPE_PROXYTYPE:
-			{
-				const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
-				int upAxis = cylinder->getUpAxis();
-				btScalar radius = cylinder->getRadius();
-				btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
-				btVector3 start = worldTransform.getOrigin();
-				btVector3	offsetHeight(0,0,0);
-				offsetHeight[upAxis] = halfHeight;
-				btVector3	offsetRadius(0,0,0);
-				offsetRadius[(upAxis+1)%3] = radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
-				break;
-			}
-
-			case STATIC_PLANE_PROXYTYPE:
-				{
-					const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
-					btScalar planeConst = staticPlaneShape->getPlaneConstant();
-					const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
-					btVector3 planeOrigin = planeNormal * planeConst;
-					btVector3 vec0,vec1;
-					btPlaneSpace1(planeNormal,vec0,vec1);
-					btScalar vecLen = 100.f;
-					btVector3 pt0 = planeOrigin + vec0*vecLen;
-					btVector3 pt1 = planeOrigin - vec0*vecLen;
-					btVector3 pt2 = planeOrigin + vec1*vecLen;
-					btVector3 pt3 = planeOrigin - vec1*vecLen;
-					getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
-					getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
-					break;
-
-				}
-		default:
-			{
-
-				if (shape->isConcave())
-				{
-					btConcaveShape* concaveMesh = (btConcaveShape*) shape;
-					
-					///@todo pass camera, for some culling? no -> we are not a graphics lib
-					btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-					btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
-
-					DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
-					concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
-
-				}
-
-				if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
-				{
-					btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
-					//todo: pass camera for some culling			
-					btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-					btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
-					//DebugDrawcallback drawCallback;
-					DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
-					convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
-				}
-
-
-				/// for polyhedral shapes
-				if (shape->isPolyhedral())
-				{
-					btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
-
-					int i;
-					for (i=0;i<polyshape->getNumEdges();i++)
-					{
-						btVector3 a,b;
-						polyshape->getEdge(i,a,b);
-						btVector3 wa = worldTransform * a;
-						btVector3 wb = worldTransform * b;
-						getDebugDrawer()->drawLine(wa,wb,color);
-
-					}
-
-					
-				}
-			}
-		}
-	}
-}
-
-
 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 {
 	bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index 23284a9b7..1231d9fc8 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -77,8 +77,7 @@ protected:
 
 	virtual void	saveKinematicState(btScalar timeStep);
 
-	void	debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
-
+	
 
 public:
 
@@ -135,7 +134,6 @@ public:
 	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
 	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
 
-	void	debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
 
 	void	debugDrawConstraint(btTypedConstraint* constraint);