From 93d1d24234f8d6bfc51b4e2b8951cdb2573fcb9a Mon Sep 17 00:00:00 2001
From: "erwin.coumans" <erwin.coumans@08e121b0-ae19-0410-a57b-3be3395fd4fd>
Date: Wed, 10 Sep 2008 05:20:04 +0000
Subject: [PATCH] Some changes in rendering, to get shadows for trimeshes Add
 dynamic aabb tree (btDbvt) optimization for
 btCompoundShape/btCompoundCollisionAlgorithm Add btTransformAabb util, todo:
 deploy it throughout the codebase

---
 .../ConvexDecompositionDemo.cpp               |  57 +-
 Demos/OpenGL/DemoApplication.cpp              | 740 +++++++++---------
 Demos/OpenGL/GL_ShapeDrawer.cpp               |  14 +-
 .../btCompoundCollisionAlgorithm.cpp          | 211 ++++-
 .../btCompoundCollisionAlgorithm.h            |   4 +-
 .../CollisionShapes/btBoxShape.cpp            |  15 +-
 .../CollisionShapes/btCompoundShape.cpp       | 213 ++---
 .../CollisionShapes/btCompoundShape.h         |  34 +-
 .../CollisionShapes/btPolyhedralConvexShape.h |  24 +-
 src/LinearMath/btAabbUtil2.h                  |  33 +
 10 files changed, 773 insertions(+), 572 deletions(-)

diff --git a/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp b/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
index 4d6f4710b..312f940d3 100644
--- a/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
+++ b/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
@@ -45,7 +45,7 @@ subject to the following restrictions:
 #include "GlutStuff.h"
 
 
-btVector3	centroid;
+btVector3	centroid=btVector3(0,0,0);
 btVector3   convexDecompositionObjectOffset(10,0,0);
 
 #define CUBE_HALF_EXTENTS 4
@@ -100,6 +100,7 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
 #endif//USE_PARALLEL_DISPATCHER
 
 
+	convexDecompositionObjectOffset.setValue(10,0,0);
 
 	btVector3 worldAabbMin(-10000,-10000,-10000);
 	btVector3 worldAabbMax(10000,10000,10000);
@@ -124,10 +125,13 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
 
 	class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
 	{
-
 		ConvexDecompositionDemo*	m_convexDemo;
+		
 		public:
 
+		btAlignedObjectArray<btConvexHullShape*> m_convexShapes;
+		btAlignedObjectArray<btVector3> m_convexCentroids;
+
 		MyConvexDecomposition (FILE* outputFile,ConvexDecompositionDemo* demo)
 			:m_convexDemo(demo),
 				mBaseCount(0),
@@ -162,6 +166,7 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
 
 					//calc centroid, to shift vertices around center of mass
 					centroid.setValue(0,0,0);
+
 					btAlignedObjectArray<btVector3> vertices;
 					if ( 1 )
 					{
@@ -245,29 +250,17 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
 					btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,shiftedVertices);
 
 					
-					btCollisionShape* convexShape = new btConvexHullShape(&(shiftedVertices[0].getX()),shiftedVertices.size());
+					btConvexHullShape* convexShape = new btConvexHullShape(&(shiftedVertices[0].getX()),shiftedVertices.size());
 					
 #else //SHRINK_OBJECT_INWARDS
 					
-#ifdef USE_PARALLEL_DISPATCHER
-					//SPU/multi threaded version only supports convex hull with contiguous vertices at the moment
-					btCollisionShape* convexShape = new btConvexHullShape(&(vertices[0].getX()),vertices.size());
-#else
-					btCollisionShape* convexShape = new btConvexTriangleMeshShape(trimesh);
-#endif //USE_PARALLEL_DISPATCHER
-
+					btConvexHullShape* convexShape = new btConvexHullShape(&(vertices[0].getX()),vertices.size());
 #endif 
 
 					convexShape->setMargin(0.01);
-
+					m_convexShapes.push_back(convexShape);
+					m_convexCentroids.push_back(centroid);
 					m_convexDemo->m_collisionShapes.push_back(convexShape);
-
-					btTransform trans;
-					trans.setIdentity();
-					trans.setOrigin(centroid-convexDecompositionObjectOffset);
-					
-					//btRigidBody* body = m_convexDemo->localCreateRigidBody( mass, trans,convexShape);
-					m_convexDemo->localCreateRigidBody( mass, trans,convexShape);
 					mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
 
 
@@ -394,6 +387,34 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
 
 		ConvexBuilder cb(desc.mCallback);
 		cb.process(desc);
+		//now create some bodies
+		
+		{
+			btCompoundShape* compound = new btCompoundShape();
+			m_collisionShapes.push_back (compound);
+
+			btTransform trans;
+			trans.setIdentity();
+			for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
+			{
+				
+				btVector3 centroid = convexDecomposition.m_convexCentroids[i];
+				trans.setOrigin(centroid);
+				btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
+				compound->addChildShape(trans,convexShape);
+			}
+			btScalar mass=10.f;
+			trans.setOrigin(-convexDecompositionObjectOffset);
+			localCreateRigidBody( mass, trans,compound);
+			convexDecompositionObjectOffset.setZ(6);
+			trans.setOrigin(-convexDecompositionObjectOffset);
+			localCreateRigidBody( mass, trans,compound);
+			convexDecompositionObjectOffset.setZ(-6);
+			trans.setOrigin(-convexDecompositionObjectOffset);
+			localCreateRigidBody( mass, trans,compound);
+			
+		}
+
 		
 		if (outputFile)
 			fclose(outputFile);
diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp
index f4d74d86b..0237e0f11 100644
--- a/Demos/OpenGL/DemoApplication.cpp
+++ b/Demos/OpenGL/DemoApplication.cpp
@@ -51,29 +51,29 @@ extern int gTotalBytesAlignedAllocs;
 
 
 DemoApplication::DemoApplication()
-		//see btIDebugDraw.h for modes
+//see btIDebugDraw.h for modes
 :
 m_dynamicsWorld(0),
 m_pickConstraint(0),
 m_shootBoxShape(0),
-	m_cameraDistance(15.0),
-	m_debugMode(0),
-	m_ele(20.f),
-	m_azi(0.f),
-	m_cameraPosition(0.f,0.f,0.f),
-	m_cameraTargetPosition(0.f,0.f,0.f),
-	m_scaleBottom(0.5f),
-	m_scaleFactor(2.f),
-	m_cameraUp(0,1,0),
-	m_forwardAxis(2),	
-	m_glutScreenWidth(0),
-	m_glutScreenHeight(0),
-	m_ShootBoxInitialSpeed(40.f),
-	m_stepping(true),
-	m_singleStep(false),
-	m_idle(false),
-	m_enableshadows(false),
-	m_sundirection(btVector3(1,-2,1)*1000)
+m_cameraDistance(15.0),
+m_debugMode(0),
+m_ele(20.f),
+m_azi(0.f),
+m_cameraPosition(0.f,0.f,0.f),
+m_cameraTargetPosition(0.f,0.f,0.f),
+m_scaleBottom(0.5f),
+m_scaleFactor(2.f),
+m_cameraUp(0,1,0),
+m_forwardAxis(2),	
+m_glutScreenWidth(0),
+m_glutScreenHeight(0),
+m_ShootBoxInitialSpeed(40.f),
+m_stepping(true),
+m_singleStep(false),
+m_idle(false),
+m_enableshadows(false),
+m_sundirection(btVector3(1,-2,1)*1000)
 {
 #ifndef BT_NO_PROFILE
 	m_profileIterator = CProfileManager::Get_Iterator();
@@ -100,36 +100,36 @@ DemoApplication::~DemoApplication()
 void DemoApplication::myinit(void)
 {
 
-    GLfloat light_ambient[] = { 0.2, 0.2, 0.2, 1.0 };
-    GLfloat light_diffuse[] = { 1.0, 1.0, 1.0, 1.0 };
-    GLfloat light_specular[] = { 1.0, 1.0, 1.0, 1.0 };
-    /*	light_position is NOT default value	*/
-    GLfloat light_position0[] = { 1.0, 10.0, 1.0, 0.0 };
-    GLfloat light_position1[] = { -1.0, -10.0, -1.0, 0.0 };
-  
-    glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
-    glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
-    glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
-    glLightfv(GL_LIGHT0, GL_POSITION, light_position0);
-  
-    glLightfv(GL_LIGHT1, GL_AMBIENT, light_ambient);
-    glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse);
-    glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular);
-    glLightfv(GL_LIGHT1, GL_POSITION, light_position1);
+	GLfloat light_ambient[] = { 0.2, 0.2, 0.2, 1.0 };
+	GLfloat light_diffuse[] = { 1.0, 1.0, 1.0, 1.0 };
+	GLfloat light_specular[] = { 1.0, 1.0, 1.0, 1.0 };
+	/*	light_position is NOT default value	*/
+	GLfloat light_position0[] = { 1.0, 10.0, 1.0, 0.0 };
+	GLfloat light_position1[] = { -1.0, -10.0, -1.0, 0.0 };
 
-    glEnable(GL_LIGHTING);
-    glEnable(GL_LIGHT0);
-    glEnable(GL_LIGHT1);
- 
+	glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
+	glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
+	glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
+	glLightfv(GL_LIGHT0, GL_POSITION, light_position0);
 
-    glShadeModel(GL_SMOOTH);
-    glEnable(GL_DEPTH_TEST);
-    glDepthFunc(GL_LESS);
+	glLightfv(GL_LIGHT1, GL_AMBIENT, light_ambient);
+	glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse);
+	glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular);
+	glLightfv(GL_LIGHT1, GL_POSITION, light_position1);
 
-		glClearColor(0.7,0.7,0.7,0);
+	glEnable(GL_LIGHTING);
+	glEnable(GL_LIGHT0);
+	glEnable(GL_LIGHT1);
 
-    //  glEnable(GL_CULL_FACE);
-    //  glCullFace(GL_BACK);
+
+	glShadeModel(GL_SMOOTH);
+	glEnable(GL_DEPTH_TEST);
+	glDepthFunc(GL_LESS);
+
+	glClearColor(0.7,0.7,0.7,0);
+
+	//  glEnable(GL_CULL_FACE);
+	//  glCullFace(GL_BACK);
 }
 
 
@@ -146,12 +146,12 @@ float	DemoApplication::getCameraDistance()
 
 
 void DemoApplication::toggleIdle() {
-    if (m_idle) {
-        m_idle = false;
-    }
-    else {
-        m_idle = true;
-    }
+	if (m_idle) {
+		m_idle = false;
+	}
+	else {
+		m_idle = true;
+	}
 }
 
 
@@ -159,12 +159,12 @@ void DemoApplication::toggleIdle() {
 
 void DemoApplication::updateCamera() {
 
-	
+
 	glMatrixMode(GL_PROJECTION);
 	glLoadIdentity();
 	float rele = m_ele * 0.01745329251994329547;// rads per deg
 	float razi = m_azi * 0.01745329251994329547;// rads per deg
-	
+
 
 	btQuaternion rot(m_cameraUp,razi);
 
@@ -198,11 +198,11 @@ void DemoApplication::updateCamera() {
 	}
 
 
-    glMatrixMode(GL_MODELVIEW);
+	glMatrixMode(GL_MODELVIEW);
 	glLoadIdentity();
 	gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2], 
-              m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2], 
-			  m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
+		m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2], 
+		m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
 }
 
 
@@ -235,7 +235,7 @@ void DemoApplication::zoomIn()
 void DemoApplication::zoomOut() 
 { 
 	m_cameraDistance += 0.4; updateCamera(); 
-	
+
 }
 
 
@@ -246,12 +246,12 @@ void DemoApplication::zoomOut()
 
 
 
-	
+
 void DemoApplication::reshape(int w, int h) 
 {
 	m_glutScreenWidth = w;
 	m_glutScreenHeight = h;
-	
+
 	glViewport(0, 0, w, h);
 	updateCamera();
 }
@@ -262,23 +262,23 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y)
 	(void)x;
 	(void)y;
 
-		m_lastKey = 0;
+	m_lastKey = 0;
 
 #ifndef BT_NO_PROFILE
-        if (key >= 0x31 && key < 0x37)
-        {
-                int child = key-0x31;
-                m_profileIterator->Enter_Child(child);
-        }
-        if (key==0x30)
-        {
-                m_profileIterator->Enter_Parent();
-        }
+	if (key >= 0x31 && key < 0x37)
+	{
+		int child = key-0x31;
+		m_profileIterator->Enter_Child(child);
+	}
+	if (key==0x30)
+	{
+		m_profileIterator->Enter_Parent();
+	}
 #endif //BT_NO_PROFILE
 
-    switch (key) 
-    {
-    case 'q' : 
+	switch (key) 
+	{
+	case 'q' : 
 #ifdef BT_USE_FREEGLUT
 		//return from glutMainLoop(), detect memory leaks etc.
 		glutLeaveMainLoop();
@@ -287,91 +287,91 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y)
 #endif
 		break;
 
-    case 'l' : stepLeft(); break;
-    case 'r' : stepRight(); break;
-    case 'f' : stepFront(); break;
-    case 'b' : stepBack(); break;
-    case 'z' : zoomIn(); break;
-    case 'x' : zoomOut(); break;
-    case 'i' : toggleIdle(); break;
-    case 'g' : m_enableshadows=!m_enableshadows;break;
-    case 'u' : m_shapeDrawer.enableTexture(!m_shapeDrawer.enableTexture(false));break;
+	case 'l' : stepLeft(); break;
+	case 'r' : stepRight(); break;
+	case 'f' : stepFront(); break;
+	case 'b' : stepBack(); break;
+	case 'z' : zoomIn(); break;
+	case 'x' : zoomOut(); break;
+	case 'i' : toggleIdle(); break;
+	case 'g' : m_enableshadows=!m_enableshadows;break;
+	case 'u' : m_shapeDrawer.enableTexture(!m_shapeDrawer.enableTexture(false));break;
 	case 'h':
-			if (m_debugMode & btIDebugDraw::DBG_NoHelpText)
-				m_debugMode = m_debugMode & (~btIDebugDraw::DBG_NoHelpText);
-			else
-				m_debugMode |= btIDebugDraw::DBG_NoHelpText;
-			break;
+		if (m_debugMode & btIDebugDraw::DBG_NoHelpText)
+			m_debugMode = m_debugMode & (~btIDebugDraw::DBG_NoHelpText);
+		else
+			m_debugMode |= btIDebugDraw::DBG_NoHelpText;
+		break;
 
 	case 'w':
-			if (m_debugMode & btIDebugDraw::DBG_DrawWireframe)
-				m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawWireframe);
-			else
-				m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
-		   break;
+		if (m_debugMode & btIDebugDraw::DBG_DrawWireframe)
+			m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawWireframe);
+		else
+			m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
+		break;
 
-   case 'p':
-	   if (m_debugMode & btIDebugDraw::DBG_ProfileTimings)
-		m_debugMode = m_debugMode & (~btIDebugDraw::DBG_ProfileTimings);
-	else
-		m_debugMode |= btIDebugDraw::DBG_ProfileTimings;
-   break;
+	case 'p':
+		if (m_debugMode & btIDebugDraw::DBG_ProfileTimings)
+			m_debugMode = m_debugMode & (~btIDebugDraw::DBG_ProfileTimings);
+		else
+			m_debugMode |= btIDebugDraw::DBG_ProfileTimings;
+		break;
 
-   case 'm':
-	   if (m_debugMode & btIDebugDraw::DBG_EnableSatComparison)
-		m_debugMode = m_debugMode & (~btIDebugDraw::DBG_EnableSatComparison);
-	else
-		m_debugMode |= btIDebugDraw::DBG_EnableSatComparison;
-   break;
+	case 'm':
+		if (m_debugMode & btIDebugDraw::DBG_EnableSatComparison)
+			m_debugMode = m_debugMode & (~btIDebugDraw::DBG_EnableSatComparison);
+		else
+			m_debugMode |= btIDebugDraw::DBG_EnableSatComparison;
+		break;
 
-   case 'n':
-	   if (m_debugMode & btIDebugDraw::DBG_DisableBulletLCP)
-		m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DisableBulletLCP);
-	else
-		m_debugMode |= btIDebugDraw::DBG_DisableBulletLCP;
-   break;
+	case 'n':
+		if (m_debugMode & btIDebugDraw::DBG_DisableBulletLCP)
+			m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DisableBulletLCP);
+		else
+			m_debugMode |= btIDebugDraw::DBG_DisableBulletLCP;
+		break;
 
 	case 't' : 
-			if (m_debugMode & btIDebugDraw::DBG_DrawText)
-				m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawText);
-			else
-				m_debugMode |= btIDebugDraw::DBG_DrawText;
-		   break;
+		if (m_debugMode & btIDebugDraw::DBG_DrawText)
+			m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawText);
+		else
+			m_debugMode |= btIDebugDraw::DBG_DrawText;
+		break;
 	case 'y':		
-			if (m_debugMode & btIDebugDraw::DBG_DrawFeaturesText)
-				m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawFeaturesText);
-			else
-				m_debugMode |= btIDebugDraw::DBG_DrawFeaturesText;
+		if (m_debugMode & btIDebugDraw::DBG_DrawFeaturesText)
+			m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawFeaturesText);
+		else
+			m_debugMode |= btIDebugDraw::DBG_DrawFeaturesText;
 		break;
 	case 'a':	
 		if (m_debugMode & btIDebugDraw::DBG_DrawAabb)
-				m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawAabb);
-			else
-				m_debugMode |= btIDebugDraw::DBG_DrawAabb;
-			break;
-		case 'c' : 
-			if (m_debugMode & btIDebugDraw::DBG_DrawContactPoints)
-				m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawContactPoints);
-			else
-				m_debugMode |= btIDebugDraw::DBG_DrawContactPoints;
-			break;
+			m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawAabb);
+		else
+			m_debugMode |= btIDebugDraw::DBG_DrawAabb;
+		break;
+	case 'c' : 
+		if (m_debugMode & btIDebugDraw::DBG_DrawContactPoints)
+			m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawContactPoints);
+		else
+			m_debugMode |= btIDebugDraw::DBG_DrawContactPoints;
+		break;
+
+	case 'd' : 
+		if (m_debugMode & btIDebugDraw::DBG_NoDeactivation)
+			m_debugMode = m_debugMode & (~btIDebugDraw::DBG_NoDeactivation);
+		else
+			m_debugMode |= btIDebugDraw::DBG_NoDeactivation;
+		if (m_debugMode & btIDebugDraw::DBG_NoDeactivation)
+		{
+			gDisableDeactivation = true;
+		} else
+		{
+			gDisableDeactivation = false;
+		}
+		break;
+
 
-		case 'd' : 
-			if (m_debugMode & btIDebugDraw::DBG_NoDeactivation)
-				m_debugMode = m_debugMode & (~btIDebugDraw::DBG_NoDeactivation);
-			else
-				m_debugMode |= btIDebugDraw::DBG_NoDeactivation;
-			if (m_debugMode & btIDebugDraw::DBG_NoDeactivation)
-			{
-				gDisableDeactivation = true;
-			} else
-			{
-				gDisableDeactivation = false;
-			}
-			break;
-			
 
-		
 
 	case 'o' :
 		{
@@ -379,10 +379,10 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y)
 			break;
 		}
 	case 's' : clientMoveAndDisplay(); break;
-//    case ' ' : newRandom(); break;
+		//    case ' ' : newRandom(); break;
 	case ' ':
 		clientResetScene();
-			break;
+		break;
 	case '1':
 		{
 			if (m_debugMode & btIDebugDraw::DBG_EnableCCD)
@@ -392,27 +392,27 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y)
 			break;
 		}
 
-		case '.':
+	case '.':
 		{
 			shootBox(getCameraTargetPosition());
 			break;
 		}
 
-		case '+':
+	case '+':
 		{
 			m_ShootBoxInitialSpeed += 10.f;
 			break;
 		}
-		case '-':
+	case '-':
 		{
 			m_ShootBoxInitialSpeed -= 10.f;
 			break;
 		}
 
-    default:
-//        std::cout << "unused key : " << key << std::endl;
-        break;
-    }
+	default:
+		//        std::cout << "unused key : " << key << std::endl;
+		break;
+	}
 
 	if (getDynamicsWorld() && getDynamicsWorld()->getDebugDrawer())
 		getDynamicsWorld()->getDebugDrawer()->setDebugMode(m_debugMode);
@@ -420,7 +420,7 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y)
 	glutPostRedisplay();
 
 }
-	
+
 void	DemoApplication::setDebugMode(int mode)
 {
 	m_debugMode = mode;
@@ -432,7 +432,7 @@ void	DemoApplication::setDebugMode(int mode)
 void DemoApplication::specialKeyboardUp(int key, int x, int y)
 {
 
-		glutPostRedisplay();
+	glutPostRedisplay();
 
 }
 
@@ -441,8 +441,8 @@ void DemoApplication::specialKeyboard(int key, int x, int y)
 	(void)x;
 	(void)y;
 
-    switch (key) 
-    {
+	switch (key) 
+	{
 	case GLUT_KEY_F1:
 		{
 
@@ -450,10 +450,10 @@ void DemoApplication::specialKeyboard(int key, int x, int y)
 		}
 
 	case GLUT_KEY_F2:
-	{
+		{
 
-		break;
-	}
+			break;
+		}
 
 
 	case GLUT_KEY_END:
@@ -462,7 +462,7 @@ void DemoApplication::specialKeyboard(int key, int x, int y)
 			if (numObj)
 			{
 				btCollisionObject* obj = getDynamicsWorld()->getCollisionObjectArray()[numObj-1];
-				
+
 				getDynamicsWorld()->removeCollisionObject(obj);
 				btRigidBody* body = btRigidBody::upcast(obj);
 				if (body && body->getMotionState())
@@ -470,22 +470,22 @@ void DemoApplication::specialKeyboard(int key, int x, int y)
 					delete body->getMotionState();					
 				}
 				delete obj;
-				
+
 
 			}
 			break;
 		}
-    case GLUT_KEY_LEFT : stepLeft(); break;
-    case GLUT_KEY_RIGHT : stepRight(); break;
-    case GLUT_KEY_UP : stepFront(); break;
-    case GLUT_KEY_DOWN : stepBack(); break;
-    case GLUT_KEY_PAGE_UP : zoomIn(); break;
-    case GLUT_KEY_PAGE_DOWN : zoomOut(); break;
-    case GLUT_KEY_HOME : toggleIdle(); break;
-    default:
-//        std::cout << "unused (special) key : " << key << std::endl;
-        break;
-    }
+	case GLUT_KEY_LEFT : stepLeft(); break;
+	case GLUT_KEY_RIGHT : stepRight(); break;
+	case GLUT_KEY_UP : stepFront(); break;
+	case GLUT_KEY_DOWN : stepBack(); break;
+	case GLUT_KEY_PAGE_UP : zoomIn(); break;
+	case GLUT_KEY_PAGE_DOWN : zoomOut(); break;
+	case GLUT_KEY_HOME : toggleIdle(); break;
+	default:
+		//        std::cout << "unused (special) key : " << key << std::endl;
+		break;
+	}
 
 	glutPostRedisplay();
 
@@ -501,7 +501,7 @@ void DemoApplication::moveAndDisplay()
 
 
 
-	
+
 void DemoApplication::displayCallback()
 {
 }
@@ -522,12 +522,12 @@ void	DemoApplication::shootBox(const btVector3& destination)
 
 		if (!m_shootBoxShape)
 		{
-		//#define TEST_UNIFORM_SCALING_SHAPE 1
+			//#define TEST_UNIFORM_SCALING_SHAPE 1
 #ifdef TEST_UNIFORM_SCALING_SHAPE
-		btConvexShape* childShape = new btBoxShape(btVector3(1.f,1.f,1.f));
-		m_shootBoxShape = new btUniformScalingShape(childShape,0.5f);
+			btConvexShape* childShape = new btBoxShape(btVector3(1.f,1.f,1.f));
+			m_shootBoxShape = new btUniformScalingShape(childShape,0.5f);
 #else
-		m_shootBoxShape = new btBoxShape(btVector3(1.f,1.f,1.f));
+			m_shootBoxShape = new btBoxShape(btVector3(1.f,1.f,1.f));
 #endif//
 		}
 
@@ -555,7 +555,7 @@ btRigidBody* pickedBody = 0;//for deactivation state
 btVector3	DemoApplication::getRayTo(int x,int y)
 {
 
-		float top = 1.f;
+	float top = 1.f;
 	float bottom = -1.f;
 	float nearPlane = 1.f;
 	float tanFov = (top-bottom)*0.5f / nearPlane;
@@ -577,7 +577,7 @@ btVector3	DemoApplication::getRayTo(int x,int y)
 	vertical.normalize();
 
 	float tanfov = tanf(0.5f*fov);
-	
+
 	btScalar aspect = m_glutScreenHeight / (btScalar)m_glutScreenWidth;
 
 	hor *= 2.f * farPlane * tanfov;
@@ -594,7 +594,7 @@ btVector3	DemoApplication::getRayTo(int x,int y)
 	btVector3 rayToCenter = rayFrom + rayForward;
 	btVector3 dHor = hor * 1.f/float(m_glutScreenWidth);
 	btVector3 dVert = vertical * 1.f/float(m_glutScreenHeight);
-	
+
 
 	btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
 	rayTo += x * dHor;
@@ -629,7 +629,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 
 			if (state==0)
 			{
-				
+
 
 				//apply an impulse
 				if (m_dynamicsWorld)
@@ -638,22 +638,22 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 					m_dynamicsWorld->rayTest(m_cameraPosition,rayTo,rayCallback);
 					if (rayCallback.hasHit())
 					{
-						
+
 						btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
 						if (body)
 						{
-								body->setActivationState(ACTIVE_TAG);
-								btVector3 impulse = rayTo;
-								impulse.normalize();
-								float impulseStrength = 10.f;
-								impulse *= impulseStrength;
-								btVector3 relPos = rayCallback.m_hitPointWorld - body->getCenterOfMassPosition();
-								body->applyImpulse(impulse,relPos);
-							}
+							body->setActivationState(ACTIVE_TAG);
+							btVector3 impulse = rayTo;
+							impulse.normalize();
+							float impulseStrength = 10.f;
+							impulse *= impulseStrength;
+							btVector3 relPos = rayCallback.m_hitPointWorld - body->getCenterOfMassPosition();
+							body->applyImpulse(impulse,relPos);
+						}
 					}
 				}
 
-				
+
 
 			} else
 			{
@@ -665,7 +665,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 		{
 			if (state==0)
 			{
-		
+
 
 				//add a point to point constraint for picking
 				if (m_dynamicsWorld)
@@ -674,8 +674,8 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 					m_dynamicsWorld->rayTest(m_cameraPosition,rayTo,rayCallback);
 					if (rayCallback.hasHit())
 					{
-						
-						
+
+
 						btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
 						if (body)
 						{
@@ -685,7 +685,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 								pickedBody = body;
 								pickedBody->setActivationState(DISABLE_DEACTIVATION);
 
-								
+
 								btVector3 pickPos = rayCallback.m_hitPointWorld;
 
 								btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
@@ -695,7 +695,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 
 								m_dynamicsWorld->addConstraint(p2p);
 								m_pickConstraint = p2p;
-								
+
 								//save mouse position for dragging
 								gOldPickingPos = rayTo;
 
@@ -724,7 +724,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 					pickedBody = 0;
 				}
 
-				
+
 			}
 
 			break;
@@ -760,7 +760,7 @@ void	DemoApplication::mouseMotionFunc(int x,int y)
 
 	}
 
-	
+
 }
 
 
@@ -790,7 +790,7 @@ btRigidBody*	DemoApplication::localCreateRigidBody(float mass, const btTransform
 #endif//
 
 	m_dynamicsWorld->addRigidBody(body);
-	
+
 	return body;
 }
 
@@ -847,7 +847,7 @@ void DemoApplication::showProfileInfo(float& xOffset,float& yStart, float yIncr)
 	static double time_since_reset = 0.f;
 	if (!m_idle)
 	{
-			time_since_reset = CProfileManager::Get_Time_Since_Reset();
+		time_since_reset = CProfileManager::Get_Time_Since_Reset();
 	}
 
 
@@ -857,7 +857,7 @@ void DemoApplication::showProfileInfo(float& xOffset,float& yStart, float yIncr)
 		char blockTime[128];
 
 		double totalTime = 0;
-		
+
 		int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset();
 
 		m_profileIterator->First();
@@ -916,57 +916,64 @@ void DemoApplication::showProfileInfo(float& xOffset,float& yStart, float yIncr)
 //
 void	DemoApplication::renderscene(int pass)
 {
-btScalar	m[16];
-btMatrix3x3	rot;rot.setIdentity();
-const int	numObjects=m_dynamicsWorld->getNumCollisionObjects();
-btVector3 wireColor(1,0,0);
-for(int i=0;i<numObjects;i++)
+	btScalar	m[16];
+	btMatrix3x3	rot;rot.setIdentity();
+	const int	numObjects=m_dynamicsWorld->getNumCollisionObjects();
+	btVector3 wireColor(1,0,0);
+	for(int i=0;i<numObjects;i++)
 	{
-	btCollisionObject*	colObj=m_dynamicsWorld->getCollisionObjectArray()[i];
-	btRigidBody*		body=btRigidBody::upcast(colObj);
-	if(body&&body->getMotionState())
+		btCollisionObject*	colObj=m_dynamicsWorld->getCollisionObjectArray()[i];
+		btRigidBody*		body=btRigidBody::upcast(colObj);
+		if(body&&body->getMotionState())
 		{
-		btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
-		myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);
-		rot=myMotionState->m_graphicsWorldTrans.getBasis();
+			btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
+			myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);
+			rot=myMotionState->m_graphicsWorldTrans.getBasis();
 		}
 		else
 		{
-		colObj->getWorldTransform().getOpenGLMatrix(m);
-		rot=colObj->getWorldTransform().getBasis();
+			colObj->getWorldTransform().getOpenGLMatrix(m);
+			rot=colObj->getWorldTransform().getBasis();
 		}
-	btVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation
-	if(i&1) wireColor=btVector3(0.f,0.0f,1.f);
-	///color differently for active, sleeping, wantsdeactivation states
-	if (colObj->getActivationState() == 1) //active
+		btVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation
+		if(i&1) wireColor=btVector3(0.f,0.0f,1.f);
+		///color differently for active, sleeping, wantsdeactivation states
+		if (colObj->getActivationState() == 1) //active
 		{
-		if (i & 1)
+			if (i & 1)
 			{
-			wireColor += btVector3 (1.f,0.f,0.f);
+				wireColor += btVector3 (1.f,0.f,0.f);
 			}
 			else
 			{			
-			wireColor += btVector3 (.5f,0.f,0.f);
+				wireColor += btVector3 (.5f,0.f,0.f);
 			}
 		}
-	if(colObj->getActivationState()==2) //ISLAND_SLEEPING
+		if(colObj->getActivationState()==2) //ISLAND_SLEEPING
 		{
-		if(i&1)
+			if(i&1)
 			{
-			wireColor += btVector3 (0.f,1.f, 0.f);
+				wireColor += btVector3 (0.f,1.f, 0.f);
 			}
 			else
 			{
-			wireColor += btVector3 (0.f,0.5f,0.f);
+				wireColor += btVector3 (0.f,0.5f,0.f);
 			}
 		}
 
-	btVector3 aabbMin,aabbMax;
-	m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax);
+		btVector3 aabbMin,aabbMax;
+		m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax);
+		
+		aabbMin-=btVector3(1e30,1e30,1e30);
+		aabbMax+=btVector3(1e30,1e30,1e30);
+//		printf("aabbMin=(%f,%f,%f)\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ());
+//		printf("aabbMax=(%f,%f,%f)\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ());
+//		m_dynamicsWorld->getDebugDrawer()->drawAabb(aabbMin,aabbMax,btVector3(1,1,1));
 
-	switch(pass)
+
+		switch(pass)
 		{
-	case	0:	m_shapeDrawer.drawOpenGL(m,colObj->getCollisionShape(),wireColor,getDebugMode(),aabbMin,aabbMax);break;
+		case	0:	m_shapeDrawer.drawOpenGL(m,colObj->getCollisionShape(),wireColor,getDebugMode(),aabbMin,aabbMax);break;
 		case	1:	m_shapeDrawer.drawShadow(m,m_sundirection*rot,colObj->getCollisionShape(),aabbMin,aabbMax);break;
 		case	2:	m_shapeDrawer.drawOpenGL(m,colObj->getCollisionShape(),wireColor*0.3,0,aabbMin,aabbMax);break;
 		}
@@ -983,11 +990,11 @@ void DemoApplication::renderme()
 	if (m_dynamicsWorld)
 	{			
 		if(m_enableshadows)
-			{
+		{
 			glClear(GL_STENCIL_BUFFER_BIT);
 			glEnable(GL_CULL_FACE);
 			renderscene(0);
-			
+
 			glDisable(GL_LIGHTING);
 			glDepthMask(GL_FALSE);
 			glDepthFunc(GL_LEQUAL);
@@ -1001,7 +1008,7 @@ void DemoApplication::renderme()
 			glStencilOp(GL_KEEP,GL_KEEP,GL_DECR);
 			renderscene(1);
 			glFrontFace(GL_CCW);
-			
+
 			glPolygonMode(GL_FRONT,GL_FILL);
 			glPolygonMode(GL_BACK,GL_FILL);
 			glShadeModel(GL_SMOOTH);
@@ -1013,7 +1020,7 @@ void DemoApplication::renderme()
 			glFrontFace(GL_CCW);
 			glEnable(GL_CULL_FACE);
 			glColorMask(GL_TRUE,GL_TRUE,GL_TRUE,GL_TRUE);
-			
+
 			glDepthFunc(GL_LEQUAL);
 			glStencilFunc( GL_NOTEQUAL, 0, 0xFFFFFFFFL );
 			glStencilOp( GL_KEEP, GL_KEEP, GL_KEEP );
@@ -1023,27 +1030,28 @@ void DemoApplication::renderme()
 			glDepthFunc(GL_LESS);
 			glDisable(GL_STENCIL_TEST);
 			glDisable(GL_CULL_FACE);
-			}
-			else
-			{
+		}
+		else
+		{
+			glDisable(GL_CULL_FACE);
 			renderscene(0);
-			}
+		}
 
-			float xOffset = 10.f;
-			float yStart = 20.f;
-			float yIncr = 20.f;
-			char buf[124];
+		float xOffset = 10.f;
+		float yStart = 20.f;
+		float yIncr = 20.f;
+		char buf[124];
 
-			glDisable(GL_LIGHTING);
-			glColor3f(0, 0, 0);
+		glDisable(GL_LIGHTING);
+		glColor3f(0, 0, 0);
 
-			if ((m_debugMode & btIDebugDraw::DBG_NoHelpText)==0)
-				{
-				setOrthographicProjection();
+		if ((m_debugMode & btIDebugDraw::DBG_NoHelpText)==0)
+		{
+			setOrthographicProjection();
 
-				showProfileInfo(xOffset,yStart,yIncr);
+			showProfileInfo(xOffset,yStart,yIncr);
 
-	#ifdef USE_QUICKPROF
+#ifdef USE_QUICKPROF
 
 
 			if ( getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
@@ -1062,141 +1070,141 @@ void DemoApplication::renderme()
 				}
 
 			}
-	#endif //USE_QUICKPROF
-
-			
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"mouse to interact");
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
-
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"space to reset");
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
-			
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"cursor keys and z,x to navigate");
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
-
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"i to toggle simulation, s single step");
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
-
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"q to quit");
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
-
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,". to shoot box");
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
-
-				// not yet hooked up again after refactoring...
-
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"d to toggle deactivation");
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
-
-				
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"g to toggle mesh animation (ConcaveDemo)");
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
-			
-
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"h to toggle help text");
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
-
-				
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"p to toggle profiling (+results to file)");
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
+#endif //USE_QUICKPROF
 
 
-				//bool useBulletLCP = !(getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP);
-				//bool useCCD = (getDebugMode() & btIDebugDraw::DBG_EnableCCD);
-				//glRasterPos3f(xOffset,yStart,0);
-				//sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD);
-				//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				//yStart += yIncr;
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"mouse to interact");
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
+
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"space to reset");
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
+
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"cursor keys and z,x to navigate");
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
+
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"i to toggle simulation, s single step");
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
+
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"q to quit");
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
+
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,". to shoot box");
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
+
+			// not yet hooked up again after refactoring...
+
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"d to toggle deactivation");
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
 
 
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"+- shooting speed = %10.2f",m_ShootBoxInitialSpeed);
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"g to toggle mesh animation (ConcaveDemo)");
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
+
+
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"h to toggle help text");
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
+
+
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"p to toggle profiling (+results to file)");
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
+
+
+			//bool useBulletLCP = !(getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP);
+			//bool useCCD = (getDebugMode() & btIDebugDraw::DBG_EnableCCD);
+			//glRasterPos3f(xOffset,yStart,0);
+			//sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD);
+			//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			//yStart += yIncr;
+
+
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"+- shooting speed = %10.2f",m_ShootBoxInitialSpeed);
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
 
 #ifdef SHOW_NUM_DEEP_PENETRATIONS
-				
 
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"gNumDeepPenetrationChecks = %d",gNumDeepPenetrationChecks);
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
 
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"gNumGjkChecks= %d",gNumGjkChecks);
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"gNumDeepPenetrationChecks = %d",gNumDeepPenetrationChecks);
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
 
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"gNumSplitImpulseRecoveries= %d",gNumSplitImpulseRecoveries);
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
-								
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"gNumAlignedAllocs = %d",gNumAlignedAllocs);
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"gNumGjkChecks= %d",gNumGjkChecks);
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
 
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"gNumAlignedFree= %d",gNumAlignedFree);
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"gNumSplitImpulseRecoveries= %d",gNumSplitImpulseRecoveries);
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
 
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"# alloc-free = %d",gNumAlignedAllocs-gNumAlignedFree);
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"gNumAlignedAllocs = %d",gNumAlignedAllocs);
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
 
-//enable BT_DEBUG_MEMORY_ALLOCATIONS define in Bullet/src/LinearMath/btAlignedAllocator.h for memory leak detection
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"gNumAlignedFree= %d",gNumAlignedFree);
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
+
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"# alloc-free = %d",gNumAlignedAllocs-gNumAlignedFree);
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
+
+			//enable BT_DEBUG_MEMORY_ALLOCATIONS define in Bullet/src/LinearMath/btAlignedAllocator.h for memory leak detection
 #ifdef BT_DEBUG_MEMORY_ALLOCATIONS
-				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"gTotalBytesAlignedAllocs = %d",gTotalBytesAlignedAllocs);
-				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-				yStart += yIncr;
+			glRasterPos3f(xOffset,yStart,0);
+			sprintf(buf,"gTotalBytesAlignedAllocs = %d",gTotalBytesAlignedAllocs);
+			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+			yStart += yIncr;
 #endif //BT_DEBUG_MEMORY_ALLOCATIONS
 
-				if (getDynamicsWorld())
-				{
-					glRasterPos3f(xOffset,yStart,0);
-					sprintf(buf,"# objects = %d",getDynamicsWorld()->getNumCollisionObjects());
-					BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-					yStart += yIncr;
-					glRasterPos3f(xOffset,yStart,0);
-					sprintf(buf,"# pairs = %d",getDynamicsWorld()->getBroadphase()->getOverlappingPairCache()->getNumOverlappingPairs());
-					BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-					yStart += yIncr;
+			if (getDynamicsWorld())
+			{
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"# objects = %d",getDynamicsWorld()->getNumCollisionObjects());
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"# pairs = %d",getDynamicsWorld()->getBroadphase()->getOverlappingPairCache()->getNumOverlappingPairs());
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
 
-				}
+			}
 
 
 #endif //SHOW_NUM_DEEP_PENETRATIONS
 
-				resetPerspectiveProjection();
-			}
+			resetPerspectiveProjection();
+		}
+
+		glEnable(GL_LIGHTING);
 
-			glEnable(GL_LIGHTING);
 
-		
 	}
 
 	updateCamera();
@@ -1216,7 +1224,7 @@ void	DemoApplication::clientResetScene()
 		m_dynamicsWorld->stepSimulation(1.f/60.f,0);
 		numObjects = m_dynamicsWorld->getNumCollisionObjects();
 	}
-	
+
 	for (int i=0;i<numObjects;i++)
 	{
 		btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[i];
@@ -1243,13 +1251,13 @@ void	DemoApplication::clientResetScene()
 			}
 		}
 
-	/*
-	//quickly search some issue at a certain simulation frame, pressing space to reset
+		/*
+		//quickly search some issue at a certain simulation frame, pressing space to reset
 		int fixed=18;
 		for (int i=0;i<fixed;i++)
 		{
-			getDynamicsWorld()->stepSimulation(1./60.f,1);
+		getDynamicsWorld()->stepSimulation(1./60.f,1);
 		}
-	*/
+		*/
 	}
 }
diff --git a/Demos/OpenGL/GL_ShapeDrawer.cpp b/Demos/OpenGL/GL_ShapeDrawer.cpp
index 185e4424b..ee6a98848 100644
--- a/Demos/OpenGL/GL_ShapeDrawer.cpp
+++ b/Demos/OpenGL/GL_ShapeDrawer.cpp
@@ -166,7 +166,7 @@ void OGL_displaylist_register_shape(btCollisionShape * shape)
 
 	glNewList(dlist.m_dlist,GL_COMPILE);
 
-	glEnable(GL_CULL_FACE);
+//	glEnable(GL_CULL_FACE);
 
 	glCullFace(GL_BACK);
 
@@ -177,7 +177,7 @@ void OGL_displaylist_register_shape(btCollisionShape * shape)
 		concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
 	}
 
-	glDisable(GL_CULL_FACE);	
+//	glDisable(GL_CULL_FACE);	
 
 	glEndList();
 }
@@ -237,12 +237,16 @@ public:
 		} else
 		{
 			glBegin(GL_TRIANGLES);
-			glColor3f(1, 1, 1);
+			//glColor3f(1, 1, 1);
+			
+			
 			glVertex3d(triangle[0].getX(), triangle[0].getY(), triangle[0].getZ());
-			//glColor3f(0, 1, 0);
 			glVertex3d(triangle[1].getX(), triangle[1].getY(), triangle[1].getZ());
-			//glColor3f(0, 0, 1);
 			glVertex3d(triangle[2].getX(), triangle[2].getY(), triangle[2].getZ());
+
+			glVertex3d(triangle[2].getX(), triangle[2].getY(), triangle[2].getZ());
+			glVertex3d(triangle[1].getX(), triangle[1].getY(), triangle[1].getZ());
+			glVertex3d(triangle[0].getX(), triangle[0].getY(), triangle[0].getZ());
 			glEnd();
 		}
 	}
diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
index a29a6d624..d26482e03 100644
--- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
@@ -16,12 +16,17 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
-
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btAabbUtil2.h"
 
 btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
 :btCollisionAlgorithm(ci),
-m_isSwapped(isSwapped)
+m_isSwapped(isSwapped),
+m_sharedManifold(ci.m_manifold)
 {
+	m_ownsManifold = false;
+
 	btCollisionObject* colObj = m_isSwapped? body1 : body0;
 	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
 	assert (colObj->getCollisionShape()->isCompound());
@@ -33,11 +38,17 @@ m_isSwapped(isSwapped)
 	m_childCollisionAlgorithms.resize(numChildren);
 	for (i=0;i<numChildren;i++)
 	{
-		btCollisionShape* tmpShape = colObj->getCollisionShape();
-		btCollisionShape* childShape = compoundShape->getChildShape(i);
-		colObj->internalSetTemporaryCollisionShape( childShape );
-		m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj);
-		colObj->internalSetTemporaryCollisionShape( tmpShape );
+		if (compoundShape->getDynamicAabbTree())
+		{
+			m_childCollisionAlgorithms[i] = 0;
+		} else
+		{
+			btCollisionShape* tmpShape = colObj->getCollisionShape();
+			btCollisionShape* childShape = compoundShape->getChildShape(i);
+			colObj->internalSetTemporaryCollisionShape( childShape );
+			m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj,m_sharedManifold);
+			colObj->internalSetTemporaryCollisionShape( tmpShape );
+		}
 	}
 }
 
@@ -48,11 +59,109 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
 	int i;
 	for (i=0;i<numChildren;i++)
 	{
-		m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
-		m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+		if (m_childCollisionAlgorithms[i])
+		{
+			m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
+			m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+		}
 	}
 }
 
+
+
+
+struct	btCompoundLeafCallback : btDbvt::ICollide
+{
+
+public:
+
+	btCollisionObject* m_compoundColObj;
+	btCollisionObject* m_otherObj;
+	btDispatcher* m_dispatcher;
+	const btDispatcherInfo& m_dispatchInfo;
+	btManifoldResult*	m_resultOut;
+	btCollisionAlgorithm**	m_childCollisionAlgorithms;
+	btPersistentManifold*	m_sharedManifold;
+
+
+
+
+	btCompoundLeafCallback (btCollisionObject* compoundObj,btCollisionObject* otherObj,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult*	resultOut,btCollisionAlgorithm**	childCollisionAlgorithms,btPersistentManifold*	sharedManifold)
+		:m_compoundColObj(compoundObj),m_otherObj(otherObj),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
+		m_childCollisionAlgorithms(childCollisionAlgorithms),
+		m_sharedManifold(sharedManifold)
+	{
+
+	}
+
+
+	void	ProcessChildShape(btCollisionShape* childShape,int index)
+	{
+		
+		btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
+
+
+		//backup
+		btTransform	orgTrans = m_compoundColObj->getWorldTransform();
+		btTransform	orgInterpolationTrans = m_compoundColObj->getInterpolationWorldTransform();
+		const btTransform& childTrans = compoundShape->getChildTransform(index);
+		btTransform	newChildWorldTrans = orgTrans*childTrans ;
+
+		//perform an AABB check first
+		btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+		childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+		m_otherObj->getCollisionShape()->getAabb(m_otherObj->getWorldTransform(),aabbMin1,aabbMax1);
+
+		if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+		{
+
+			m_compoundColObj->setWorldTransform( newChildWorldTrans);
+			m_compoundColObj->setInterpolationWorldTransform(newChildWorldTrans);
+
+			//the contactpoint is still projected back using the original inverted worldtrans
+			btCollisionShape* tmpShape = m_compoundColObj->getCollisionShape();
+			m_compoundColObj->internalSetTemporaryCollisionShape( childShape );
+
+			if (!m_childCollisionAlgorithms[index])
+				m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold);
+
+			m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut);
+			if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+			{
+				btVector3 worldAabbMin,worldAabbMax;
+				m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
+				m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
+			}
+			
+			//revert back transform
+			m_compoundColObj->internalSetTemporaryCollisionShape( tmpShape);
+			m_compoundColObj->setWorldTransform(  orgTrans );
+			m_compoundColObj->setInterpolationWorldTransform(orgInterpolationTrans);
+		}
+	}
+	void		Process(const btDbvtNode* leaf)
+	{
+		int index = int(leaf->data);
+
+		btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
+		btCollisionShape* childShape = compoundShape->getChildShape(index);
+		if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+		{
+			btVector3 worldAabbMin,worldAabbMax;
+			btTransform	orgTrans = m_compoundColObj->getWorldTransform();
+			btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
+			m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
+		}
+		ProcessChildShape(childShape,index);
+
+	}
+};
+
+
+
+
+
+
 void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
 	btCollisionObject* colObj = m_isSwapped? body1 : body0;
@@ -61,37 +170,69 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt
 	assert (colObj->getCollisionShape()->isCompound());
 	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
 
-	//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
-	//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
-	//given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
-	//determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
-	//then use each overlapping node AABB against Tree0
-	//and vise versa.
+	btDbvt* tree = compoundShape->getDynamicAabbTree();
+	//use a dynamic aabb tree to cull potential child-overlaps
+	btCompoundLeafCallback  callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
 
-	int numChildren = m_childCollisionAlgorithms.size();
-	int i;
-	for (i=0;i<numChildren;i++)
+
+	if (tree)
 	{
-		//temporarily exchange parent btCollisionShape with childShape, and recurse
-		btCollisionShape* childShape = compoundShape->getChildShape(i);
 
-		//backup
-		btTransform	orgTrans = colObj->getWorldTransform();
-		btTransform	orgInterpolationTrans = colObj->getInterpolationWorldTransform();
+		btVector3 localAabbMin,localAabbMax;
+		btTransform otherInCompoundSpace;
+		otherInCompoundSpace = colObj->getWorldTransform().inverse() * otherObj->getWorldTransform();
+		otherObj->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
 
-		const btTransform& childTrans = compoundShape->getChildTransform(i);
-		btTransform	newChildWorldTrans = orgTrans*childTrans ;
-		colObj->setWorldTransform( newChildWorldTrans);
-		colObj->setInterpolationWorldTransform(newChildWorldTrans);
+		const btDbvtVolume	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+		//process all children, that overlap with  the given AABB bounds
+		tree->collideTV(tree->m_root,bounds,callback);
+
+	} else
+	{
+		//iterate over all children, perform an AABB check inside ProcessChildShape
+		int numChildren = m_childCollisionAlgorithms.size();
+		int i;
+		for (i=0;i<numChildren;i++)
+		{
+			callback.ProcessChildShape(compoundShape->getChildShape(i),i);
+		}
+	}
+
+	{
+				//iterate over all children, perform an AABB check inside ProcessChildShape
+		int numChildren = m_childCollisionAlgorithms.size();
+		int i;
+		btManifoldArray	manifoldArray;
+
+		for (i=0;i<numChildren;i++)
+		{
+			if (m_childCollisionAlgorithms[i])
+			{
+				btCollisionShape* childShape = compoundShape->getChildShape(i);
+			//if not longer overlapping, remove the algorithm
+				btTransform	orgTrans = colObj->getWorldTransform();
+				btTransform	orgInterpolationTrans = colObj->getInterpolationWorldTransform();
+				const btTransform& childTrans = compoundShape->getChildTransform(i);
+				btTransform	newChildWorldTrans = orgTrans*childTrans ;
+
+				//perform an AABB check first
+				btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+				childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+				otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1);
+
+				if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+				{
+					m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
+					m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+					m_childCollisionAlgorithms[i] = 0;
+				}
+
+			}
+			
+		}
+
+		
 
-		//the contactpoint is still projected back using the original inverted worldtrans
-		btCollisionShape* tmpShape = colObj->getCollisionShape();
-		colObj->internalSetTemporaryCollisionShape( childShape );
-		m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut);
-		//revert back
-		colObj->internalSetTemporaryCollisionShape( tmpShape);
-		colObj->setWorldTransform(  orgTrans );
-		colObj->setInterpolationWorldTransform(orgInterpolationTrans);
 	}
 }
 
diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
index 1682c6761..fbae26571 100644
--- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
@@ -28,11 +28,13 @@ class btDispatcher;
 class btDispatcher;
 
 /// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
-/// Place holder, not fully implemented yet
 class btCompoundCollisionAlgorithm  : public btCollisionAlgorithm
 {
 	btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
 	bool m_isSwapped;
+
+	class btPersistentManifold*	m_sharedManifold;
+	bool					m_ownsManifold;
 	
 public:
 
diff --git a/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/src/BulletCollision/CollisionShapes/btBoxShape.cpp
index f3a21d5f7..8bc2cfa6f 100644
--- a/src/BulletCollision/CollisionShapes/btBoxShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btBoxShape.cpp
@@ -21,20 +21,7 @@ subject to the following restrictions:
 
 void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
 {
-	btVector3 halfExtents = getHalfExtentsWithoutMargin();
-	halfExtents += btVector3(getMargin(),getMargin(),getMargin());
-
-
-	btMatrix3x3 abs_b = t.getBasis().absolute();  
-	btPoint3 center = t.getOrigin();
-	btVector3 extent = btVector3(abs_b[0].dot(halfExtents),
-		   abs_b[1].dot(halfExtents),
-		  abs_b[2].dot(halfExtents));
-
-	aabbMin = center - extent;
-	aabbMax = center + extent;
-
-
+	btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
 }
 
 
diff --git a/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/src/BulletCollision/CollisionShapes/btCompoundShape.cpp
index bfd0a122a..f08b810ea 100644
--- a/src/BulletCollision/CollisionShapes/btCompoundShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btCompoundShape.cpp
@@ -14,23 +14,29 @@ subject to the following restrictions:
 */
 
 #include "btCompoundShape.h"
-
-
 #include "btCollisionShape.h"
-
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
 
 btCompoundShape::btCompoundShape()
 :m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)),
 m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)),
-m_aabbTree(0),
 m_collisionMargin(btScalar(0.)),
-m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
+m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
+m_dynamicAabbTree(0)
 {
+	void* mem = btAlignedAlloc(sizeof(btDbvt),16);
+	m_dynamicAabbTree = new(mem) btDbvt();
+	btAssert(mem==m_dynamicAabbTree);
 }
 
 
 btCompoundShape::~btCompoundShape()
 {
+	if (m_dynamicAabbTree)
+	{
+		m_dynamicAabbTree->~btDbvt();
+		btAlignedFree(m_dynamicAabbTree);
+	}
 }
 
 void	btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
@@ -60,68 +66,85 @@ void	btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio
 		}
 
 	}
+	if (m_dynamicAabbTree)
+	{
+		const btDbvtVolume	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+		int index = m_children.size()-1;
+		child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
+	}
+
+}
+
+void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
+{
+	btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
+	if (m_dynamicAabbTree)
+	{
+		m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
+	}
+	m_children.swap(childShapeIndex,m_children.size()-1);
+	m_children.pop_back();
+
 }
 
 void btCompoundShape::removeChildShape(btCollisionShape* shape)
 {
-   bool done_removing;
-   
-   // Find the children containing the shape specified, and remove those children.
-   do
-   {
-      done_removing = true;
-      
-      for(int i = 0; i < m_children.size(); i++)
-      {
-         if(m_children[i].m_childShape == shape)
-         {
-            m_children.remove(m_children[i]);
-            done_removing = false;  // Do another iteration pass after removing from the vector
-            break;
-         }
-      }
-   }
-   while (!done_removing);
-   
-   recalculateLocalAabb();
+	// Find the children containing the shape specified, and remove those children.
+	//note: there might be multiple children using the same shape!
+	for(int i = m_children.size()-1; i >= 0 ; i--)
+	{
+		if(m_children[i].m_childShape == shape)
+		{
+			m_children.swap(i,m_children.size()-1);
+			m_children.pop_back();
+			//remove it from the m_dynamicAabbTree too
+			//m_dynamicAabbTree->remove(m_aabbProxies[i]);
+			//m_aabbProxies.swap(i,m_children.size()-1);
+			//m_aabbProxies.pop_back();
+		}
+	}
+
+
+
+	recalculateLocalAabb();
 }
 
 void btCompoundShape::recalculateLocalAabb()
 {
-   // Recalculate the local aabb
-   // Brute force, it iterates over all the shapes left.
-   m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
-   m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
-   
-   //extend the local aabbMin/aabbMax
-   for (int j = 0; j < m_children.size(); j++)
-   {
-      btVector3 localAabbMin,localAabbMax;
-      m_children[j].m_childShape->getAabb(m_children[j].m_transform, 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];
-      }
-   }
+	// Recalculate the local aabb
+	// Brute force, it iterates over all the shapes left.
+	m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+	m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+
+	//extend the local aabbMin/aabbMax
+	for (int j = 0; j < m_children.size(); j++)
+	{
+		btVector3 localAabbMin,localAabbMax;
+		m_children[j].m_childShape->getAabb(m_children[j].m_transform, 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
+
+///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
 void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
 {
 	btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
 	localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
 	btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
-	
+
 	btMatrix3x3 abs_b = trans.getBasis().absolute();  
 
 	btPoint3 center = trans(localCenter);
 
 	btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
-		   abs_b[1].dot(localHalfExtents),
-		  abs_b[2].dot(localHalfExtents));
+		abs_b[1].dot(localHalfExtents),
+		abs_b[2].dot(localHalfExtents));
 	aabbMin = center-extent;
 	aabbMax = center+extent;
 
@@ -134,9 +157,9 @@ void	btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co
 	ident.setIdentity();
 	btVector3 aabbMin,aabbMax;
 	getAabb(ident,aabbMin,aabbMax);
-	
+
 	btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
-	
+
 	btScalar lx=btScalar(2.)*(halfExtents.x());
 	btScalar ly=btScalar(2.)*(halfExtents.y());
 	btScalar lz=btScalar(2.)*(halfExtents.z());
@@ -152,57 +175,57 @@ void	btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co
 
 void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
 {
-   int n = m_children.size();
+	int n = m_children.size();
 
-   btScalar totalMass = 0;
-   btVector3 center(0, 0, 0);
-   for (int k = 0; k < n; k++)
-   {
-      center += m_children[k].m_transform.getOrigin() * masses[k];
-      totalMass += masses[k];
-   }
-   center /= totalMass;
-   principal.setOrigin(center);
+	btScalar totalMass = 0;
+	btVector3 center(0, 0, 0);
+	for (int k = 0; k < n; k++)
+	{
+		center += m_children[k].m_transform.getOrigin() * masses[k];
+		totalMass += masses[k];
+	}
+	center /= totalMass;
+	principal.setOrigin(center);
 
-   btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
-   for (int k = 0; k < n; k++)
-   {
-      btVector3 i;
-      m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
+	btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	for (int k = 0; k < n; k++)
+	{
+		btVector3 i;
+		m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
 
-      const btTransform& t = m_children[k].m_transform;
-      btVector3 o = t.getOrigin() - center;
-      
-      //compute inertia tensor in coordinate system of compound shape
-      btMatrix3x3 j = t.getBasis().transpose();
-      j[0] *= i[0];
-      j[1] *= i[1];
-      j[2] *= i[2];
-      j = t.getBasis() * j;
-      
-      //add inertia tensor
-      tensor[0] += j[0];
-      tensor[1] += j[1];
-      tensor[2] += j[2];
+		const btTransform& t = m_children[k].m_transform;
+		btVector3 o = t.getOrigin() - center;
 
-      //compute inertia tensor of pointmass at o
-      btScalar o2 = o.length2();
-      j[0].setValue(o2, 0, 0);
-      j[1].setValue(0, o2, 0);
-      j[2].setValue(0, 0, o2);
-      j[0] += o * -o.x(); 
-      j[1] += o * -o.y(); 
-      j[2] += o * -o.z();
+		//compute inertia tensor in coordinate system of compound shape
+		btMatrix3x3 j = t.getBasis().transpose();
+		j[0] *= i[0];
+		j[1] *= i[1];
+		j[2] *= i[2];
+		j = t.getBasis() * j;
 
-      //add inertia tensor of pointmass
-      tensor[0] += masses[k] * j[0];
-      tensor[1] += masses[k] * j[1];
-      tensor[2] += masses[k] * j[2];
-   }
+		//add inertia tensor
+		tensor[0] += j[0];
+		tensor[1] += j[1];
+		tensor[2] += j[2];
 
-   tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
-   inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
+		//compute inertia tensor of pointmass at o
+		btScalar o2 = o.length2();
+		j[0].setValue(o2, 0, 0);
+		j[1].setValue(0, o2, 0);
+		j[2].setValue(0, 0, o2);
+		j[0] += o * -o.x(); 
+		j[1] += o * -o.y(); 
+		j[2] += o * -o.z();
+
+		//add inertia tensor of pointmass
+		tensor[0] += masses[k] * j[0];
+		tensor[1] += masses[k] * j[1];
+		tensor[2] += masses[k] * j[2];
+	}
+
+	tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
+	inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
 }
 
 
-	
+
diff --git a/src/BulletCollision/CollisionShapes/btCompoundShape.h b/src/BulletCollision/CollisionShapes/btCompoundShape.h
index 5a687a7fa..3624749f7 100644
--- a/src/BulletCollision/CollisionShapes/btCompoundShape.h
+++ b/src/BulletCollision/CollisionShapes/btCompoundShape.h
@@ -24,8 +24,8 @@ subject to the following restrictions:
 #include "btCollisionMargin.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
-class btOptimizedBvh;
-
+//class btOptimizedBvh;
+struct btDbvt;
 
 ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild
 {
@@ -35,14 +35,15 @@ ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild
 	btCollisionShape*	m_childShape;
 	int					m_childShapeType;
 	btScalar			m_childMargin;
+	struct btDbvtNode*	m_node;
 };
 
 SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompoundShapeChild& c2)
 {
-   return  ( c1.m_transform      == c2.m_transform &&
-             c1.m_childShape     == c2.m_childShape &&
-             c1.m_childShapeType == c2.m_childShapeType &&
-             c1.m_childMargin    == c2.m_childMargin );
+	return  ( c1.m_transform      == c2.m_transform &&
+		c1.m_childShape     == c2.m_childShape &&
+		c1.m_childShapeType == c2.m_childShapeType &&
+		c1.m_childMargin    == c2.m_childMargin );
 }
 
 /// btCompoundShape allows to store multiple other btCollisionShapes
@@ -55,7 +56,8 @@ ATTRIBUTE_ALIGNED16(class) btCompoundShape	: public btCollisionShape
 	btVector3						m_localAabbMin;
 	btVector3						m_localAabbMax;
 
-	btOptimizedBvh*					m_aabbTree;
+	//btOptimizedBvh*					m_aabbTree;
+	btDbvt*							m_dynamicAabbTree;
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -66,11 +68,11 @@ public:
 
 	void	addChildShape(const btTransform& localTransform,btCollisionShape* shape);
 
-   /** Remove all children shapes that contain the specified shape. */
+	/// Remove all children shapes that contain the specified shape
 	virtual void removeChildShape(btCollisionShape* shape);
 
-	  
-	
+	void removeChildShapeByIndex(int childShapeindex);
+
 
 	int		getNumChildShapes() const
 	{
@@ -103,9 +105,9 @@ public:
 
 	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
 	virtual	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
-	
-   /** Re-calculate the local Aabb. Is called at the end of removeChildShapes. 
-       Use this yourself if you modify the children or their transforms. */
+
+	/** Re-calculate the local Aabb. Is called at the end of removeChildShapes. 
+	Use this yourself if you modify the children or their transforms. */
 	virtual void recalculateLocalAabb(); 
 
 	virtual void	setLocalScaling(const btVector3& scaling)
@@ -118,7 +120,7 @@ public:
 	}
 
 	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
-	
+
 	virtual int	getShapeType() const { return COMPOUND_SHAPE_PROXYTYPE;}
 
 	virtual void	setMargin(btScalar margin)
@@ -137,9 +139,9 @@ public:
 	//this is optional, but should make collision queries faster, by culling non-overlapping nodes
 	void	createAabbTreeFromChildren();
 
-	const btOptimizedBvh*					getAabbTree() const
+	btDbvt*							getDynamicAabbTree()
 	{
-		return m_aabbTree;
+		return m_dynamicAabbTree;
 	}
 
 	///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
index a47dfea14..4c4ce7fea 100644
--- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
+++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
@@ -18,6 +18,7 @@ subject to the following restrictions:
 
 #include "LinearMath/btPoint3.h"
 #include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btAabbUtil2.h"
 #include "btConvexInternalShape.h"
 
 
@@ -46,28 +47,7 @@ public:
 
 		//lazy evaluation of local aabb
 		btAssert(m_isLocalAabbValid);
-
-		btAssert(m_localAabbMin.getX() <= m_localAabbMax.getX());
-		btAssert(m_localAabbMin.getY() <= m_localAabbMax.getY());
-		btAssert(m_localAabbMin.getZ() <= m_localAabbMax.getZ());
-
-
-		btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
-		localHalfExtents+= btVector3(margin,margin,margin);
-		btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
-		
-		btMatrix3x3 abs_b = trans.getBasis().absolute();  
-
-		btPoint3 center = trans(localCenter);
-
-		btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
-			   abs_b[1].dot(localHalfExtents),
-			  abs_b[2].dot(localHalfExtents));
-
-		aabbMin = center-extent;
-		aabbMax = center+extent;
-
-		
+		btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax);
 	}
 
 	
diff --git a/src/LinearMath/btAabbUtil2.h b/src/LinearMath/btAabbUtil2.h
index 8bb6b3af7..def42f12c 100644
--- a/src/LinearMath/btAabbUtil2.h
+++ b/src/LinearMath/btAabbUtil2.h
@@ -17,6 +17,7 @@ subject to the following restrictions:
 #ifndef AABB_UTIL2
 #define AABB_UTIL2
 
+#include "btTransform.h"
 #include "btVector3.h"
 #include "btMinMax.h"
 
@@ -163,6 +164,38 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
 }
 
 
+
+SIMD_FORCE_INLINE	void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut)
+{
+	btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin);
+	btMatrix3x3 abs_b = t.getBasis().absolute();  
+	btVector3 center = t.getOrigin();
+	btVector3 extent = btVector3(abs_b[0].dot(halfExtentsWithMargin),
+		   abs_b[1].dot(halfExtentsWithMargin),
+		  abs_b[2].dot(halfExtentsWithMargin));
+	aabbMinOut = center - extent;
+	aabbMaxOut = center + extent;
+};
+
+SIMD_FORCE_INLINE	void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut)
+{
+		btAssert(localAabbMin.getX() <= localAabbMax.getX());
+		btAssert(localAabbMin.getY() <= localAabbMax.getY());
+		btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
+		btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
+		localHalfExtents+=btVector3(margin,margin,margin);
+
+		btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
+		btMatrix3x3 abs_b = trans.getBasis().absolute();  
+		btVector3 center = trans(localCenter);
+		btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
+			   abs_b[1].dot(localHalfExtents),
+			  abs_b[2].dot(localHalfExtents));
+		aabbMinOut = center-extent;
+		aabbMaxOut = center+extent;
+}
+
+
 #endif