diff --git a/Demos/BspDemo/BspDemo.cpp b/Demos/BspDemo/BspDemo.cpp
index 2a2b37991..28fffe36a 100644
--- a/Demos/BspDemo/BspDemo.cpp
+++ b/Demos/BspDemo/BspDemo.cpp
@@ -134,6 +134,7 @@ void	BspDemo::initPhysics(char* bspfilename)
 
 	///Setup a Physics Simulation Environment
 	m_dynamicsWorld = new btDiscreteDynamicsWorld();
+	m_dynamicsWorld->setGravity(-m_cameraUp);
 
 
 #ifdef QUAKE_BSP_IMPORTING
diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
index 34765f963..3550aa649 100644
--- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
+++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
@@ -14,7 +14,8 @@ subject to the following restrictions:
 */
 
 //#define USER_DEFINED_FRICTION_MODEL 1
-//#define PRINT_CONTACT_STATISTICS 1
+#define PRINT_CONTACT_STATISTICS 1
+//#define USE_KINEMATIC_GROUND 1
 #define REGISTER_CUSTOM_COLLISION_ALGORITHM 1
 
 #include "btBulletDynamicsCommon.h"
@@ -121,6 +122,14 @@ void CcdPhysicsDemo::clientMoveAndDisplay()
 {
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
 
+	
+#ifdef USE_KINEMATIC_GROUND
+	//btQuaternion kinRotation(btVector3(0,0,1),0.);
+	btVector3 kinTranslation(0,0,0.01);
+	//kinematic object
+	m_dynamicsWorld->getCollisionObjectArray()[0]->m_worldTransform.getOrigin() += kinTranslation;
+#endif //USE_KINEMATIC_GROUND
+
 	if (m_dynamicsWorld)
 		m_dynamicsWorld->stepSimulation(deltaTime);
 	
@@ -243,7 +252,7 @@ void	CcdPhysicsDemo::initPhysics()
 		btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
 
 		m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver);
-		//setGravity(btVector3(0,0,1));
+		m_dynamicsWorld->setGravity(btVector3(0,-10,0));
 
 		m_dynamicsWorld->setDebugDrawer(&debugDrawer);
 		
@@ -328,6 +337,14 @@ void	CcdPhysicsDemo::initPhysics()
 			mass = 0.f;
 	
 		btRigidBody* body = localCreateRigidBody(mass,trans,shape);
+#ifdef USE_KINEMATIC_GROUND
+		if (mass == 0.f)
+		{
+			body->m_collisionFlags = btCollisionObject::CF_KINEMATIC_OJBECT;
+			body->SetActivationState(DISABLE_DEACTIVATION);
+			body->setLinearVelocity(btVector3(0,0,1));
+		}
+#endif //USE_KINEMATIC_GROUND
 		
 		
 		// Only do CCD if  motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS
diff --git a/Demos/ColladaDemo/ColladaConverter.cpp b/Demos/ColladaDemo/ColladaConverter.cpp
index 8f715ad2d..c217cd232 100644
--- a/Demos/ColladaDemo/ColladaConverter.cpp
+++ b/Demos/ColladaDemo/ColladaConverter.cpp
@@ -1,1264 +1,1264 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include "ColladaConverter.h"
-#include "btBulletDynamicsCommon.h"
-#include "dae.h"
-#include "dom/domCOLLADA.h"
-
-#include "BulletCollision/CollisionShapes/btBoxShape.h"
-#include "BulletCollision/CollisionShapes/btSphereShape.h"
-#include "BulletCollision/CollisionShapes/btCylinderShape.h"
-#include "BulletCollision/CollisionShapes/btConeShape.h"
-#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
-#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
-#include "BulletCollision/CollisionShapes/btTriangleMesh.h"
-#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
-#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
-//#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
-#include "BulletCollision/CollisionShapes/btCompoundShape.h"
-
-
-
-char* getLastFileName();
-char* fixFileName(const char* lpCmdLine);
-//todo: sort out this domInstance_rigid_bodyRef forward definition, put it in the headerfile and make it virtual (make code more re-usable)
-
-struct	btRigidBodyInput
-{
-	domInstance_rigid_bodyRef	m_instanceRigidBodyRef;
-	domRigid_bodyRef	m_rigidBodyRef2;
-
-	char* m_bodyName;
-};
-
-struct	ConstraintInput
-{
-	domInstance_physics_modelRef	m_instance_physicsModelRef;
-	domPhysics_modelRef	m_model;
-};
-
-
-struct	btRigidBodyOutput
-{
-	float	m_mass;
-	bool	m_isDynamics;
-	btCollisionShape*	m_colShape;
-	btCompoundShape*	m_compoundShape;
-};
-
-
-
-domMatrix_Array emptyMatrixArray;
-domNodeRef	m_colladadomNodes[COLLADA_CONVERTER_MAX_NUM_OBJECTS];
-
-
-btTransform	GetbtTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
-														domRotate_Array& rotateArray,
-														domTranslate_Array& translateArray
-														)
-
-{
-	btTransform	startTransform;
-	startTransform.setIdentity();
-	
-	unsigned int i;
-	//either load the matrix (worldspace) or incrementally build the transform from 'translate'/'rotate'
-	for (i=0;i<matrixArray.getCount();i++)
-	{
-		domMatrixRef matrixRef = matrixArray[i];
-		domFloat4x4 fl16 = matrixRef->getValue();
-		btVector3 origin(fl16.get(3),fl16.get(7),fl16.get(11));
-		startTransform.setOrigin(origin);
-		btMatrix3x3 basis(fl16.get(0),fl16.get(1),fl16.get(2),
-							fl16.get(4),fl16.get(5),fl16.get(6),
-							fl16.get(8),fl16.get(9),fl16.get(10));
-		startTransform.setBasis(basis);
-	}
-
-	for (i=0;i<rotateArray.getCount();i++)
-	{
-		domRotateRef rotateRef = rotateArray[i];
-		domFloat4 fl4 = rotateRef->getValue();
-		float angleRad = SIMD_RADS_PER_DEG*fl4.get(3);
-		btQuaternion rotQuat(btVector3(fl4.get(0),fl4.get(1),fl4.get(2)),angleRad);
-		startTransform.getBasis() = startTransform.getBasis() * btMatrix3x3(rotQuat);
-	}
-
-	for (i=0;i<translateArray.getCount();i++)
-	{
-		domTranslateRef translateRef = translateArray[i];
-		domFloat3 fl3 = translateRef->getValue();
-		startTransform.getOrigin() += btVector3(fl3.get(0),fl3.get(1),fl3.get(2));
-	}
-	return startTransform;
-}
-
-
-
-
-
-ColladaConverter::ColladaConverter()
-:m_collada(0),
-m_dom(0),
-m_filename(0),
-m_numObjects(0)
-{
-	//Collada-m_dom
-	m_collada = new DAE;
-
-	//clear 
-	{
-
-		for (int i=0;i<COLLADA_CONVERTER_MAX_NUM_OBJECTS;i++)
-		{
-			m_colladadomNodes[i] = 0;
-		}
-	}
-}
-	
-
-bool	ColladaConverter::load(const char* orgfilename)
-{
-
-	const char* filename = fixFileName(orgfilename);
-	
-	int res = m_collada->load(filename);//,docBuffer);
-
-	if (res != DAE_OK)
-	{
-		//some platforms might require different path, try two additional locations
-		char newname[256];
-		sprintf(newname,"../../%s",orgfilename);
-		filename = fixFileName(newname);
-		res = m_collada->load(filename);
-		if (res != DAE_OK)
-		{
-			printf("DAE/Collada-m_dom: Couldn't load %s\n",filename);
-			return false;
-		}
-	}
-	
-	if (res == DAE_OK)
-	{
-
-		m_dom = m_collada->getDom(filename);
-		if ( !m_dom )
-		{
-			printf("COLLADA File loaded to the m_dom, but query for the m_dom assets failed \n" );
-			printf("COLLADA Load Aborted! \n" );
-			delete m_collada;	
-			return false;
-		}
-	}
-	return true;
-}
-
-
-
-
-
-bool ColladaConverter::convert()
-{
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "ColladaConverter.h"
+#include "btBulletDynamicsCommon.h"
+#include "dae.h"
+#include "dom/domCOLLADA.h"
+
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btConeShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleMesh.h"
+#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
+//#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+
+
+
+char* getLastFileName();
+char* fixFileName(const char* lpCmdLine);
+//todo: sort out this domInstance_rigid_bodyRef forward definition, put it in the headerfile and make it virtual (make code more re-usable)
+
+struct	btRigidBodyInput
+{
+	domInstance_rigid_bodyRef	m_instanceRigidBodyRef;
+	domRigid_bodyRef	m_rigidBodyRef2;
+
+	char* m_bodyName;
+};
+
+struct	ConstraintInput
+{
+	domInstance_physics_modelRef	m_instance_physicsModelRef;
+	domPhysics_modelRef	m_model;
+};
+
+
+struct	btRigidBodyOutput
+{
+	float	m_mass;
+	bool	m_isDynamics;
+	btCollisionShape*	m_colShape;
+	btCompoundShape*	m_compoundShape;
+};
+
+
+
+domMatrix_Array emptyMatrixArray;
+domNodeRef	m_colladadomNodes[COLLADA_CONVERTER_MAX_NUM_OBJECTS];
+
+
+btTransform	GetbtTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
+														domRotate_Array& rotateArray,
+														domTranslate_Array& translateArray
+														)
+
+{
+	btTransform	startTransform;
+	startTransform.setIdentity();
+	
+	unsigned int i;
+	//either load the matrix (worldspace) or incrementally build the transform from 'translate'/'rotate'
+	for (i=0;i<matrixArray.getCount();i++)
+	{
+		domMatrixRef matrixRef = matrixArray[i];
+		domFloat4x4 fl16 = matrixRef->getValue();
+		btVector3 origin(fl16.get(3),fl16.get(7),fl16.get(11));
+		startTransform.setOrigin(origin);
+		btMatrix3x3 basis(fl16.get(0),fl16.get(1),fl16.get(2),
+							fl16.get(4),fl16.get(5),fl16.get(6),
+							fl16.get(8),fl16.get(9),fl16.get(10));
+		startTransform.setBasis(basis);
+	}
+
+	for (i=0;i<rotateArray.getCount();i++)
+	{
+		domRotateRef rotateRef = rotateArray[i];
+		domFloat4 fl4 = rotateRef->getValue();
+		float angleRad = SIMD_RADS_PER_DEG*fl4.get(3);
+		btQuaternion rotQuat(btVector3(fl4.get(0),fl4.get(1),fl4.get(2)),angleRad);
+		startTransform.getBasis() = startTransform.getBasis() * btMatrix3x3(rotQuat);
+	}
+
+	for (i=0;i<translateArray.getCount();i++)
+	{
+		domTranslateRef translateRef = translateArray[i];
+		domFloat3 fl3 = translateRef->getValue();
+		startTransform.getOrigin() += btVector3(fl3.get(0),fl3.get(1),fl3.get(2));
+	}
+	return startTransform;
+}
+
+
+
+
+
+ColladaConverter::ColladaConverter()
+:m_collada(0),
+m_dom(0),
+m_filename(0),
+m_numObjects(0)
+{
+	//Collada-m_dom
+	m_collada = new DAE;
+
+	//clear 
+	{
+
+		for (int i=0;i<COLLADA_CONVERTER_MAX_NUM_OBJECTS;i++)
+		{
+			m_colladadomNodes[i] = 0;
+		}
+	}
+}
+	
+
+bool	ColladaConverter::load(const char* orgfilename)
+{
+
+	const char* filename = fixFileName(orgfilename);
+	
+	int res = m_collada->load(filename);//,docBuffer);
+
+	if (res != DAE_OK)
+	{
+		//some platforms might require different path, try two additional locations
+		char newname[256];
+		sprintf(newname,"../../%s",orgfilename);
+		filename = fixFileName(newname);
+		res = m_collada->load(filename);
+		if (res != DAE_OK)
+		{
+			printf("DAE/Collada-m_dom: Couldn't load %s\n",filename);
+			return false;
+		}
+	}
+	
+	if (res == DAE_OK)
+	{
+
+		m_dom = m_collada->getDom(filename);
+		if ( !m_dom )
+		{
+			printf("COLLADA File loaded to the m_dom, but query for the m_dom assets failed \n" );
+			printf("COLLADA Load Aborted! \n" );
+			delete m_collada;	
+			return false;
+		}
+	}
+	return true;
+}
+
+
+
+
+
+bool ColladaConverter::convert()
+{
 
 	unsigned i;
-
-//succesfully loaded file, now convert data
-
-			if ( m_dom->getAsset()->getUp_axis() )
-			{
-				domAsset::domUp_axis * up = m_dom->getAsset()->getUp_axis();
-				switch( up->getValue() )
-				{
-				case UPAXISTYPE_X_UP:
-					printf("	X is Up Data and Hiearchies must be converted!\n" ); 
-					printf("  Conversion to X axis Up isn't currently supported!\n" ); 
-					printf("  COLLADA_RT defaulting to Y Up \n" ); 
-					setGravity(btVector3(-10,0,0));
-					setCameraInfo(btVector3(1,0,0),1);
-					break; 
-				case UPAXISTYPE_Y_UP:
-					printf("	Y Axis is Up for this file \n" ); 
-					printf("  COLLADA_RT set to Y Up \n" ); 
-					setGravity(btVector3(0,-10,0));
-					setCameraInfo(btVector3(0,1,0),0);
-
-					break;
-				case UPAXISTYPE_Z_UP:
-					printf("	Z Axis is Up for this file \n" ); 
-					printf("  All Geometry and Hiearchies must be converted!\n" ); 
-					setGravity(btVector3(0,0,-10));
-					break; 
-				default:
-
-					break; 
-				}
-			}
-
-
-			//we don't handle visual objects, physics objects are rered as such
-			for (unsigned int s=0;s<m_dom->getLibrary_visual_scenes_array().getCount();s++)
-			{
-				domLibrary_visual_scenesRef scenesRef = m_dom->getLibrary_visual_scenes_array()[s];
-				for (unsigned int i=0;i<scenesRef->getVisual_scene_array().getCount();i++)
-				{
-					domVisual_sceneRef sceneRef = scenesRef->getVisual_scene_array()[i];
-					for (unsigned int n=0;n<sceneRef->getNode_array().getCount();n++)
-					{
-						domNodeRef nodeRef = sceneRef->getNode_array()[n];
-						nodeRef->getRotate_array();
-						nodeRef->getTranslate_array();
-						nodeRef->getScale_array();
-
-					}
-				}
-			}
-
-
-
-
-			// Load all the geometry libraries
-			for (  i = 0; i < m_dom->getLibrary_geometries_array().getCount(); i++)
-			{
-				domLibrary_geometriesRef libgeom = m_dom->getLibrary_geometries_array()[i];
-
-				printf(" CrtScene::Reading Geometry Library \n" );
-				for ( unsigned int  i = 0; i < libgeom->getGeometry_array().getCount(); i++)
-				{
-					//ReadGeometry(  ); 
-					domGeometryRef lib = libgeom->getGeometry_array()[i];
-
-					domMesh			*meshElement		= lib->getMesh();
-					if (meshElement)
-					{
-						// Find out how many groups we need to allocate space for 
-						int	numTriangleGroups = (int)meshElement->getTriangles_array().getCount();
-						int	numPolygonGroups  = (int)meshElement->getPolygons_array().getCount();
-						int	totalGroups		  = numTriangleGroups + numPolygonGroups;
-						if (totalGroups == 0) 
-						{
-							printf("No Triangles or Polygons found int Geometry %s \n", lib->getId() ); 
-						} else
-						{
-							printf("Found mesh geometry (%s): numTriangleGroups:%i numPolygonGroups:%i\n",lib->getId(),numTriangleGroups,numPolygonGroups);
-						}
-
-
-					}
-					domConvex_mesh	*convexMeshElement	= lib->getConvex_mesh();
-					if (convexMeshElement)
-					{
-						printf("found convexmesh element\n");
-						// Find out how many groups we need to allocate space for 
-						int	numTriangleGroups = (int)convexMeshElement->getTriangles_array().getCount();
-						int	numPolygonGroups  = (int)convexMeshElement->getPolygons_array().getCount();
-
-						int	totalGroups		  = numTriangleGroups + numPolygonGroups;
-						if (totalGroups == 0) 
-						{
-							printf("No Triangles or Polygons found in ConvexMesh Geometry %s \n", lib->getId() ); 
-						}else
-						{
-							printf("Found convexmesh geometry: numTriangleGroups:%i numPolygonGroups:%i\n",numTriangleGroups,numPolygonGroups);
-						}
-					}//fi
-				}//for each geometry
-
-			}//for all geometry libraries
-
-
-			//m_dom->getLibrary_physics_models_array()
-
-			for (  i = 0; i < m_dom->getLibrary_physics_scenes_array().getCount(); i++)
-			{
-				domLibrary_physics_scenesRef physicsScenesRef = m_dom->getLibrary_physics_scenes_array()[i];
-				for (unsigned int s=0;s<physicsScenesRef->getPhysics_scene_array().getCount();s++)
-				{
-					domPhysics_sceneRef physicsSceneRef = physicsScenesRef->getPhysics_scene_array()[s];
-
-					if (physicsSceneRef->getTechnique_common())
-					{
-						if (physicsSceneRef->getTechnique_common()->getGravity())
-						{
-							const domFloat3 grav = physicsSceneRef->getTechnique_common()->getGravity()->getValue();
-							printf("gravity set to %f,%f,%f\n",grav.get(0),grav.get(1),grav.get(2));
-
-							setGravity(btVector3(grav.get(0),grav.get(1),grav.get(2)));
-						}
-
-					} 
-
-					for (unsigned int ps=0;ps<physicsSceneRef->getInstance_physics_model_array().getCount();ps++)
-					{
-						domInstance_physics_modelRef instance_physicsModelRef = physicsSceneRef->getInstance_physics_model_array()[ps];
-
-						daeElementRef ref = instance_physicsModelRef->getUrl().getElement();
-
-						domPhysics_modelRef model = *(domPhysics_modelRef*)&ref; 
-
-
-						unsigned int p,r;
-						for ( p=0;p<model->getInstance_physics_model_array().getCount();p++)
-						{
-							domInstance_physics_modelRef	instancePhysicsModelRef = model->getInstance_physics_model_array()[p];
-
-							daeElementRef ref = instancePhysicsModelRef->getUrl().getElement();
-	
-							domPhysics_modelRef model = *(domPhysics_modelRef*)&ref; 
-
-							//todo: group some shared functionality in following 2 'blocks'.
-							for (r=0;r<instancePhysicsModelRef->getInstance_rigid_body_array().getCount();r++)
-							{
-								domInstance_rigid_bodyRef instRigidbodyRef = instancePhysicsModelRef->getInstance_rigid_body_array()[r];
-
-								float mass = 1.f;
-								bool isDynamics = true;
-								btCollisionShape* colShape = 0;
-								btCompoundShape* compoundShape = 0;
-
-								xsNCName bodyName = instRigidbodyRef->getBody();
-
-								domInstance_rigid_body::domTechnique_commonRef techniqueRef = instRigidbodyRef->getTechnique_common();
-								if (techniqueRef)
-								{
-									if (techniqueRef->getMass())
-									{
-										mass = techniqueRef->getMass()->getValue();
-									}
-									if (techniqueRef->getDynamic())
-									{
-										isDynamics = techniqueRef->getDynamic()->getValue();
-									}
-								}
-
-								printf("mass = %f, isDynamics %i\n",mass,isDynamics);
-
-								if (bodyName && model)
-								{
-									//try to find the rigid body
-
-									for (unsigned int r=0;r<model->getRigid_body_array().getCount();r++)
-									{
-										domRigid_bodyRef rigidBodyRef = model->getRigid_body_array()[r];
-										if (rigidBodyRef->getSid() && !strcmp(rigidBodyRef->getSid(),bodyName))
-										{
-
-
-											btRigidBodyOutput output;
-											output.m_colShape = colShape;
-											output.m_compoundShape = compoundShape;
-
-											btRigidBodyInput rbInput;
-											rbInput.m_rigidBodyRef2 = rigidBodyRef;
-											rbInput.m_instanceRigidBodyRef = instRigidbodyRef;
-											ConvertRigidBodyRef( rbInput , output );
-
-											mass = output.m_mass;
-											isDynamics = output.m_isDynamics;
-											colShape = output.m_colShape;
-											compoundShape = output.m_compoundShape;
-
-										}
-									}
-
-									//////////////////////
-								}
-
-								if (compoundShape)
-									colShape = compoundShape;
-
-								if (colShape)
-								{
-									btRigidBodyInput input;
-									input.m_instanceRigidBodyRef = instRigidbodyRef;
-									input.m_rigidBodyRef2 = 0;
-									input.m_bodyName = (char*)bodyName;
-									PreparePhysicsObject(input, isDynamics,mass,colShape);
-								}
-							}
-						}
-
-
-						for (r=0;r<instance_physicsModelRef->getInstance_rigid_body_array().getCount();r++)
-						{
-
-							domInstance_rigid_bodyRef instRigidbodyRef = instance_physicsModelRef->getInstance_rigid_body_array()[r];
-
-							
-
-							float mass = 1.f;
-							bool isDynamics = true;
-							btCollisionShape* colShape = 0;
-							btCompoundShape* compoundShape = 0;
-
-							xsNCName bodyName = instRigidbodyRef->getBody();
-
-							domInstance_rigid_body::domTechnique_commonRef techniqueRef = instRigidbodyRef->getTechnique_common();
-							if (techniqueRef)
-							{
-								if (techniqueRef->getMass())
-								{
-									mass = techniqueRef->getMass()->getValue();
-								}
-								if (techniqueRef->getDynamic())
-								{
-									isDynamics = techniqueRef->getDynamic()->getValue();
-								}
-							}
-
-							printf("mass = %f, isDynamics %i\n",mass,isDynamics);
-
-							if (bodyName && model)
-							{
-								//try to find the rigid body
-
-								for (unsigned int r=0;r<model->getRigid_body_array().getCount();r++)
-								{
-									domRigid_bodyRef rigidBodyRef = model->getRigid_body_array()[r];
-									if (rigidBodyRef->getSid() && !strcmp(rigidBodyRef->getSid(),bodyName))
-									{
-
-
-										btRigidBodyOutput output;
-										output.m_colShape = colShape;
-										output.m_compoundShape = compoundShape;
-
-										btRigidBodyInput rbInput;
-										rbInput.m_rigidBodyRef2 = rigidBodyRef;
-										rbInput.m_instanceRigidBodyRef = instRigidbodyRef;
-										ConvertRigidBodyRef( rbInput , output );
-
-										mass = output.m_mass;
-										isDynamics = output.m_isDynamics;
-										colShape = output.m_colShape;
-										compoundShape = output.m_compoundShape;
-
-									}
-								}
-
-								//////////////////////
-							}
-
-							if (compoundShape)
-								colShape = compoundShape;
-
-							if (colShape)
-							{
-								btRigidBodyInput input;
-								input.m_instanceRigidBodyRef = instRigidbodyRef;
-								input.m_rigidBodyRef2 = 0;
-								input.m_bodyName = (char*)bodyName;
-								PreparePhysicsObject(input, isDynamics,mass,colShape);
-							}
-
-						} //for  each  instance_rigid_body
-
-						
-					} //for each physics model
-
-					
-					//handle constraints
-					for (unsigned int ma=0;ma<physicsSceneRef->getInstance_physics_model_array().getCount();ma++)
-					{
-						domInstance_physics_modelRef instance_physicsModelRef = physicsSceneRef->getInstance_physics_model_array()[ma];
-
-						daeElementRef ref = instance_physicsModelRef->getUrl().getElement();
-
-						domPhysics_modelRef model = *(domPhysics_modelRef*)&ref; 
-
-						{
-							ConstraintInput cInput;
-							cInput.m_instance_physicsModelRef = instance_physicsModelRef;
-							cInput.m_model = model;
-							prepareConstraints(cInput);
-						}
-
-						//also don't forget the model's 'instance_physics_models!
-						for ( unsigned int p=0;p<model->getInstance_physics_model_array().getCount();p++)
-						{
-							domInstance_physics_modelRef	instancePhysicsModelRef = model->getInstance_physics_model_array()[p];
-
-							daeElementRef ref = instancePhysicsModelRef->getUrl().getElement();
-	
-							domPhysics_modelRef model = *(domPhysics_modelRef*)&ref; 
-							
-							ConstraintInput cInput;
-							cInput.m_instance_physicsModelRef = instancePhysicsModelRef;
-							cInput.m_model = model;
-							prepareConstraints(cInput);
-						}
-
-											
-					} //2nd time, for each physics model
-
-				}
-			}
-
-			return true;
-}
-
-
-void	ColladaConverter::prepareConstraints(ConstraintInput& input)
-{
-	domInstance_physics_modelRef instance_physicsModelRef = input.m_instance_physicsModelRef;
-	domPhysics_modelRef model = input.m_model;
-
-	for (unsigned int c=0;c<instance_physicsModelRef->getInstance_rigid_constraint_array().getCount();c++)
-	{
-		domInstance_rigid_constraintRef constraintRef = instance_physicsModelRef->getInstance_rigid_constraint_array().get(c);
-		xsNCName constraintName = constraintRef->getConstraint();
-
-		if (constraintName && model)
-		{
-			//try to find the rigid body
-			int numConstraints= model->getRigid_constraint_array().getCount();
-
-			for (int r=0;r<numConstraints;r++)
-			{
-				domRigid_constraintRef rigidConstraintRef = model->getRigid_constraint_array()[r];
-				
-				if (rigidConstraintRef->getSid() && !strcmp(rigidConstraintRef->getSid(),constraintName))
-				{
-					
-					//two bodies
-					const domRigid_constraint::domRef_attachmentRef attachRefBody = rigidConstraintRef->getRef_attachment();
-					const domRigid_constraint::domAttachmentRef attachBody1 = rigidConstraintRef->getAttachment();
-
-					daeString orgUri0 = attachRefBody->getRigid_body().getOriginalURI();
-					daeString orgUri1 = attachBody1->getRigid_body().getOriginalURI();
-					btRigidBody* body0=0,*body1=0;
-					
-					for (int i=0;i<m_numObjects;i++)
-					{
-						char* bodyName = (char*)m_rigidBodies[i]->m_userObjectPointer;
-						if (!strcmp(bodyName,orgUri0))
-						{
-							body0=m_rigidBodies[i];
-						}
-						if (!strcmp(bodyName,orgUri1))
-						{
-							body1=m_rigidBodies[i];
-						}
-					}
-
-
-
-					const domRigid_constraint::domAttachmentRef attachOtherBody = rigidConstraintRef->getAttachment();
-
-					
-					const domRigid_constraint::domTechnique_commonRef commonRef = rigidConstraintRef->getTechnique_common();
-					
-					domFloat3 flMin = commonRef->getLimits()->getLinear()->getMin()->getValue();
-					btVector3 minLinearLimit(flMin.get(0),flMin.get(1),flMin.get(2));
-					
-					domFloat3 flMax = commonRef->getLimits()->getLinear()->getMax()->getValue();
-					btVector3 maxLinearLimit(flMax.get(0),flMax.get(1),flMax.get(2));
-														
-					domFloat3 coneMinLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMin()->getValue();
-					btVector3 angularMin(coneMinLimit.get(0),coneMinLimit.get(1),coneMinLimit.get(2));
-
-					domFloat3 coneMaxLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMax()->getValue();
-					btVector3 angularMax(coneMaxLimit.get(0),coneMaxLimit.get(1),coneMaxLimit.get(2));
-
-					{
-						
-						btTransform attachFrameRef0;
-						attachFrameRef0 = 
-							GetbtTransformFromCOLLADA_DOM
-							(
-							emptyMatrixArray,
-							attachRefBody->getRotate_array(),
-							attachRefBody->getTranslate_array());
-
-						btTransform attachFrameOther;
-						attachFrameOther =
-							GetbtTransformFromCOLLADA_DOM
-							(
-							emptyMatrixArray,
-							attachBody1->getRotate_array(),
-							attachBody1->getTranslate_array()
-							);
-
-
-						//convert INF / -INF into lower > upper
-
-						//currently there is a hack in the DOM to detect INF / -INF
-						//see daeMetaAttribute.cpp
-						//INF -> 999999.9
-						//-INF -> -999999.9
-						float linearCheckTreshold = 999999.0;
-						float angularCheckTreshold = 180.0;//check this
-
-
-
-						
-						//free means upper < lower, 
-						//locked means upper == lower
-						//limited means upper > lower
-						//limitIndex: first 3 are linear, next 3 are angular
-
-						btVector3 linearLowerLimits = minLinearLimit;
-						btVector3 linearUpperLimits = maxLinearLimit;
-						btVector3 angularLowerLimits = angularMin;
-						btVector3 angularUpperLimits = angularMax;
-						{
-							for (int i=0;i<3;i++)
-							{
-								if  ((linearLowerLimits[i] < -linearCheckTreshold) ||
-									(linearUpperLimits[i] > linearCheckTreshold))
-								{
-									//disable limits
-									linearLowerLimits[i] = 1;
-									linearUpperLimits[i] = 0;
-								}
-
-								if  ((angularLowerLimits[i] < -angularCheckTreshold) ||
-									(angularUpperLimits[i] > angularCheckTreshold))
-								{
-									//disable limits
-									angularLowerLimits[i] = 1;
-									angularUpperLimits[i] = 0;
-								}
-							}
-						}
-
-
-						if (body0&& body1)
-						{
-						createUniversalD6Constraint(
-						body0,
-						body1,
-						attachFrameRef0,
-						attachFrameOther,
-						linearLowerLimits,
-						linearUpperLimits,
-						angularLowerLimits,
-						angularUpperLimits
-							);
-						} else
-						{
-							printf("Error: Cannot find Rigidbodies(%s,%s) for constraint %s\n",orgUri0,orgUri1,constraintName);
-						}
-
-
-					}
-
-
-
-				}
-			}
-		}
-
-	}
-
-}
-
-void	ColladaConverter::PreparePhysicsObject(struct btRigidBodyInput& input, bool isDynamics, float mass,btCollisionShape* colShape)
-{
-	btTransform startTransform;
-	startTransform.setIdentity();
-	btVector3 startScale(1.f,1.f,1.f);
-
-	//The 'target' points to a graphics element/node, which contains the start (world) transform
-	daeElementRef elem = input.m_instanceRigidBodyRef->getTarget().getElement();
-	if (elem)
-	{
-		domNodeRef node = *(domNodeRef*)&elem;
-		m_colladadomNodes[m_numObjects] = node;
-
-		//find transform of the node that this rigidbody maps to
-
-
-		startTransform = GetbtTransformFromCOLLADA_DOM(
-							node->getMatrix_array(),
-							node->getRotate_array(),
-							node->getTranslate_array()
-							);
-
-		unsigned int i;
-		for (i=0;i<node->getScale_array().getCount();i++)
-		{
-			domScaleRef scaleRef = node->getScale_array()[i];
-			domFloat3 fl3 = scaleRef->getValue();
-			startScale = btVector3(fl3.get(0),fl3.get(1),fl3.get(2));
-		}
-
-	}
-
-
-
-	btRigidBody* body= createRigidBody(isDynamics,mass,startTransform,colShape);
-	if (body)
-	{
-		//for bodyName lookup in constraints
-		body->m_userObjectPointer = (void*)input.m_bodyName;
-		m_rigidBodies[m_numObjects] = body;
-		m_numObjects++;
-	}
-
-}
-
-bool ColladaConverter::saveAs(const char* filename)
-{
-	if (m_collada)
-		{
-			for (int i=0;i<m_numObjects;i++)
-			{
-				assert(m_colladadomNodes[i]);
-				if (!m_colladadomNodes[i]->getTranslate_array().getCount())
-				{
-					domTranslate* transl = (domTranslate*) m_colladadomNodes[i]->createAndPlace("translate");
-					transl->getValue().append(0.);
-					transl->getValue().append(0.);
-					transl->getValue().append(0.);
-				}
-
-				while (m_colladadomNodes[i]->getTranslate_array().getCount() > 1)
-				{
-					m_colladadomNodes[i]->removeFromParent(m_colladadomNodes[i]->getTranslate_array().get(1));
-					//m_colladadomNodes[i]->getTranslate_array().removeIndex(1);
-				}
-
-				{
-
-					btVector3 np = m_rigidBodies[i]->m_worldTransform.getOrigin();
-					domFloat3 newPos = m_colladadomNodes[i]->getTranslate_array().get(0)->getValue();
-					newPos.set(0,np[0]);
-					newPos.set(1,np[1]);
-					newPos.set(2,np[2]);
-					m_colladadomNodes[i]->getTranslate_array().get(0)->setValue(newPos);
-
-				}
-				
-
-				if (!m_colladadomNodes[i]->getRotate_array().getCount())
-				{
-					domRotate* rot = (domRotate*)m_colladadomNodes[i]->createAndPlace("rotate");
-					rot->getValue().append(1.0);
-					rot->getValue().append(0.0);
-					rot->getValue().append(0.0);
-					rot->getValue().append(0.0);
-				}
-
-				while (m_colladadomNodes[i]->getRotate_array().getCount()>1)
-				{
-					m_colladadomNodes[i]->removeFromParent(m_colladadomNodes[i]->getRotate_array().get(1));
-					//m_colladadomNodes[i]->getRotate_array().removeIndex(1);
-
-				}
-
-				{
-					btQuaternion quat = m_rigidBodies[i]->getCenterOfMassTransform().getRotation();
-					btVector3 axis(quat.getX(),quat.getY(),quat.getZ());
-					axis[3] = 0.f;
-					//check for axis length
-					btScalar len = axis.length2();
-					if (len < SIMD_EPSILON*SIMD_EPSILON)
-						axis = btVector3(1.f,0.f,0.f);
-					else
-						axis /= btSqrt(len);
-					m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(0,axis[0]);
-					m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(1,axis[1]);
-					m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(2,axis[2]);
-					m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(3,quat.getAngle()*SIMD_DEGS_PER_RAD);
-				}
-
-				while (m_colladadomNodes[i]->getMatrix_array().getCount())
-				{
-					m_colladadomNodes[i]->removeFromParent(m_colladadomNodes[i]->getMatrix_array().get(0));
-					//m_colladadomNodes[i]->getMatrix_array().removeIndex(0);
-				}
-			}
-			char	saveName[550];
-			static int saveCount=1;
-			sprintf(saveName,"%s%i",getLastFileName(),saveCount++);
-			char* name = &saveName[0];
-			if (name[0] == '/')
-			{
-				name = &saveName[1];
-			} 
-			
-			if (m_dom->getAsset()->getContributor_array().getCount())
-			{
-				if (!m_dom->getAsset()->getContributor_array().get(0)->getAuthor())
-				{
-					m_dom->getAsset()->getContributor_array().get(0)->createAndPlace("author");
-				}
-
-				m_dom->getAsset()->getContributor_array().get(0)->getAuthor()->setValue
-					("http://bullet.sf.net Erwin Coumans");
-
-				if (!m_dom->getAsset()->getContributor_array().get(0)->getAuthoring_tool())
-				{
-					m_dom->getAsset()->getContributor_array().get(0)->createAndPlace("authoring_tool");
-				}
-
-				m_dom->getAsset()->getContributor_array().get(0)->getAuthoring_tool()->setValue
-#ifdef WIN32
-					("Bullet ColladaPhysicsViewer-Win32-0.8");
-#else
-#ifdef __APPLE__
-					("Bullet ColladaPhysicsViewer-MacOSX-0.8");
-#else
-					("Bullet ColladaPhysicsViewer-UnknownPlatform-0.8");
-#endif
-#endif
-				if (!m_dom->getAsset()->getContributor_array().get(0)->getComments())
-				{
-					m_dom->getAsset()->getContributor_array().get(0)->createAndPlace("comments");
-				}
-				 m_dom->getAsset()->getContributor_array().get(0)->getComments()->setValue
-					 ("Comments to Physics Forum at http://www.continuousphysics.com/Bullet/phpBB2/index.php");
-			}
-
-			m_collada->saveAs(name);
-			return true;
-			
-		}
-		return false;
-}
-
-//some code that de-mangles the windows filename passed in as argument
-char cleaned_filename[512];
-char* getLastFileName()
-{
-	return cleaned_filename;
-}
-
-
-char* fixFileName(const char* lpCmdLine)
-{
-
-
-	// We might get a windows-style path on the command line, this can mess up the DOM which expects
-	// all paths to be URI's.  This block of code does some conversion to try and make the input
-	// compliant without breaking the ability to accept a properly formatted URI.  Right now this only
-	// displays the first filename
-	const char *in = lpCmdLine;
-	char* out = cleaned_filename;
-	*out = '\0';
-	// If the first character is a ", skip it (filenames with spaces in them are quoted)
-	if(*in == '\"')
-	{
-		in++;
-	}
-	if(*(in+1) == ':')
-	{
-		// Second character is a :, assume we have a path with a drive letter and add a slash at the beginning
-		*(out++) = '/';
-	}
-	int i;
-	for(i =0; i<512; i++)
-	{
-		// If we hit a null or a quote, stop copying.  This will get just the first filename.
-		if(*in == '\0' || *in == '\"')
-			break;
-		// Copy while swapping backslashes for forward ones
-		if(*in == '\\')
-		{
-			*out = '/';
-		}
-		else
-		{
-			*out = *in;
-		}
-		in++;
-		out++;
-	}
-	
-	return cleaned_filename;
-}
-
-
-void	ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBodyOutput& rbOutput)
-{
-	
-	const domRigid_body::domTechnique_commonRef techniqueRef = rbInput.m_rigidBodyRef2->getTechnique_common();
-	if (techniqueRef)
-	{
-
-		if (techniqueRef->getMass())
-		{
-			rbOutput.m_mass = techniqueRef->getMass()->getValue();
-		}
-		if (techniqueRef->getDynamic())
-		{
-			rbOutput.m_isDynamics = techniqueRef->getDynamic()->getValue();
-		}
-
-		//shapes
-		for (unsigned int s=0;s<techniqueRef->getShape_array().getCount();s++)
-		{
-			domRigid_body::domTechnique_common::domShapeRef shapeRef = techniqueRef->getShape_array()[s];
-
-			if (shapeRef->getPlane())
-			{
-				domPlaneRef planeRef = shapeRef->getPlane();
-				if (planeRef->getEquation())
-				{
-					const domFloat4 planeEq = planeRef->getEquation()->getValue();
-					btVector3 planeNormal(planeEq.get(0),planeEq.get(1),planeEq.get(2));
-					btScalar planeConstant = planeEq.get(3);
-					rbOutput.m_colShape = new btStaticPlaneShape(planeNormal,planeConstant);
-				}
-
-			}
-
-			if (shapeRef->getBox())
-			{
-				domBoxRef boxRef = shapeRef->getBox();
-				domBox::domHalf_extentsRef	domHalfExtentsRef = boxRef->getHalf_extents();
-				domFloat3& halfExtents = domHalfExtentsRef->getValue();
-				float x = halfExtents.get(0);
-				float y = halfExtents.get(1);
-				float z = halfExtents.get(2);
-				rbOutput.m_colShape = new btBoxShape(btVector3(x,y,z));
-			}
-			if (shapeRef->getSphere())
-			{
-				domSphereRef sphereRef = shapeRef->getSphere();
-				domSphere::domRadiusRef radiusRef = sphereRef->getRadius();
-				domFloat radius = radiusRef->getValue();
-				rbOutput.m_colShape = new btSphereShape(radius);
-			}
-
-			if (shapeRef->getCylinder())
-			{
-				domCylinderRef cylinderRef = shapeRef->getCylinder();
-				domFloat height = cylinderRef->getHeight()->getValue();
-				domFloat2 radius2 = cylinderRef->getRadius()->getValue();
-				domFloat radius0 = radius2.get(0);
-
-				//Cylinder around the local Y axis
-				rbOutput.m_colShape = new btCylinderShape(btVector3(radius0,height,radius0));
-
-			}
-
-			if (shapeRef->getInstance_geometry())
-			{
-				const domInstance_geometryRef geomInstRef = shapeRef->getInstance_geometry();
-				daeElement* geomElem = geomInstRef->getUrl().getElement();
-				//elemRef->getTypeName();
-				domGeometry* geom = (domGeometry*) geomElem;
-				if (geom && geom->getMesh())
-				{
-					const domMeshRef meshRef = geom->getMesh();
-
-					btTriangleMesh* trimesh = new btTriangleMesh();
-
-					
-					for (unsigned int tg = 0;tg<meshRef->getTriangles_array().getCount();tg++)
-					{
-
-
-						domTrianglesRef triRef = meshRef->getTriangles_array()[tg];
-						const domPRef pRef = triRef->getP();
-						btIndexedMesh meshPart;
-						meshPart.m_triangleIndexStride=0;
-
-
-						
-						int vertexoffset = -1;
-						domInputLocalOffsetRef indexOffsetRef;
-						
-
-						for (unsigned int w=0;w<triRef->getInput_array().getCount();w++)
-						{
-							int offset = triRef->getInput_array()[w]->getOffset();
-							daeString str = triRef->getInput_array()[w]->getSemantic();
-							if (!strcmp(str,"VERTEX"))
-							{
-								indexOffsetRef = triRef->getInput_array()[w];
-								vertexoffset = offset;
-							}
-							if (offset > meshPart.m_triangleIndexStride)
-							{
-								meshPart.m_triangleIndexStride = offset;
-							}
-						}
-						meshPart.m_triangleIndexStride++;
-						domListOfUInts indexArray =triRef->getP()->getValue(); 
-
-						//int*		m_triangleIndexBase;
-
-
-
-						meshPart.m_numTriangles = triRef->getCount();
-
-						const domVerticesRef vertsRef = meshRef->getVertices();
-						int numInputs = vertsRef->getInput_array().getCount();
-						for (int i=0;i<numInputs;i++)
-						{
-							domInputLocalRef localRef = vertsRef->getInput_array()[i];
-							daeString str = localRef->getSemantic();
-							if ( !strcmp(str,"POSITION"))
-							{
-								const domURIFragmentType& frag = localRef->getSource();
-
-								daeElementConstRef constElem = frag.getElement();
-
-								const domSourceRef node = *(const domSourceRef*)&constElem;
-								const domFloat_arrayRef flArray = node->getFloat_array();
-								if (flArray)
-								{
-									const domListOfFloats& listFloats = flArray->getValue();
-
-									int k=vertexoffset;
-									int t=0;
-									int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
-									for (;t<meshPart.m_numTriangles;t++)
-									{
-										btVector3 verts[3];
-										int index0;
-										for (int i=0;i<3;i++)
-										{
-											index0 = indexArray.get(k)*vertexStride;
-											domFloat fl0 = listFloats.get(index0);
-											domFloat fl1 = listFloats.get(index0+1);
-											domFloat fl2 = listFloats.get(index0+2);
-											k+=meshPart.m_triangleIndexStride;
-											verts[i].setValue(fl0,fl1,fl2);
-										}
-										trimesh->addTriangle(verts[0],verts[1],verts[2]);
-									}
-								}
-							}
-						}
-
-
-
-
-
-							//int			m_triangleIndexStride;//calculate max offset
-							//int			m_numVertices;
-							//float*		m_vertexBase;//getRawData on floatArray
-							//int			m_vertexStride;//use the accessor for this
-
-						//};
-						//tindexArray->addIndexedMesh(meshPart);
-						if (rbOutput.m_isDynamics)
-						{
-							printf("moving concave <mesh> not supported, transformed into convex\n");
-							rbOutput.m_colShape = new btConvexTriangleMeshShape(trimesh);
-						} else
-						{
-							printf("static concave triangle <mesh> added\n");
-							rbOutput.m_colShape = new btBvhTriangleMeshShape(trimesh);
-							//rbOutput.m_colShape = new btBvhTriangleMeshShape(trimesh);
-							//rbOutput.m_colShape = new btConvexTriangleMeshShape(trimesh);
-							
-							//btTriangleMeshShape
-						}
-
-					}
-
-				}
-
-				if (geom && geom->getConvex_mesh())
-				{
-
-					{
-						const domConvex_meshRef convexRef = geom->getConvex_mesh();
-						daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
-						if ( otherElemRef != NULL )
-						{
-							domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
-							printf( "otherLinked\n");
-						} else
-						{
-							printf("convexMesh polyCount = %i\n",convexRef->getPolygons_array().getCount());
-							printf("convexMesh triCount = %i\n",convexRef->getTriangles_array().getCount());
-
-						}
-					}
-
-
-
-					btConvexHullShape* convexHullShape = new btConvexHullShape(0,0);
-
-					//it is quite a trick to get to the vertices, using Collada.
-					//we are not there yet...
-
-					const domConvex_meshRef convexRef = geom->getConvex_mesh();
-					//daeString urlref = convexRef->getConvex_hull_of().getURI();
-					daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI();
-					if (urlref2)
-					{
-						daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
-						//	if ( otherElemRef != NULL )
-						//	{
-						//		domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
-
-						// Load all the geometry libraries
-						for ( unsigned int i = 0; i < m_dom->getLibrary_geometries_array().getCount(); i++)
-						{
-							domLibrary_geometriesRef libgeom = m_dom->getLibrary_geometries_array()[i];
-							//int index = libgeom->findLastIndexOf(urlref2);
-							//can't find it
-
-							for ( unsigned int  i = 0; i < libgeom->getGeometry_array().getCount(); i++)
-							{
-								//ReadGeometry(  ); 
-								domGeometryRef lib = libgeom->getGeometry_array()[i];
-								if (!strcmp(lib->getName(),urlref2))
-								{
-									//found convex_hull geometry
-									domMesh			*meshElement		= lib->getMesh();//linkedGeom->getMesh();
-									if (meshElement)
-									{
-										const domVerticesRef vertsRef = meshElement->getVertices();
-										int numInputs = vertsRef->getInput_array().getCount();
-										for (int i=0;i<numInputs;i++)
-										{
-											domInputLocalRef localRef = vertsRef->getInput_array()[i];
-											daeString str = localRef->getSemantic();
-											if ( !strcmp(str,"POSITION"))
-											{
-												const domURIFragmentType& frag = localRef->getSource();
-
-												daeElementConstRef constElem = frag.getElement();
-
-												const domSourceRef node = *(const domSourceRef*)&constElem;
-												const domFloat_arrayRef flArray = node->getFloat_array();
-												if (flArray)
-												{
-													int numElem = flArray->getCount();
-													const domListOfFloats& listFloats = flArray->getValue();
-
-													for (int k=0;k+2<numElem;k+=3)
-													{
-														domFloat fl0 = listFloats.get(k);
-														domFloat fl1 = listFloats.get(k+1);
-														domFloat fl2 = listFloats.get(k+2);
-														//printf("float %f %f %f\n",fl0,fl1,fl2);
-
-														convexHullShape->addPoint(btPoint3(fl0,fl1,fl2));
-													}
-
-												}
-
-											}
-
-
-										}
-
-									}
-								}
-							
-									
-								
-							}
-						}
-
-
-					} else
-					{
-						//no getConvex_hull_of but direct vertices
-						const domVerticesRef vertsRef = convexRef->getVertices();
-						int numInputs = vertsRef->getInput_array().getCount();
-						for (int i=0;i<numInputs;i++)
-						{
-							domInputLocalRef localRef = vertsRef->getInput_array()[i];
-							daeString str = localRef->getSemantic();
-							if ( !strcmp(str,"POSITION"))
-							{
-								const domURIFragmentType& frag = localRef->getSource();
-
-								daeElementConstRef constElem = frag.getElement();
-
-								const domSourceRef node = *(const domSourceRef*)&constElem;
-								const domFloat_arrayRef flArray = node->getFloat_array();
-								if (flArray)
-								{
-									int numElem = flArray->getCount();
-									const domListOfFloats& listFloats = flArray->getValue();
-
-									for (int k=0;k+2<numElem;k+=3)
-									{
-										domFloat fl0 = listFloats.get(k);
-										domFloat fl1 = listFloats.get(k+1);
-										domFloat fl2 = listFloats.get(k+2);
-										//printf("float %f %f %f\n",fl0,fl1,fl2);
-
-										convexHullShape->addPoint(btPoint3(fl0,fl1,fl2));
-									}
-
-								}
-
-							}
-
-
-						}
-
-
-					}
-
-					if (convexHullShape->getNumVertices())
-					{
-						rbOutput.m_colShape = convexHullShape;
-						printf("created convexHullShape with %i points\n",convexHullShape->getNumVertices());
-					} else
-					{
-						delete convexHullShape;
-						printf("failed to create convexHullShape\n");
-					}
-
-
-					//domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
-
-					printf("convexmesh\n");
-
-				}
-			}
-
-			//if more then 1 shape, or a non-identity local shapetransform
-			//use a compound
-
-			bool hasShapeLocalTransform = ((shapeRef->getRotate_array().getCount() > 0) ||
-				(shapeRef->getTranslate_array().getCount() > 0));
-			
-			if (rbOutput.m_colShape)
-			{
-				if ((techniqueRef->getShape_array().getCount()>1) ||
-					(hasShapeLocalTransform))
-				{
-					
-					if (!rbOutput.m_compoundShape)
-					{
-						rbOutput.m_compoundShape = new btCompoundShape();
-					}
-
-					btTransform localTransform;
-					localTransform.setIdentity();
-					if (hasShapeLocalTransform)
-					{
-					localTransform = GetbtTransformFromCOLLADA_DOM(
-						emptyMatrixArray,
-						shapeRef->getRotate_array(),
-						shapeRef->getTranslate_array()
-						);
-					}
-
-					rbOutput.m_compoundShape->addChildShape(localTransform,rbOutput.m_colShape);
-					rbOutput.m_colShape = 0;
-				}
-			}
-
-
-		}//for each shape
-	}
-
-
-}
+
+//succesfully loaded file, now convert data
+
+			if ( m_dom->getAsset()->getUp_axis() )
+			{
+				domAsset::domUp_axis * up = m_dom->getAsset()->getUp_axis();
+				switch( up->getValue() )
+				{
+				case UPAXISTYPE_X_UP:
+					printf("	X is Up Data and Hiearchies must be converted!\n" ); 
+					printf("  Conversion to X axis Up isn't currently supported!\n" ); 
+					printf("  COLLADA_RT defaulting to Y Up \n" ); 
+					setGravity(btVector3(-10,0,0));
+					setCameraInfo(btVector3(1,0,0),1);
+					break; 
+				case UPAXISTYPE_Y_UP:
+					printf("	Y Axis is Up for this file \n" ); 
+					printf("  COLLADA_RT set to Y Up \n" ); 
+					setGravity(btVector3(0,-10,0));
+					setCameraInfo(btVector3(0,1,0),0);
+
+					break;
+				case UPAXISTYPE_Z_UP:
+					printf("	Z Axis is Up for this file \n" ); 
+					printf("  All Geometry and Hiearchies must be converted!\n" ); 
+					setGravity(btVector3(0,0,-10));
+					break; 
+				default:
+
+					break; 
+				}
+			}
+
+
+			//we don't handle visual objects, physics objects are rered as such
+			for (unsigned int s=0;s<m_dom->getLibrary_visual_scenes_array().getCount();s++)
+			{
+				domLibrary_visual_scenesRef scenesRef = m_dom->getLibrary_visual_scenes_array()[s];
+				for (unsigned int i=0;i<scenesRef->getVisual_scene_array().getCount();i++)
+				{
+					domVisual_sceneRef sceneRef = scenesRef->getVisual_scene_array()[i];
+					for (unsigned int n=0;n<sceneRef->getNode_array().getCount();n++)
+					{
+						domNodeRef nodeRef = sceneRef->getNode_array()[n];
+						nodeRef->getRotate_array();
+						nodeRef->getTranslate_array();
+						nodeRef->getScale_array();
+
+					}
+				}
+			}
+
+
+
+
+			// Load all the geometry libraries
+			for (  i = 0; i < m_dom->getLibrary_geometries_array().getCount(); i++)
+			{
+				domLibrary_geometriesRef libgeom = m_dom->getLibrary_geometries_array()[i];
+
+				printf(" CrtScene::Reading Geometry Library \n" );
+				for ( unsigned int  i = 0; i < libgeom->getGeometry_array().getCount(); i++)
+				{
+					//ReadGeometry(  ); 
+					domGeometryRef lib = libgeom->getGeometry_array()[i];
+
+					domMesh			*meshElement		= lib->getMesh();
+					if (meshElement)
+					{
+						// Find out how many groups we need to allocate space for 
+						int	numTriangleGroups = (int)meshElement->getTriangles_array().getCount();
+						int	numPolygonGroups  = (int)meshElement->getPolygons_array().getCount();
+						int	totalGroups		  = numTriangleGroups + numPolygonGroups;
+						if (totalGroups == 0) 
+						{
+							printf("No Triangles or Polygons found int Geometry %s \n", lib->getId() ); 
+						} else
+						{
+							printf("Found mesh geometry (%s): numTriangleGroups:%i numPolygonGroups:%i\n",lib->getId(),numTriangleGroups,numPolygonGroups);
+						}
+
+
+					}
+					domConvex_mesh	*convexMeshElement	= lib->getConvex_mesh();
+					if (convexMeshElement)
+					{
+						printf("found convexmesh element\n");
+						// Find out how many groups we need to allocate space for 
+						int	numTriangleGroups = (int)convexMeshElement->getTriangles_array().getCount();
+						int	numPolygonGroups  = (int)convexMeshElement->getPolygons_array().getCount();
+
+						int	totalGroups		  = numTriangleGroups + numPolygonGroups;
+						if (totalGroups == 0) 
+						{
+							printf("No Triangles or Polygons found in ConvexMesh Geometry %s \n", lib->getId() ); 
+						}else
+						{
+							printf("Found convexmesh geometry: numTriangleGroups:%i numPolygonGroups:%i\n",numTriangleGroups,numPolygonGroups);
+						}
+					}//fi
+				}//for each geometry
+
+			}//for all geometry libraries
+
+
+			//m_dom->getLibrary_physics_models_array()
+
+			for (  i = 0; i < m_dom->getLibrary_physics_scenes_array().getCount(); i++)
+			{
+				domLibrary_physics_scenesRef physicsScenesRef = m_dom->getLibrary_physics_scenes_array()[i];
+				for (unsigned int s=0;s<physicsScenesRef->getPhysics_scene_array().getCount();s++)
+				{
+					domPhysics_sceneRef physicsSceneRef = physicsScenesRef->getPhysics_scene_array()[s];
+
+					if (physicsSceneRef->getTechnique_common())
+					{
+						if (physicsSceneRef->getTechnique_common()->getGravity())
+						{
+							const domFloat3 grav = physicsSceneRef->getTechnique_common()->getGravity()->getValue();
+							printf("gravity set to %f,%f,%f\n",grav.get(0),grav.get(1),grav.get(2));
+
+							setGravity(btVector3(grav.get(0),grav.get(1),grav.get(2)));
+						}
+
+					} 
+
+					for (unsigned int ps=0;ps<physicsSceneRef->getInstance_physics_model_array().getCount();ps++)
+					{
+						domInstance_physics_modelRef instance_physicsModelRef = physicsSceneRef->getInstance_physics_model_array()[ps];
+
+						daeElementRef ref = instance_physicsModelRef->getUrl().getElement();
+
+						domPhysics_modelRef model = *(domPhysics_modelRef*)&ref; 
+
+
+						unsigned int p,r;
+						for ( p=0;p<model->getInstance_physics_model_array().getCount();p++)
+						{
+							domInstance_physics_modelRef	instancePhysicsModelRef = model->getInstance_physics_model_array()[p];
+
+							daeElementRef ref = instancePhysicsModelRef->getUrl().getElement();
+	
+							domPhysics_modelRef model = *(domPhysics_modelRef*)&ref; 
+
+							//todo: group some shared functionality in following 2 'blocks'.
+							for (r=0;r<instancePhysicsModelRef->getInstance_rigid_body_array().getCount();r++)
+							{
+								domInstance_rigid_bodyRef instRigidbodyRef = instancePhysicsModelRef->getInstance_rigid_body_array()[r];
+
+								float mass = 1.f;
+								bool isDynamics = true;
+								btCollisionShape* colShape = 0;
+								btCompoundShape* compoundShape = 0;
+
+								xsNCName bodyName = instRigidbodyRef->getBody();
+
+								domInstance_rigid_body::domTechnique_commonRef techniqueRef = instRigidbodyRef->getTechnique_common();
+								if (techniqueRef)
+								{
+									if (techniqueRef->getMass())
+									{
+										mass = techniqueRef->getMass()->getValue();
+									}
+									if (techniqueRef->getDynamic())
+									{
+										isDynamics = techniqueRef->getDynamic()->getValue();
+									}
+								}
+
+								printf("mass = %f, isDynamics %i\n",mass,isDynamics);
+
+								if (bodyName && model)
+								{
+									//try to find the rigid body
+
+									for (unsigned int r=0;r<model->getRigid_body_array().getCount();r++)
+									{
+										domRigid_bodyRef rigidBodyRef = model->getRigid_body_array()[r];
+										if (rigidBodyRef->getSid() && !strcmp(rigidBodyRef->getSid(),bodyName))
+										{
+
+
+											btRigidBodyOutput output;
+											output.m_colShape = colShape;
+											output.m_compoundShape = compoundShape;
+
+											btRigidBodyInput rbInput;
+											rbInput.m_rigidBodyRef2 = rigidBodyRef;
+											rbInput.m_instanceRigidBodyRef = instRigidbodyRef;
+											ConvertRigidBodyRef( rbInput , output );
+
+											mass = output.m_mass;
+											isDynamics = output.m_isDynamics;
+											colShape = output.m_colShape;
+											compoundShape = output.m_compoundShape;
+
+										}
+									}
+
+									//////////////////////
+								}
+
+								if (compoundShape)
+									colShape = compoundShape;
+
+								if (colShape)
+								{
+									btRigidBodyInput input;
+									input.m_instanceRigidBodyRef = instRigidbodyRef;
+									input.m_rigidBodyRef2 = 0;
+									input.m_bodyName = (char*)bodyName;
+									PreparePhysicsObject(input, isDynamics,mass,colShape);
+								}
+							}
+						}
+
+
+						for (r=0;r<instance_physicsModelRef->getInstance_rigid_body_array().getCount();r++)
+						{
+
+							domInstance_rigid_bodyRef instRigidbodyRef = instance_physicsModelRef->getInstance_rigid_body_array()[r];
+
+							
+
+							float mass = 1.f;
+							bool isDynamics = true;
+							btCollisionShape* colShape = 0;
+							btCompoundShape* compoundShape = 0;
+
+							xsNCName bodyName = instRigidbodyRef->getBody();
+
+							domInstance_rigid_body::domTechnique_commonRef techniqueRef = instRigidbodyRef->getTechnique_common();
+							if (techniqueRef)
+							{
+								if (techniqueRef->getMass())
+								{
+									mass = techniqueRef->getMass()->getValue();
+								}
+								if (techniqueRef->getDynamic())
+								{
+									isDynamics = techniqueRef->getDynamic()->getValue();
+								}
+							}
+
+							printf("mass = %f, isDynamics %i\n",mass,isDynamics);
+
+							if (bodyName && model)
+							{
+								//try to find the rigid body
+
+								for (unsigned int r=0;r<model->getRigid_body_array().getCount();r++)
+								{
+									domRigid_bodyRef rigidBodyRef = model->getRigid_body_array()[r];
+									if (rigidBodyRef->getSid() && !strcmp(rigidBodyRef->getSid(),bodyName))
+									{
+
+
+										btRigidBodyOutput output;
+										output.m_colShape = colShape;
+										output.m_compoundShape = compoundShape;
+
+										btRigidBodyInput rbInput;
+										rbInput.m_rigidBodyRef2 = rigidBodyRef;
+										rbInput.m_instanceRigidBodyRef = instRigidbodyRef;
+										ConvertRigidBodyRef( rbInput , output );
+
+										mass = output.m_mass;
+										isDynamics = output.m_isDynamics;
+										colShape = output.m_colShape;
+										compoundShape = output.m_compoundShape;
+
+									}
+								}
+
+								//////////////////////
+							}
+
+							if (compoundShape)
+								colShape = compoundShape;
+
+							if (colShape)
+							{
+								btRigidBodyInput input;
+								input.m_instanceRigidBodyRef = instRigidbodyRef;
+								input.m_rigidBodyRef2 = 0;
+								input.m_bodyName = (char*)bodyName;
+								PreparePhysicsObject(input, isDynamics,mass,colShape);
+							}
+
+						} //for  each  instance_rigid_body
+
+						
+					} //for each physics model
+
+					
+					//handle constraints
+					for (unsigned int ma=0;ma<physicsSceneRef->getInstance_physics_model_array().getCount();ma++)
+					{
+						domInstance_physics_modelRef instance_physicsModelRef = physicsSceneRef->getInstance_physics_model_array()[ma];
+
+						daeElementRef ref = instance_physicsModelRef->getUrl().getElement();
+
+						domPhysics_modelRef model = *(domPhysics_modelRef*)&ref; 
+
+						{
+							ConstraintInput cInput;
+							cInput.m_instance_physicsModelRef = instance_physicsModelRef;
+							cInput.m_model = model;
+							prepareConstraints(cInput);
+						}
+
+						//also don't forget the model's 'instance_physics_models!
+						for ( unsigned int p=0;p<model->getInstance_physics_model_array().getCount();p++)
+						{
+							domInstance_physics_modelRef	instancePhysicsModelRef = model->getInstance_physics_model_array()[p];
+
+							daeElementRef ref = instancePhysicsModelRef->getUrl().getElement();
+	
+							domPhysics_modelRef model = *(domPhysics_modelRef*)&ref; 
+							
+							ConstraintInput cInput;
+							cInput.m_instance_physicsModelRef = instancePhysicsModelRef;
+							cInput.m_model = model;
+							prepareConstraints(cInput);
+						}
+
+											
+					} //2nd time, for each physics model
+
+				}
+			}
+
+			return true;
+}
+
+
+void	ColladaConverter::prepareConstraints(ConstraintInput& input)
+{
+	domInstance_physics_modelRef instance_physicsModelRef = input.m_instance_physicsModelRef;
+	domPhysics_modelRef model = input.m_model;
+
+	for (unsigned int c=0;c<instance_physicsModelRef->getInstance_rigid_constraint_array().getCount();c++)
+	{
+		domInstance_rigid_constraintRef constraintRef = instance_physicsModelRef->getInstance_rigid_constraint_array().get(c);
+		xsNCName constraintName = constraintRef->getConstraint();
+
+		if (constraintName && model)
+		{
+			//try to find the rigid body
+			int numConstraints= model->getRigid_constraint_array().getCount();
+
+			for (int r=0;r<numConstraints;r++)
+			{
+				domRigid_constraintRef rigidConstraintRef = model->getRigid_constraint_array()[r];
+				
+				if (rigidConstraintRef->getSid() && !strcmp(rigidConstraintRef->getSid(),constraintName))
+				{
+					
+					//two bodies
+					const domRigid_constraint::domRef_attachmentRef attachRefBody = rigidConstraintRef->getRef_attachment();
+					const domRigid_constraint::domAttachmentRef attachBody1 = rigidConstraintRef->getAttachment();
+
+					daeString orgUri0 = attachRefBody->getRigid_body().getOriginalURI();
+					daeString orgUri1 = attachBody1->getRigid_body().getOriginalURI();
+					btRigidBody* body0=0,*body1=0;
+					
+					for (int i=0;i<m_numObjects;i++)
+					{
+						char* bodyName = (char*)m_rigidBodies[i]->m_userObjectPointer;
+						if (!strcmp(bodyName,orgUri0))
+						{
+							body0=m_rigidBodies[i];
+						}
+						if (!strcmp(bodyName,orgUri1))
+						{
+							body1=m_rigidBodies[i];
+						}
+					}
+
+
+
+					const domRigid_constraint::domAttachmentRef attachOtherBody = rigidConstraintRef->getAttachment();
+
+					
+					const domRigid_constraint::domTechnique_commonRef commonRef = rigidConstraintRef->getTechnique_common();
+					
+					domFloat3 flMin = commonRef->getLimits()->getLinear()->getMin()->getValue();
+					btVector3 minLinearLimit(flMin.get(0),flMin.get(1),flMin.get(2));
+					
+					domFloat3 flMax = commonRef->getLimits()->getLinear()->getMax()->getValue();
+					btVector3 maxLinearLimit(flMax.get(0),flMax.get(1),flMax.get(2));
+														
+					domFloat3 coneMinLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMin()->getValue();
+					btVector3 angularMin(coneMinLimit.get(0),coneMinLimit.get(1),coneMinLimit.get(2));
+
+					domFloat3 coneMaxLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMax()->getValue();
+					btVector3 angularMax(coneMaxLimit.get(0),coneMaxLimit.get(1),coneMaxLimit.get(2));
+
+					{
+						
+						btTransform attachFrameRef0;
+						attachFrameRef0 = 
+							GetbtTransformFromCOLLADA_DOM
+							(
+							emptyMatrixArray,
+							attachRefBody->getRotate_array(),
+							attachRefBody->getTranslate_array());
+
+						btTransform attachFrameOther;
+						attachFrameOther =
+							GetbtTransformFromCOLLADA_DOM
+							(
+							emptyMatrixArray,
+							attachBody1->getRotate_array(),
+							attachBody1->getTranslate_array()
+							);
+
+
+						//convert INF / -INF into lower > upper
+
+						//currently there is a hack in the DOM to detect INF / -INF
+						//see daeMetaAttribute.cpp
+						//INF -> 999999.9
+						//-INF -> -999999.9
+						float linearCheckTreshold = 999999.0;
+						float angularCheckTreshold = 180.0;//check this
+
+
+
+						
+						//free means upper < lower, 
+						//locked means upper == lower
+						//limited means upper > lower
+						//limitIndex: first 3 are linear, next 3 are angular
+
+						btVector3 linearLowerLimits = minLinearLimit;
+						btVector3 linearUpperLimits = maxLinearLimit;
+						btVector3 angularLowerLimits = angularMin;
+						btVector3 angularUpperLimits = angularMax;
+						{
+							for (int i=0;i<3;i++)
+							{
+								if  ((linearLowerLimits[i] < -linearCheckTreshold) ||
+									(linearUpperLimits[i] > linearCheckTreshold))
+								{
+									//disable limits
+									linearLowerLimits[i] = 1;
+									linearUpperLimits[i] = 0;
+								}
+
+								if  ((angularLowerLimits[i] < -angularCheckTreshold) ||
+									(angularUpperLimits[i] > angularCheckTreshold))
+								{
+									//disable limits
+									angularLowerLimits[i] = 1;
+									angularUpperLimits[i] = 0;
+								}
+							}
+						}
+
+
+						if (body0&& body1)
+						{
+						createUniversalD6Constraint(
+						body0,
+						body1,
+						attachFrameRef0,
+						attachFrameOther,
+						linearLowerLimits,
+						linearUpperLimits,
+						angularLowerLimits,
+						angularUpperLimits
+							);
+						} else
+						{
+							printf("Error: Cannot find Rigidbodies(%s,%s) for constraint %s\n",orgUri0,orgUri1,constraintName);
+						}
+
+
+					}
+
+
+
+				}
+			}
+		}
+
+	}
+
+}
+
+void	ColladaConverter::PreparePhysicsObject(struct btRigidBodyInput& input, bool isDynamics, float mass,btCollisionShape* colShape)
+{
+	btTransform startTransform;
+	startTransform.setIdentity();
+	btVector3 startScale(1.f,1.f,1.f);
+
+	//The 'target' points to a graphics element/node, which contains the start (world) transform
+	daeElementRef elem = input.m_instanceRigidBodyRef->getTarget().getElement();
+	if (elem)
+	{
+		domNodeRef node = *(domNodeRef*)&elem;
+		m_colladadomNodes[m_numObjects] = node;
+
+		//find transform of the node that this rigidbody maps to
+
+
+		startTransform = GetbtTransformFromCOLLADA_DOM(
+							node->getMatrix_array(),
+							node->getRotate_array(),
+							node->getTranslate_array()
+							);
+
+		unsigned int i;
+		for (i=0;i<node->getScale_array().getCount();i++)
+		{
+			domScaleRef scaleRef = node->getScale_array()[i];
+			domFloat3 fl3 = scaleRef->getValue();
+			startScale = btVector3(fl3.get(0),fl3.get(1),fl3.get(2));
+		}
+
+	}
+
+
+
+	btRigidBody* body= createRigidBody(isDynamics,mass,startTransform,colShape);
+	if (body)
+	{
+		//for bodyName lookup in constraints
+		body->m_userObjectPointer = (void*)input.m_bodyName;
+		m_rigidBodies[m_numObjects] = body;
+		m_numObjects++;
+	}
+
+}
+
+bool ColladaConverter::saveAs(const char* filename)
+{
+	if (m_collada)
+		{
+			for (int i=0;i<m_numObjects;i++)
+			{
+				assert(m_colladadomNodes[i]);
+				if (!m_colladadomNodes[i]->getTranslate_array().getCount())
+				{
+					domTranslate* transl = (domTranslate*) m_colladadomNodes[i]->createAndPlace("translate");
+					transl->getValue().append(0.);
+					transl->getValue().append(0.);
+					transl->getValue().append(0.);
+				}
+
+				while (m_colladadomNodes[i]->getTranslate_array().getCount() > 1)
+				{
+					m_colladadomNodes[i]->removeFromParent(m_colladadomNodes[i]->getTranslate_array().get(1));
+					//m_colladadomNodes[i]->getTranslate_array().removeIndex(1);
+				}
+
+				{
+
+					btVector3 np = m_rigidBodies[i]->m_worldTransform.getOrigin();
+					domFloat3 newPos = m_colladadomNodes[i]->getTranslate_array().get(0)->getValue();
+					newPos.set(0,np[0]);
+					newPos.set(1,np[1]);
+					newPos.set(2,np[2]);
+					m_colladadomNodes[i]->getTranslate_array().get(0)->setValue(newPos);
+
+				}
+				
+
+				if (!m_colladadomNodes[i]->getRotate_array().getCount())
+				{
+					domRotate* rot = (domRotate*)m_colladadomNodes[i]->createAndPlace("rotate");
+					rot->getValue().append(1.0);
+					rot->getValue().append(0.0);
+					rot->getValue().append(0.0);
+					rot->getValue().append(0.0);
+				}
+
+				while (m_colladadomNodes[i]->getRotate_array().getCount()>1)
+				{
+					m_colladadomNodes[i]->removeFromParent(m_colladadomNodes[i]->getRotate_array().get(1));
+					//m_colladadomNodes[i]->getRotate_array().removeIndex(1);
+
+				}
+
+				{
+					btQuaternion quat = m_rigidBodies[i]->getCenterOfMassTransform().getRotation();
+					btVector3 axis(quat.getX(),quat.getY(),quat.getZ());
+					axis[3] = 0.f;
+					//check for axis length
+					btScalar len = axis.length2();
+					if (len < SIMD_EPSILON*SIMD_EPSILON)
+						axis = btVector3(1.f,0.f,0.f);
+					else
+						axis /= btSqrt(len);
+					m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(0,axis[0]);
+					m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(1,axis[1]);
+					m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(2,axis[2]);
+					m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(3,quat.getAngle()*SIMD_DEGS_PER_RAD);
+				}
+
+				while (m_colladadomNodes[i]->getMatrix_array().getCount())
+				{
+					m_colladadomNodes[i]->removeFromParent(m_colladadomNodes[i]->getMatrix_array().get(0));
+					//m_colladadomNodes[i]->getMatrix_array().removeIndex(0);
+				}
+			}
+			char	saveName[550];
+			static int saveCount=1;
+			sprintf(saveName,"%s%i",getLastFileName(),saveCount++);
+			char* name = &saveName[0];
+			if (name[0] == '/')
+			{
+				name = &saveName[1];
+			} 
+			
+			if (m_dom->getAsset()->getContributor_array().getCount())
+			{
+				if (!m_dom->getAsset()->getContributor_array().get(0)->getAuthor())
+				{
+					m_dom->getAsset()->getContributor_array().get(0)->createAndPlace("author");
+				}
+
+				m_dom->getAsset()->getContributor_array().get(0)->getAuthor()->setValue
+					("http://bullet.sf.net Erwin Coumans");
+
+				if (!m_dom->getAsset()->getContributor_array().get(0)->getAuthoring_tool())
+				{
+					m_dom->getAsset()->getContributor_array().get(0)->createAndPlace("authoring_tool");
+				}
+
+				m_dom->getAsset()->getContributor_array().get(0)->getAuthoring_tool()->setValue
+#ifdef WIN32
+					("Bullet ColladaPhysicsViewer-Win32-0.8");
+#else
+#ifdef __APPLE__
+					("Bullet ColladaPhysicsViewer-MacOSX-0.8");
+#else
+					("Bullet ColladaPhysicsViewer-UnknownPlatform-0.8");
+#endif
+#endif
+				if (!m_dom->getAsset()->getContributor_array().get(0)->getComments())
+				{
+					m_dom->getAsset()->getContributor_array().get(0)->createAndPlace("comments");
+				}
+				 m_dom->getAsset()->getContributor_array().get(0)->getComments()->setValue
+					 ("Comments to Physics Forum at http://www.continuousphysics.com/Bullet/phpBB2/index.php");
+			}
+
+			m_collada->saveAs(name);
+			return true;
+			
+		}
+		return false;
+}
+
+//some code that de-mangles the windows filename passed in as argument
+char cleaned_filename[512];
+char* getLastFileName()
+{
+	return cleaned_filename;
+}
+
+
+char* fixFileName(const char* lpCmdLine)
+{
+
+
+	// We might get a windows-style path on the command line, this can mess up the DOM which expects
+	// all paths to be URI's.  This block of code does some conversion to try and make the input
+	// compliant without breaking the ability to accept a properly formatted URI.  Right now this only
+	// displays the first filename
+	const char *in = lpCmdLine;
+	char* out = cleaned_filename;
+	*out = '\0';
+	// If the first character is a ", skip it (filenames with spaces in them are quoted)
+	if(*in == '\"')
+	{
+		in++;
+	}
+	if(*(in+1) == ':')
+	{
+		// Second character is a :, assume we have a path with a drive letter and add a slash at the beginning
+		*(out++) = '/';
+	}
+	int i;
+	for(i =0; i<512; i++)
+	{
+		// If we hit a null or a quote, stop copying.  This will get just the first filename.
+		if(*in == '\0' || *in == '\"')
+			break;
+		// Copy while swapping backslashes for forward ones
+		if(*in == '\\')
+		{
+			*out = '/';
+		}
+		else
+		{
+			*out = *in;
+		}
+		in++;
+		out++;
+	}
+	
+	return cleaned_filename;
+}
+
+
+void	ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBodyOutput& rbOutput)
+{
+	
+	const domRigid_body::domTechnique_commonRef techniqueRef = rbInput.m_rigidBodyRef2->getTechnique_common();
+	if (techniqueRef)
+	{
+
+		if (techniqueRef->getMass())
+		{
+			rbOutput.m_mass = techniqueRef->getMass()->getValue();
+		}
+		if (techniqueRef->getDynamic())
+		{
+			rbOutput.m_isDynamics = techniqueRef->getDynamic()->getValue();
+		}
+
+		//shapes
+		for (unsigned int s=0;s<techniqueRef->getShape_array().getCount();s++)
+		{
+			domRigid_body::domTechnique_common::domShapeRef shapeRef = techniqueRef->getShape_array()[s];
+
+			if (shapeRef->getPlane())
+			{
+				domPlaneRef planeRef = shapeRef->getPlane();
+				if (planeRef->getEquation())
+				{
+					const domFloat4 planeEq = planeRef->getEquation()->getValue();
+					btVector3 planeNormal(planeEq.get(0),planeEq.get(1),planeEq.get(2));
+					btScalar planeConstant = planeEq.get(3);
+					rbOutput.m_colShape = new btStaticPlaneShape(planeNormal,planeConstant);
+				}
+
+			}
+
+			if (shapeRef->getBox())
+			{
+				domBoxRef boxRef = shapeRef->getBox();
+				domBox::domHalf_extentsRef	domHalfExtentsRef = boxRef->getHalf_extents();
+				domFloat3& halfExtents = domHalfExtentsRef->getValue();
+				float x = halfExtents.get(0);
+				float y = halfExtents.get(1);
+				float z = halfExtents.get(2);
+				rbOutput.m_colShape = new btBoxShape(btVector3(x,y,z));
+			}
+			if (shapeRef->getSphere())
+			{
+				domSphereRef sphereRef = shapeRef->getSphere();
+				domSphere::domRadiusRef radiusRef = sphereRef->getRadius();
+				domFloat radius = radiusRef->getValue();
+				rbOutput.m_colShape = new btSphereShape(radius);
+			}
+
+			if (shapeRef->getCylinder())
+			{
+				domCylinderRef cylinderRef = shapeRef->getCylinder();
+				domFloat height = cylinderRef->getHeight()->getValue();
+				domFloat2 radius2 = cylinderRef->getRadius()->getValue();
+				domFloat radius0 = radius2.get(0);
+
+				//Cylinder around the local Y axis
+				rbOutput.m_colShape = new btCylinderShape(btVector3(radius0,height,radius0));
+
+			}
+
+			if (shapeRef->getInstance_geometry())
+			{
+				const domInstance_geometryRef geomInstRef = shapeRef->getInstance_geometry();
+				daeElement* geomElem = geomInstRef->getUrl().getElement();
+				//elemRef->getTypeName();
+				domGeometry* geom = (domGeometry*) geomElem;
+				if (geom && geom->getMesh())
+				{
+					const domMeshRef meshRef = geom->getMesh();
+
+					btTriangleMesh* trimesh = new btTriangleMesh();
+
+					
+					for (unsigned int tg = 0;tg<meshRef->getTriangles_array().getCount();tg++)
+					{
+
+
+						domTrianglesRef triRef = meshRef->getTriangles_array()[tg];
+						const domPRef pRef = triRef->getP();
+						btIndexedMesh meshPart;
+						meshPart.m_triangleIndexStride=0;
+
+
+						
+						int vertexoffset = -1;
+						domInputLocalOffsetRef indexOffsetRef;
+						
+
+						for (unsigned int w=0;w<triRef->getInput_array().getCount();w++)
+						{
+							int offset = triRef->getInput_array()[w]->getOffset();
+							daeString str = triRef->getInput_array()[w]->getSemantic();
+							if (!strcmp(str,"VERTEX"))
+							{
+								indexOffsetRef = triRef->getInput_array()[w];
+								vertexoffset = offset;
+							}
+							if (offset > meshPart.m_triangleIndexStride)
+							{
+								meshPart.m_triangleIndexStride = offset;
+							}
+						}
+						meshPart.m_triangleIndexStride++;
+						domListOfUInts indexArray =triRef->getP()->getValue(); 
+
+						//int*		m_triangleIndexBase;
+
+
+
+						meshPart.m_numTriangles = triRef->getCount();
+
+						const domVerticesRef vertsRef = meshRef->getVertices();
+						int numInputs = vertsRef->getInput_array().getCount();
+						for (int i=0;i<numInputs;i++)
+						{
+							domInputLocalRef localRef = vertsRef->getInput_array()[i];
+							daeString str = localRef->getSemantic();
+							if ( !strcmp(str,"POSITION"))
+							{
+								const domURIFragmentType& frag = localRef->getSource();
+
+								daeElementConstRef constElem = frag.getElement();
+
+								const domSourceRef node = *(const domSourceRef*)&constElem;
+								const domFloat_arrayRef flArray = node->getFloat_array();
+								if (flArray)
+								{
+									const domListOfFloats& listFloats = flArray->getValue();
+
+									int k=vertexoffset;
+									int t=0;
+									int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
+									for (;t<meshPart.m_numTriangles;t++)
+									{
+										btVector3 verts[3];
+										int index0;
+										for (int i=0;i<3;i++)
+										{
+											index0 = indexArray.get(k)*vertexStride;
+											domFloat fl0 = listFloats.get(index0);
+											domFloat fl1 = listFloats.get(index0+1);
+											domFloat fl2 = listFloats.get(index0+2);
+											k+=meshPart.m_triangleIndexStride;
+											verts[i].setValue(fl0,fl1,fl2);
+										}
+										trimesh->addTriangle(verts[0],verts[1],verts[2]);
+									}
+								}
+							}
+						}
+
+
+
+
+
+							//int			m_triangleIndexStride;//calculate max offset
+							//int			m_numVertices;
+							//float*		m_vertexBase;//getRawData on floatArray
+							//int			m_vertexStride;//use the accessor for this
+
+						//};
+						//tindexArray->addIndexedMesh(meshPart);
+						if (rbOutput.m_isDynamics)
+						{
+							printf("moving concave <mesh> not supported, transformed into convex\n");
+							rbOutput.m_colShape = new btConvexTriangleMeshShape(trimesh);
+						} else
+						{
+							printf("static concave triangle <mesh> added\n");
+							rbOutput.m_colShape = new btBvhTriangleMeshShape(trimesh);
+							//rbOutput.m_colShape = new btBvhTriangleMeshShape(trimesh);
+							//rbOutput.m_colShape = new btConvexTriangleMeshShape(trimesh);
+							
+							//btTriangleMeshShape
+						}
+
+					}
+
+				}
+
+				if (geom && geom->getConvex_mesh())
+				{
+
+					{
+						const domConvex_meshRef convexRef = geom->getConvex_mesh();
+						daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
+						if ( otherElemRef != NULL )
+						{
+							domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
+							printf( "otherLinked\n");
+						} else
+						{
+							printf("convexMesh polyCount = %i\n",convexRef->getPolygons_array().getCount());
+							printf("convexMesh triCount = %i\n",convexRef->getTriangles_array().getCount());
+
+						}
+					}
+
+
+
+					btConvexHullShape* convexHullShape = new btConvexHullShape(0,0);
+
+					//it is quite a trick to get to the vertices, using Collada.
+					//we are not there yet...
+
+					const domConvex_meshRef convexRef = geom->getConvex_mesh();
+					//daeString urlref = convexRef->getConvex_hull_of().getURI();
+					daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI();
+					if (urlref2)
+					{
+						daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
+						//	if ( otherElemRef != NULL )
+						//	{
+						//		domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
+
+						// Load all the geometry libraries
+						for ( unsigned int i = 0; i < m_dom->getLibrary_geometries_array().getCount(); i++)
+						{
+							domLibrary_geometriesRef libgeom = m_dom->getLibrary_geometries_array()[i];
+							//int index = libgeom->findLastIndexOf(urlref2);
+							//can't find it
+
+							for ( unsigned int  i = 0; i < libgeom->getGeometry_array().getCount(); i++)
+							{
+								//ReadGeometry(  ); 
+								domGeometryRef lib = libgeom->getGeometry_array()[i];
+								if (!strcmp(lib->getName(),urlref2))
+								{
+									//found convex_hull geometry
+									domMesh			*meshElement		= lib->getMesh();//linkedGeom->getMesh();
+									if (meshElement)
+									{
+										const domVerticesRef vertsRef = meshElement->getVertices();
+										int numInputs = vertsRef->getInput_array().getCount();
+										for (int i=0;i<numInputs;i++)
+										{
+											domInputLocalRef localRef = vertsRef->getInput_array()[i];
+											daeString str = localRef->getSemantic();
+											if ( !strcmp(str,"POSITION"))
+											{
+												const domURIFragmentType& frag = localRef->getSource();
+
+												daeElementConstRef constElem = frag.getElement();
+
+												const domSourceRef node = *(const domSourceRef*)&constElem;
+												const domFloat_arrayRef flArray = node->getFloat_array();
+												if (flArray)
+												{
+													int numElem = flArray->getCount();
+													const domListOfFloats& listFloats = flArray->getValue();
+
+													for (int k=0;k+2<numElem;k+=3)
+													{
+														domFloat fl0 = listFloats.get(k);
+														domFloat fl1 = listFloats.get(k+1);
+														domFloat fl2 = listFloats.get(k+2);
+														//printf("float %f %f %f\n",fl0,fl1,fl2);
+
+														convexHullShape->addPoint(btPoint3(fl0,fl1,fl2));
+													}
+
+												}
+
+											}
+
+
+										}
+
+									}
+								}
+							
+									
+								
+							}
+						}
+
+
+					} else
+					{
+						//no getConvex_hull_of but direct vertices
+						const domVerticesRef vertsRef = convexRef->getVertices();
+						int numInputs = vertsRef->getInput_array().getCount();
+						for (int i=0;i<numInputs;i++)
+						{
+							domInputLocalRef localRef = vertsRef->getInput_array()[i];
+							daeString str = localRef->getSemantic();
+							if ( !strcmp(str,"POSITION"))
+							{
+								const domURIFragmentType& frag = localRef->getSource();
+
+								daeElementConstRef constElem = frag.getElement();
+
+								const domSourceRef node = *(const domSourceRef*)&constElem;
+								const domFloat_arrayRef flArray = node->getFloat_array();
+								if (flArray)
+								{
+									int numElem = flArray->getCount();
+									const domListOfFloats& listFloats = flArray->getValue();
+
+									for (int k=0;k+2<numElem;k+=3)
+									{
+										domFloat fl0 = listFloats.get(k);
+										domFloat fl1 = listFloats.get(k+1);
+										domFloat fl2 = listFloats.get(k+2);
+										//printf("float %f %f %f\n",fl0,fl1,fl2);
+
+										convexHullShape->addPoint(btPoint3(fl0,fl1,fl2));
+									}
+
+								}
+
+							}
+
+
+						}
+
+
+					}
+
+					if (convexHullShape->getNumVertices())
+					{
+						rbOutput.m_colShape = convexHullShape;
+						printf("created convexHullShape with %i points\n",convexHullShape->getNumVertices());
+					} else
+					{
+						delete convexHullShape;
+						printf("failed to create convexHullShape\n");
+					}
+
+
+					//domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
+
+					printf("convexmesh\n");
+
+				}
+			}
+
+			//if more then 1 shape, or a non-identity local shapetransform
+			//use a compound
+
+			bool hasShapeLocalTransform = ((shapeRef->getRotate_array().getCount() > 0) ||
+				(shapeRef->getTranslate_array().getCount() > 0));
+			
+			if (rbOutput.m_colShape)
+			{
+				if ((techniqueRef->getShape_array().getCount()>1) ||
+					(hasShapeLocalTransform))
+				{
+					
+					if (!rbOutput.m_compoundShape)
+					{
+						rbOutput.m_compoundShape = new btCompoundShape();
+					}
+
+					btTransform localTransform;
+					localTransform.setIdentity();
+					if (hasShapeLocalTransform)
+					{
+					localTransform = GetbtTransformFromCOLLADA_DOM(
+						emptyMatrixArray,
+						shapeRef->getRotate_array(),
+						shapeRef->getTranslate_array()
+						);
+					}
+
+					rbOutput.m_compoundShape->addChildShape(localTransform,rbOutput.m_colShape);
+					rbOutput.m_colShape = 0;
+				}
+			}
+
+
+		}//for each shape
+	}
+
+
+}
diff --git a/Demos/ColladaDemo/ColladaDemo.cpp b/Demos/ColladaDemo/ColladaDemo.cpp
index 54eec709d..01d522913 100644
--- a/Demos/ColladaDemo/ColladaDemo.cpp
+++ b/Demos/ColladaDemo/ColladaDemo.cpp
@@ -100,7 +100,7 @@ class MyColladaConverter : public ColladaConverter
 
 	virtual	void	setGravity(const btVector3& grav)
 	{
-		m_demoApp->setGravity(grav);
+		m_demoApp->getDynamicsWorld()->setGravity(grav);
 	}
 	virtual void	setCameraInfo(const btVector3& camUp,int forwardAxis) 
 	{
diff --git a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
index 3b7bd1090..267ca89ea 100644
--- a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
+++ b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
@@ -60,12 +60,12 @@ bool CustomMaterialCombinerCallback(btManifoldPoint& cp,	const btCollisionObject
 	float restitution0 = colObj0->getRestitution();
 	float restitution1 = colObj1->getRestitution();
 
-	if (colObj0->m_collisionFlags & btCollisionObject::customMaterialCallback)
+	if (colObj0->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)
 	{
 		friction0 = 1.0;//partId0,index0
 		restitution0 = 0.f;
 	}
-	if (colObj1->m_collisionFlags & btCollisionObject::customMaterialCallback)
+	if (colObj1->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)
 	{
 		if (index1&1)
 		{
@@ -161,10 +161,10 @@ void	ConcaveDemo::initPhysics()
 
 	btRigidBody* staticBody = localCreateRigidBody(mass, startTransform,trimeshShape);
 
-	staticBody->m_collisionFlags |=btCollisionObject::isStatic;
+	staticBody->m_collisionFlags |=btCollisionObject::CF_STATIC_OBJECT;
 
 	//enable custom material callback
-	staticBody->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+	staticBody->m_collisionFlags |= btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK;
 
 	{
 		for (int i=0;i<10;i++)
diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp
index 41c7272ac..c0c2d5148 100644
--- a/Demos/OpenGL/DemoApplication.cpp
+++ b/Demos/OpenGL/DemoApplication.cpp
@@ -24,8 +24,8 @@ subject to the following restrictions:
 #include "GL_ShapeDrawer.h"
 #include "LinearMath/btQuickprof.h"
 #include "BMF_Api.h"
-#include "BulletDynamics/Dynamics/btMassProps.h"
 
+extern bool gDisableDeactivation;
 int numObjects = 0;
 const int maxNumObjects = 16384;
 btTransform startTransforms[maxNumObjects];
@@ -37,7 +37,6 @@ DemoApplication::DemoApplication()
 :
 m_dynamicsWorld(0),
 m_pickConstraint(0),
-m_gravity(0,-10,0),
 	m_cameraDistance(15.0),
 	m_debugMode(0),
 	m_ele(0.f),
@@ -58,6 +57,7 @@ m_gravity(0,-10,0),
 }
 
 
+
 DemoApplication::~DemoApplication()
 {
 
@@ -290,7 +290,15 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y)
 				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;
+			
 
 		
 
@@ -346,6 +354,29 @@ void DemoApplication::specialKeyboard(int key, int x, int y)
 {
     switch (key) 
     {
+	case GLUT_KEY_F1:
+		{
+
+			break;
+		}
+
+	case GLUT_KEY_F2:
+	{
+
+		break;
+	}
+
+
+	case GLUT_KEY_END:
+		{
+			int numObj = getDynamicsWorld()->getNumCollisionObjects();
+			if (numObj)
+			{
+				btCollisionObject* obj = getDynamicsWorld()->getCollisionObjectArray()[numObj-1];
+				getDynamicsWorld()->removeCollisionObject(obj);
+			}
+			break;
+		}
     case GLUT_KEY_LEFT : stepLeft(); break;
     case GLUT_KEY_RIGHT : stepRight(); break;
     case GLUT_KEY_UP : stepFront(); break;
@@ -522,7 +553,8 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 						btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
 						if (body)
 						{
-							if (!body->IsStatic())
+							//other exclusions?
+							if (!(body->isStaticObject() || body->isKinematicObject()))
 							{
 								pickedBody = body;
 								pickedBody->SetActivationState(DISABLE_DEACTIVATION);
@@ -616,27 +648,8 @@ btRigidBody*	DemoApplication::localCreateRigidBody(float mass, const btTransform
 
 	btRigidBody* body = new btRigidBody(mass,startTransform,shape,localInertia);
 
-	//filtering allows to excluded collision pairs, early in the collision pipeline (broadphase)
-	bool useFiltering = true;
-	if (useFiltering)
-	{
-		short collisionFilterGroup = isDynamic? 
-			btBroadphaseProxy::DefaultFilter : 
-			btBroadphaseProxy::StaticFilter;
-		short collisionFilterMask = isDynamic? 
-			btBroadphaseProxy::AllFilter : 
-			btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
-
-			m_dynamicsWorld->addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
-	} else
-	{
-		//no collision filtering, so always create an overlapping pair, even between static-static etc.
-		m_dynamicsWorld->addCollisionObject(body);
-	}
-
-	//Bullet uses per-object gravity
-	body->setGravity(m_gravity);
-
+	m_dynamicsWorld->addRigidBody(body);
+	
 	return body;
 }
 
diff --git a/Demos/OpenGL/DemoApplication.h b/Demos/OpenGL/DemoApplication.h
index c2e85ad45..be43c7262 100644
--- a/Demos/OpenGL/DemoApplication.h
+++ b/Demos/OpenGL/DemoApplication.h
@@ -56,8 +56,6 @@ class DemoApplication
 	///constraint for mouse picking
 	btTypedConstraint*		m_pickConstraint;
 
-	btVector3	m_gravity;
-
 	float	m_cameraDistance;
 	int	m_debugMode;
 	
@@ -141,10 +139,7 @@ public:
 	///Demo functions
 	void	shootBox(const btVector3& destination);
 
-	void	setGravity(const btVector3& grav)
-	{
-		m_gravity = grav;
-	}
+
 	btVector3	getRayTo(int x,int y);
 
 	btRigidBody*	localCreateRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape);
diff --git a/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp b/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
index fb0ae899b..6a0cdc826 100644
--- a/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
+++ b/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
@@ -136,7 +136,7 @@ void	UserCollisionAlgorithm::initPhysics()
 
 	btRigidBody* staticBody= localCreateRigidBody(mass, startTransform,trimeshShape);
 	//enable custom material callback
-	staticBody->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+	staticBody->m_collisionFlags |= btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK;
 
 	{
 		for (int i=0;i<10;i++)
diff --git a/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
index 70fa11b2c..a57c76199 100644
--- a/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
+++ b/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
@@ -18,6 +18,9 @@ subject to the following restrictions:
 
 struct btBroadphaseProxy;
 class btDispatcher;
+class btManifoldResult;
+struct btCollisionObject;
+struct btDispatcherInfo;
 
 struct btCollisionAlgorithmConstructionInfo
 {
@@ -57,9 +60,9 @@ public:
 
 	virtual ~btCollisionAlgorithm() {};
 
-	virtual void processCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const struct btDispatcherInfo& dispatchInfo) = 0;
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
 
-	virtual float calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const struct btDispatcherInfo& dispatchInfo) = 0;
+	virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
 
 };
 
diff --git a/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/src/BulletCollision/BroadphaseCollision/btDispatcher.h
index a2c6aec76..f87c0281a 100644
--- a/src/BulletCollision/BroadphaseCollision/btDispatcher.h
+++ b/src/BulletCollision/BroadphaseCollision/btDispatcher.h
@@ -20,7 +20,6 @@ class btCollisionAlgorithm;
 struct btBroadphaseProxy;
 class btRigidBody;
 struct	btCollisionObject;
-class btManifoldResult;
 class btOverlappingPairCache;
 
 enum btCollisionDispatcherId
@@ -66,7 +65,7 @@ class btDispatcher
 public:
 	virtual ~btDispatcher() ;
 
-	virtual btCollisionAlgorithm* findAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1) = 0;
+	virtual btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1) = 0;
 
 	//
 	// asume dispatchers to have unique id's in the range [0..max dispacher]
@@ -79,13 +78,9 @@ public:
 
 	virtual void clearManifold(btPersistentManifold* manifold)=0;
 
-	virtual bool	needsCollision(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1) = 0;
+	virtual bool	needsCollision(btCollisionObject* body0,btCollisionObject* body1) = 0;
 
-	virtual bool	needsResponse(const btCollisionObject& colObj0,const btCollisionObject& colObj1)=0;
-
-	virtual	btManifoldResult* getNewManifoldResult(btCollisionObject* obj0,btCollisionObject* obj1,btPersistentManifold* manifold) =0;
-
-	virtual	void	releaseManifoldResult(btManifoldResult*)=0;
+	virtual bool	needsResponse(btCollisionObject* body0,btCollisionObject* body1)=0;
 
 	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)=0;
 
diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
index cc16a93c3..225b3ad2f 100644
--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
@@ -75,7 +75,7 @@ void	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa
 	//don't add overlap with own
 	assert(proxy0 != proxy1);
 
-	if (!needsCollision(proxy0,proxy1))
+	if (!needsBroadphaseCollision(proxy0,proxy1))
 		return;
 
 
@@ -92,7 +92,7 @@ void	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa
 ///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
  btBroadphasePair*	btOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
 {
-	if (!needsCollision(proxy0,proxy1))
+	if (!needsBroadphaseCollision(proxy0,proxy1))
 		return 0;
 
 	btBroadphasePair tmpPair(*proxy0,*proxy1);
diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
index 008349030..ab444b925 100644
--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
+++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
@@ -64,7 +64,7 @@ class	btOverlappingPairCache : public btBroadphaseInterface
 	void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy);
 
 
-	inline bool needsCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+	inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
 	{
 		bool collides = proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask;
 		collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
diff --git a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
index e0a7d25cd..1a24c7a7a 100644
--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
@@ -134,25 +134,7 @@ void	btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg)
 		assert (index < m_maxProxies);
 		m_freeProxies[--m_firstFreeProxy] = index;
 
-		//removeOverlappingPairsContainingProxy(proxyOrg);
-
-		assert(0);
-		//then remove non-overlapping ones
-		/*for (i=0;i<GetNumOverlappingPairs();i++)
-		{
-			btBroadphasePair& pair = GetOverlappingPair(i);
-
-			btSimpleBroadphaseProxy* proxy0 = getSimpleProxyFromProxy(pair.m_pProxy0);
-			btSimpleBroadphaseProxy* proxy1 = getSimpleProxyFromProxy(pair.m_pProxy1);
-			if ((proxy0==proxyOrg) || (proxy1==proxyOrg))
-			{
-				removeOverlappingPair(pair);
-			}
-		}
-		*/
-
-
-
+		removeOverlappingPairsContainingProxy(proxyOrg);
 		
 		for (i=0;i<m_numProxies;i++)
 		{
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
index a051dbb05..c76d1f999 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
@@ -20,7 +20,8 @@ subject to the following restrictions:
 
 typedef std::vector<struct btCollisionObject*> btCollisionObjectArray;
 class btCollisionAlgorithm;
-struct btBroadphaseProxy;
+struct btCollisionObject;
+
 struct btCollisionAlgorithmConstructionInfo;
 
 
@@ -34,7 +35,7 @@ struct btCollisionAlgorithmCreateFunc
 	}
 	virtual ~btCollisionAlgorithmCreateFunc(){};
 
-	virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 	{
 		return 0;
 	}
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
index a72379ce1..7eaf1c4f0 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
@@ -29,13 +29,13 @@ subject to the following restrictions:
 
 int gNumManifold = 0;
 
+#include <stdio.h>
 
 	
 
 	
 btCollisionDispatcher::btCollisionDispatcher (): 
 	m_useIslands(true),
-		m_defaultManifoldResult(0,0,0),
 		m_count(0)
 {
 	int i;
@@ -121,18 +121,17 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 
 	
 
-btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
+btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1)
 {
 #define USE_DISPATCH_REGISTRY_ARRAY 1
 #ifdef USE_DISPATCH_REGISTRY_ARRAY
-	btCollisionObject* body0 = (btCollisionObject*)proxy0.m_clientObject;
-	btCollisionObject* body1 = (btCollisionObject*)proxy1.m_clientObject;
+	
 	btCollisionAlgorithmConstructionInfo ci;
 	ci.m_dispatcher = this;
 	btCollisionAlgorithm* algo = m_doubleDispatch[body0->m_collisionShape->getShapeType()][body1->m_collisionShape->getShapeType()]
-	->CreateCollisionAlgorithm(ci,&proxy0,&proxy1);
+	->CreateCollisionAlgorithm(ci,body0,body1);
 #else
-	btCollisionAlgorithm* algo = internalFindAlgorithm(proxy0,proxy1);
+	btCollisionAlgorithm* algo = internalFindAlgorithm(body0,body1);
 #endif //USE_DISPATCH_REGISTRY_ARRAY
 	return algo;
 }
@@ -173,38 +172,36 @@ btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(in
 
 
 
-btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
+btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1)
 {
 	m_count++;
-	btCollisionObject* body0 = (btCollisionObject*)proxy0.m_clientObject;
-	btCollisionObject* body1 = (btCollisionObject*)proxy1.m_clientObject;
-
+	
 	btCollisionAlgorithmConstructionInfo ci;
 	ci.m_dispatcher = this;
 	
 	if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConvex() )
 	{
-		return new btConvexConvexAlgorithm(0,ci,&proxy0,&proxy1);			
+		return new btConvexConvexAlgorithm(0,ci,body0,body1);
 	}
 
 	if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConcave())
 	{
-		return new btConvexConcaveCollisionAlgorithm(ci,&proxy0,&proxy1);
+		return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
 	}
 
 	if (body1->m_collisionShape->isConvex() && body0->m_collisionShape->isConcave())
 	{
-		return new btConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0);
+		return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
 	}
 
 	if (body0->m_collisionShape->isCompound())
 	{
-		return new btCompoundCollisionAlgorithm(ci,&proxy0,&proxy1);
+		return new btCompoundCollisionAlgorithm(ci,body0,body1,false);
 	} else
 	{
 		if (body1->m_collisionShape->isCompound())
 		{
-			return new btCompoundCollisionAlgorithm(ci,&proxy1,&proxy0);
+			return new btCompoundCollisionAlgorithm(ci,body0,body1,true);
 		}
 	}
 
@@ -213,33 +210,29 @@ btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btBroadphaseP
 	
 }
 
-bool	btCollisionDispatcher::needsResponse(const  btCollisionObject& colObj0,const btCollisionObject& colObj1)
+bool	btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1)
 {
-
-	
 	//here you can do filtering
 	bool hasResponse = 
-		(!(colObj0.m_collisionFlags & btCollisionObject::noContactResponse)) &&
-		(!(colObj1.m_collisionFlags & btCollisionObject::noContactResponse));
+		(body0->hasContactResponse() && body1->hasContactResponse());
 	hasResponse = hasResponse &&
-		(colObj0.IsActive() || colObj1.IsActive());
+		(body0->IsActive() || body1->IsActive());
 	return hasResponse;
 }
 
-bool	btCollisionDispatcher::needsCollision(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
+bool	btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1)
 {
-
-	btCollisionObject* body0 = (btCollisionObject*)proxy0.m_clientObject;
-	btCollisionObject* body1 = (btCollisionObject*)proxy1.m_clientObject;
-
 	assert(body0);
 	assert(body1);
 
 	bool needsCollision = true;
 
-	if ((body0->m_collisionFlags & btCollisionObject::isStatic) && 
-		(body1->m_collisionFlags & btCollisionObject::isStatic))
-		needsCollision = false;
+	//broadphase filtering already deals with this
+	if ((body0->isStaticObject() || body0->isKinematicObject()) &&
+		(body1->isStaticObject() || body1->isKinematicObject()))
+	{
+		printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
+	}
 		
 	if ((!body0->IsActive()) && (!body1->IsActive()))
 		needsCollision = false;
@@ -248,22 +241,8 @@ bool	btCollisionDispatcher::needsCollision(btBroadphaseProxy& proxy0,btBroadphas
 
 }
 
-///allows the user to get contact point callbacks 
-btManifoldResult*	btCollisionDispatcher::getNewManifoldResult(btCollisionObject* obj0,btCollisionObject* obj1,btPersistentManifold* manifold)
-{
 
 
-	//in-place, this prevents parallel dispatching, but just adding a list would fix that.
-	btManifoldResult* manifoldResult = new (&m_defaultManifoldResult)	btManifoldResult(obj0,obj1,manifold);
-	return manifoldResult;
-}
-	
-///allows the user to get contact point callbacks 
-void	btCollisionDispatcher::releaseManifoldResult(btManifoldResult*)
-{
-
-}
-
 
 class btCollisionPairCallback : public btOverlapCallback
 {
@@ -281,48 +260,35 @@ public:
 
 	virtual bool	processOverlap(btBroadphasePair& pair)
 	{
-		if (m_dispatcherId>= 0)
+		btCollisionObject* body0 = (btCollisionObject*)pair.m_pProxy0->m_clientObject;
+		btCollisionObject* body1 = (btCollisionObject*)pair.m_pProxy1->m_clientObject;
+
+		if (!m_dispatcher->needsCollision(body0,body1))
+			return false;
+
+		//dispatcher will keep algorithms persistent in the collision pair
+		if (!pair.m_algorithms[m_dispatcherId])
 		{
-			//dispatcher will keep algorithms persistent in the collision pair
-			if (!pair.m_algorithms[m_dispatcherId])
-			{
-				pair.m_algorithms[m_dispatcherId] = m_dispatcher->findAlgorithm(
-					*pair.m_pProxy0,
-					*pair.m_pProxy1);
-			}
+			pair.m_algorithms[m_dispatcherId] = m_dispatcher->findAlgorithm(
+				body0,
+				body1);
+		}
 
-			if (pair.m_algorithms[m_dispatcherId])
-			{
-				if (m_dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
-				{
-					pair.m_algorithms[m_dispatcherId]->processCollision(pair.m_pProxy0,pair.m_pProxy1,m_dispatchInfo);
-				} else
-				{
-					float toi = pair.m_algorithms[m_dispatcherId]->calculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,m_dispatchInfo);
-					if (m_dispatchInfo.m_timeOfImpact > toi)
-						m_dispatchInfo.m_timeOfImpact = toi;
-
-				}
-			}
-		} else
+		if (pair.m_algorithms[m_dispatcherId])
 		{
-			//non-persistent algorithm dispatcher
-			btCollisionAlgorithm* algo = m_dispatcher->findAlgorithm(
-				*pair.m_pProxy0,
-				*pair.m_pProxy1);
-
-			if (algo)
+			btManifoldResult* resultOut = m_dispatcher->internalGetNewManifoldResult(body0,body1);
+			if (m_dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
 			{
-				if (m_dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
-				{
-					algo->processCollision(pair.m_pProxy0,pair.m_pProxy1,m_dispatchInfo);
-				} else
-				{
-					float toi = algo->calculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,m_dispatchInfo);
-					if (m_dispatchInfo.m_timeOfImpact > toi)
-						m_dispatchInfo.m_timeOfImpact = toi;
-				}
+				
+				pair.m_algorithms[m_dispatcherId]->processCollision(body0,body1,m_dispatchInfo,resultOut);
+			} else
+			{
+				float toi = pair.m_algorithms[m_dispatcherId]->calculateTimeOfImpact(body0,body1,m_dispatchInfo,resultOut);
+				if (m_dispatchInfo.m_timeOfImpact > toi)
+					m_dispatchInfo.m_timeOfImpact = toi;
+
 			}
+			m_dispatcher->internalReleaseManifoldResult(resultOut);
 		}
 		return false;
 
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
index aede551ac..93e4d09e1 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
@@ -56,8 +56,24 @@ class btCollisionDispatcher : public btDispatcher
 	btCollisionAlgorithmCreateFunc*	m_swappedCompoundCreateFunc;
 	btCollisionAlgorithmCreateFunc*   m_emptyCreateFunc;
 
+	
+
+
 public:
 
+	///allows the user to get contact point callbacks 
+	inline btManifoldResult*	internalGetNewManifoldResult(btCollisionObject* obj0,btCollisionObject* obj1)
+	{
+		//in-place, this prevents parallel dispatching, but just adding a list would fix that.
+		btManifoldResult* manifoldResult = new (&m_defaultManifoldResult)	btManifoldResult(obj0,obj1);
+		return manifoldResult;
+	}
+		
+	///allows the user to get contact point callbacks 
+	inline void	internalReleaseManifoldResult(btManifoldResult*)
+	{
+	}
+
 	///registerCollisionCreateFunc allows registration of custom/alternative collision create functions
 	void	registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
 
@@ -90,23 +106,17 @@ public:
 	
 	virtual void releaseManifold(btPersistentManifold* manifold);
 
-	
-	///allows the user to get contact point callbacks 
-	virtual	btManifoldResult*	getNewManifoldResult(btCollisionObject* obj0,btCollisionObject* obj1,btPersistentManifold* manifold);
-
-	///allows the user to get contact point callbacks 
-	virtual	void	releaseManifoldResult(btManifoldResult*);
 
 	virtual void clearManifold(btPersistentManifold* manifold);
 
 			
-	btCollisionAlgorithm* findAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1);
+	btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1);
 	
-	btCollisionAlgorithm* internalFindAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1);
+	btCollisionAlgorithm* internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1);
 	
-	virtual bool	needsCollision(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1);
+	virtual bool	needsCollision(btCollisionObject* body0,btCollisionObject* body1);
 	
-	virtual bool	needsResponse(const btCollisionObject& colObj0,const btCollisionObject& colObj1);
+	virtual bool	needsResponse(btCollisionObject* body0,btCollisionObject* body1);
 
 	virtual int getUniqueId() { return RIGIDBODY_DISPATCHER;}
 	
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
index 2e6ce9ea2..4ec3af926 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
@@ -42,15 +42,11 @@ void btCollisionObject::ForceActivationState(int newState)
 
 void btCollisionObject::activate()
 {
-	if (!(m_collisionFlags & isStatic))
+	if (!(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OJBECT)))
 	{
-		SetActivationState(1);
+		SetActivationState(ACTIVE_TAG);
 		m_deactivationTime = 0.f;
 	}
 }
 
-bool btCollisionObject::mergesSimulationIslands() const
-{
-	//static objects, and object without contact response don't merge islands
-	return ( !(m_collisionFlags & (isStatic | noContactResponse)));
-}
+
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h
index cf8492556..01aca52ef 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h
@@ -43,9 +43,10 @@ struct	btCollisionObject
 
 	enum CollisionFlags
 	{
-		isStatic = 1,
-		noContactResponse = 2,
-		customMaterialCallback = 4,//this allows per-triangle material (friction/restitution)
+		CF_STATIC_OBJECT= 1,
+		CF_KINEMATIC_OJBECT= 2,
+		CF_NO_CONTACT_RESPONSE = 4,
+		CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
 	};
 
 
@@ -75,14 +76,24 @@ struct	btCollisionObject
 	/// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold
 	float			m_ccdSquareMotionTreshold;
 
-	bool			mergesSimulationIslands() const;
-
-	inline bool		IsStatic() const {
-		return m_collisionFlags & isStatic;
+	inline bool mergesSimulationIslands() const
+	{
+		//static objects, kinematic and object without contact response don't merge islands
+		return  !(m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OJBECT | CF_NO_CONTACT_RESPONSE) );
 	}
 
-	inline bool		HasContactResponse() {
-		return !(m_collisionFlags & noContactResponse);
+
+	inline bool		isStaticObject() const {
+		return m_collisionFlags & CF_STATIC_OBJECT;
+	}
+
+	inline bool		isKinematicObject() const
+	{
+		return m_collisionFlags & CF_KINEMATIC_OJBECT;
+	}
+
+	inline bool		hasContactResponse() const {
+		return !(m_collisionFlags & CF_NO_CONTACT_RESPONSE);
 	}
 
 	
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index fa96cb56a..e24a5c6a0 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -205,7 +205,6 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 						castResult.m_normal.normalize();
 						if (castResult.m_fraction < resultCallback.m_closestHitFraction)
 						{
-							
 
 							btCollisionWorld::LocalRayResult localRayResult
 								(
diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
index 384042c57..febd726b5 100644
--- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
@@ -18,31 +18,24 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
 
 
-btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
-:m_dispatcher(ci.m_dispatcher),
-m_compoundProxy(*proxy0),
-m_otherProxy(*proxy1)
+btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
+:m_isSwapped(isSwapped)
 {
-	btCollisionObject* colObj = static_cast<btCollisionObject*>(m_compoundProxy.m_clientObject);
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
 	assert (colObj->m_collisionShape->isCompound());
 	
 	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape);
 	int numChildren = compoundShape->getNumChildShapes();
-	m_childProxies.resize( numChildren );
 	int i;
-	for (i=0;i<numChildren;i++)
-	{
-		m_childProxies[i] = btBroadphaseProxy(*proxy0);
-	}
-
+	
 	m_childCollisionAlgorithms.resize(numChildren);
 	for (i=0;i<numChildren;i++)
 	{
 		btCollisionShape* childShape = compoundShape->getChildShape(i);
-		btCollisionObject* colObj = static_cast<btCollisionObject*>(m_childProxies[i].m_clientObject);
 		btCollisionShape* orgShape = colObj->m_collisionShape;
 		colObj->m_collisionShape = childShape;
-		m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(m_childProxies[i],m_otherProxy);
+		m_childCollisionAlgorithms[i] = ci.m_dispatcher->findAlgorithm(colObj,otherObj);
 		colObj->m_collisionShape =orgShape;
 	}
 }
@@ -58,11 +51,12 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
 	}
 }
 
-void btCompoundCollisionAlgorithm::processCollision (btBroadphaseProxy* ,btBroadphaseProxy* ,const btDispatcherInfo& dispatchInfo)
+void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
-	btCollisionObject* colObj = static_cast<btCollisionObject*>(m_compoundProxy.m_clientObject);
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+
 	assert (colObj->m_collisionShape->isCompound());
-	
 	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape);
 
 	//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
@@ -72,14 +66,12 @@ void btCompoundCollisionAlgorithm::processCollision (btBroadphaseProxy* ,btBroad
 	//then use each overlapping node AABB against Tree0
 	//and vise versa.
 
-
 	int numChildren = m_childCollisionAlgorithms.size();
 	int i;
 	for (i=0;i<numChildren;i++)
 	{
 		//temporarily exchange parent btCollisionShape with childShape, and recurse
 		btCollisionShape* childShape = compoundShape->getChildShape(i);
-		btCollisionObject* colObj = static_cast<btCollisionObject*>(m_childProxies[i].m_clientObject);
 
 		//backup
 		btTransform	orgTrans = colObj->m_worldTransform;
@@ -88,18 +80,21 @@ void btCompoundCollisionAlgorithm::processCollision (btBroadphaseProxy* ,btBroad
 		btTransform childTrans = compoundShape->getChildTransform(i);
 		btTransform	newChildWorldTrans = orgTrans*childTrans ;
 		colObj->m_worldTransform = newChildWorldTrans;
-
+		//the contactpoint is still projected back using the original inverted worldtrans
 		colObj->m_collisionShape = childShape;
-		m_childCollisionAlgorithms[i]->processCollision(&m_childProxies[i],&m_otherProxy,dispatchInfo);
+		m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut);
 		//revert back
 		colObj->m_collisionShape =orgShape;
 		colObj->m_worldTransform = orgTrans;
 	}
 }
 
-float	btCompoundCollisionAlgorithm::calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
+float	btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
-	btCollisionObject* colObj = static_cast<btCollisionObject*>(m_compoundProxy.m_clientObject);
+
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+
 	assert (colObj->m_collisionShape->isCompound());
 	
 	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape);
@@ -119,7 +114,6 @@ float	btCompoundCollisionAlgorithm::calculateTimeOfImpact(btBroadphaseProxy* pro
 	{
 		//temporarily exchange parent btCollisionShape with childShape, and recurse
 		btCollisionShape* childShape = compoundShape->getChildShape(i);
-		btCollisionObject* colObj = static_cast<btCollisionObject*>(m_childProxies[i].m_clientObject);
 
 		//backup
 		btTransform	orgTrans = colObj->m_worldTransform;
@@ -130,7 +124,7 @@ float	btCompoundCollisionAlgorithm::calculateTimeOfImpact(btBroadphaseProxy* pro
 		colObj->m_worldTransform = newChildWorldTrans;
 
 		colObj->m_collisionShape = childShape;
-		float frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(&m_childProxies[i],&m_otherProxy,dispatchInfo);
+		float frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
 		if (frac<hitFraction)
 		{
 			hitFraction = frac;
diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
index 874e3aa7c..fe2d86286 100644
--- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
@@ -30,40 +30,32 @@ class btDispatcher;
 /// Place holder, not fully implemented yet
 class btCompoundCollisionAlgorithm  : public btCollisionAlgorithm
 {
-	btDispatcher*	m_dispatcher;
-	btBroadphaseProxy	m_compoundProxy;
-	btBroadphaseProxy	m_otherProxy;
-	std::vector<btBroadphaseProxy> m_childProxies;
 	std::vector<btCollisionAlgorithm*> m_childCollisionAlgorithms;
-
-	btBroadphaseProxy m_compound;
-
-	btBroadphaseProxy m_other;
-
+	bool m_isSwapped;
 	
 public:
 
-	btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+	btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
 
 	virtual ~btCompoundCollisionAlgorithm();
 
-	virtual void processCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
-	float	calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
+	float	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btCompoundCollisionAlgorithm(ci,proxy0,proxy1);
+			return new btCompoundCollisionAlgorithm(ci,body0,body1,false);
 		}
 	};
 
 	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btCompoundCollisionAlgorithm(ci,proxy1,proxy0);
+			return new btCompoundCollisionAlgorithm(ci,body0,body1,true);
 		}
 	};
 
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
index 86da3cf7f..753c769e0 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
@@ -27,10 +27,10 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
 
-btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
-: btCollisionAlgorithm(ci),m_convex(*proxy0),m_concave(*proxy1),
-m_btConvexTriangleCallback(ci.m_dispatcher,proxy0,proxy1)
-
+btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
+: btCollisionAlgorithm(ci),
+m_isSwapped(isSwapped),
+m_btConvexTriangleCallback(ci.m_dispatcher,body0,body1,isSwapped)
 {
 }
 
@@ -40,15 +40,17 @@ btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
 
 
 
-btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1):
-  m_convexProxy(proxy0),m_triangleProxy(*proxy1),m_dispatcher(dispatcher),
+btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
+	  m_dispatcher(dispatcher),
 	m_dispatchInfoPtr(0)
 {
-
+	m_convexBody = isSwapped? body1:body0;
+	m_triBody = isSwapped? body0:body1;
+	
 	  //
 	  // create the manifold from the dispatcher 'manifold pool'
 	  //
-	  m_manifoldPtr = m_dispatcher->getNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
+	  m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
 
   	  clearCache();
 }
@@ -80,7 +82,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
 	btCollisionAlgorithmConstructionInfo ci;
 	ci.m_dispatcher = m_dispatcher;
 
-	btCollisionObject* ob = static_cast<btCollisionObject*>(m_triangleProxy.m_clientObject);
+	btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
 
 
 	
@@ -102,9 +104,9 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
 	}
 
 
-	btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
+	//btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
 	
-	if (colObj->m_collisionShape->isConvex())
+	if (m_convexBody->m_collisionShape->isConvex())
 	{
 		btTriangleShape tm(triangle[0],triangle[1],triangle[2]);	
 		tm.setMargin(m_collisionMarginTriangle);
@@ -114,9 +116,9 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
 		ob->m_collisionShape = &tm;
 		
 		///this should use the btDispatcher, so the actual registered algorithm is used
-		btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexProxy,&m_triangleProxy);
+		btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
 		cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
-		cvxcvxalgo.processCollision(m_convexProxy,&m_triangleProxy,*m_dispatchInfoPtr);
+		cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
 		ob->m_collisionShape = tmpShape;
 
 	}
@@ -127,25 +129,19 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
 
 
 
-void	btConvexTriangleCallback::setTimeStepAndCounters(float collisionMarginTriangle,const btDispatcherInfo& dispatchInfo)
+void	btConvexTriangleCallback::setTimeStepAndCounters(float collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
 	m_dispatchInfoPtr = &dispatchInfo;
 	m_collisionMarginTriangle = collisionMarginTriangle;
+	m_resultOut = resultOut;
 
 	//recalc aabbs
-	btCollisionObject* convexBody = (btCollisionObject* )m_convexProxy->m_clientObject;
-	btCollisionObject* triBody = (btCollisionObject* )m_triangleProxy.m_clientObject;
-
 	btTransform convexInTriangleSpace;
-	convexInTriangleSpace = triBody->m_worldTransform.inverse() * convexBody->m_worldTransform;
-
-	btCollisionShape* convexShape = static_cast<btCollisionShape*>(convexBody->m_collisionShape);
+	convexInTriangleSpace = m_triBody->m_worldTransform.inverse() * m_convexBody->m_worldTransform;
+	btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->m_collisionShape);
 	//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
-
 	convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
-
-	float extraMargin = collisionMarginTriangle;//CONVEX_DISTANCE_MARGIN;//+0.1f;
-
+	float extraMargin = collisionMarginTriangle;
 	btVector3 extra(extraMargin,extraMargin,extraMargin);
 
 	m_aabbMax += extra;
@@ -159,34 +155,31 @@ void btConvexConcaveCollisionAlgorithm::clearCache()
 
 }
 
-void btConvexConcaveCollisionAlgorithm::processCollision (btBroadphaseProxy* ,btBroadphaseProxy* ,const btDispatcherInfo& dispatchInfo)
+void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
 	
-	btCollisionObject* convexBody = static_cast<btCollisionObject* >(m_convex.m_clientObject);
-	btCollisionObject* triBody = static_cast<btCollisionObject* >(m_concave.m_clientObject);
+	
+	btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
+	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
 
 	if (triBody->m_collisionShape->isConcave())
 	{
 
-		if (!m_dispatcher->needsCollision(m_convex,m_concave))
-			return;
 
-		
-
-		btCollisionObject*	triOb = static_cast<btCollisionObject*>(m_concave.m_clientObject);
+		btCollisionObject*	triOb = triBody;
 		ConcaveShape* concaveShape = static_cast<ConcaveShape*>( triOb->m_collisionShape);
 		
 		if (convexBody->m_collisionShape->isConvex())
 		{
 			float collisionMarginTriangle = concaveShape->getMargin();
 					
-			m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo);
+			resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+			m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
 
 			//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
 			//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
 
-
-			m_btConvexTriangleCallback.m_manifoldPtr->setBodies(m_convex.m_clientObject,m_concave.m_clientObject);
+			m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
 
 			concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
 			
@@ -198,12 +191,14 @@ void btConvexConcaveCollisionAlgorithm::processCollision (btBroadphaseProxy* ,bt
 }
 
 
-float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btBroadphaseProxy* ,btBroadphaseProxy* ,const btDispatcherInfo& dispatchInfo)
+float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
 
+	btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
+	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+
 	//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
-	btCollisionObject* convexbody = (btCollisionObject* )m_convex.m_clientObject;
-	btCollisionObject* triBody = static_cast<btCollisionObject* >(m_concave.m_clientObject);
 
 	//only perform CCD above a certain treshold, this prevents blocking on the long run
 	//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
@@ -217,9 +212,9 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btBroadphaseProxy
 	//btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
 	//todo: only do if the motion exceeds the 'radius'
 
-	btTransform worldToLocalTrimesh = triBody->m_worldTransform.inverse();
-	btTransform convexFromLocal = worldToLocalTrimesh * convexbody->m_worldTransform;
-	btTransform convexToLocal = worldToLocalTrimesh * convexbody->m_interpolationWorldTransform;
+	btTransform triInv = triBody->m_worldTransform.inverse();
+	btTransform convexFromLocal = triInv * convexbody->m_worldTransform;
+	btTransform convexToLocal = triInv * convexbody->m_interpolationWorldTransform;
 
 	struct LocalTriangleSphereCastCallback	: public btTriangleCallback
 	{
@@ -285,7 +280,7 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btBroadphaseProxy
 
 		raycastCallback.m_hitFraction = convexbody->m_hitFraction;
 
-		btCollisionObject* concavebody = (btCollisionObject* )m_concave.m_clientObject;
+		btCollisionObject* concavebody = triBody;
 
 		ConcaveShape* triangleMesh = (ConcaveShape*) concavebody->m_collisionShape;
 		
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
index 62bddb623..afcb38c94 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
@@ -28,12 +28,14 @@ class btDispatcher;
 ///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called.
 class btConvexTriangleCallback : public btTriangleCallback
 {
-	btBroadphaseProxy* m_convexProxy;
-	btBroadphaseProxy m_triangleProxy;
+	btCollisionObject* m_convexBody;
+	btCollisionObject* m_triBody;
 
 	btVector3	m_aabbMin;
 	btVector3	m_aabbMax ;
 
+	btManifoldResult* m_resultOut;
+
 	btDispatcher*	m_dispatcher;
 	const btDispatcherInfo* m_dispatchInfoPtr;
 	float m_collisionMarginTriangle;
@@ -43,9 +45,9 @@ int	m_triangleCount;
 	
 	btPersistentManifold*	m_manifoldPtr;
 
-	btConvexTriangleCallback(btDispatcher* dispatcher,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+	btConvexTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
 
-	void	setTimeStepAndCounters(float collisionMarginTriangle,const btDispatcherInfo& dispatchInfo);
+	void	setTimeStepAndCounters(float collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
 	virtual ~btConvexTriangleCallback();
 
@@ -71,38 +73,36 @@ int	m_triangleCount;
 class btConvexConcaveCollisionAlgorithm  : public btCollisionAlgorithm
 {
 
-	btBroadphaseProxy m_convex;
-
-	btBroadphaseProxy m_concave;
+	bool	m_isSwapped;
 
 	btConvexTriangleCallback m_btConvexTriangleCallback;
 
 
 public:
 
-	btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+	btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
 
 	virtual ~btConvexConcaveCollisionAlgorithm();
 
-	virtual void processCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
-	float	calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
+	float	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
 	void	clearCache();
 
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btConvexConcaveCollisionAlgorithm(ci,proxy0,proxy1);
+			return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
 		}
 	};
 
 	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btConvexConcaveCollisionAlgorithm(ci,proxy1,proxy0);
+			return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
 		}
 	};
 
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
index b03486903..ade7f21ad 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
@@ -81,30 +81,21 @@ bool gDisableConvexCollision = false;
 
 
 
-btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1)
 : btCollisionAlgorithm(ci),
 m_gjkPairDetector(0,0,&m_simplexSolver,0),
 m_useEpa(!gUseEpa),
-m_box0(*proxy0),
-m_box1(*proxy1),
 m_ownManifold (false),
 m_manifoldPtr(mf),
 m_lowLevelOfDetail(false)
 {
 	checkPenetrationDepthSolver();
 
-	{
-		if (!m_manifoldPtr && m_dispatcher->needsCollision(m_box0,m_box1))
-		{
-			m_manifoldPtr = m_dispatcher->getNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
-			m_ownManifold = true;
-		}
-	}
-
 }
 
 
 
+
 btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
 {
 	if (m_ownManifold)
@@ -121,26 +112,6 @@ void	btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
 
 
 
-class FlippedContactResult : public btDiscreteCollisionDetectorInterface::Result
-{
-	btDiscreteCollisionDetectorInterface::Result* m_org;
-
-public:
-
-	FlippedContactResult(btDiscreteCollisionDetectorInterface::Result* org)
-		: m_org(org)
-	{
-
-	}
-
-	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
-	{
-		btVector3 flippedNormal = -normalOnBInWorld;
-
-		m_org->addContactPoint(flippedNormal,pointInWorld,depth);
-	}
-
-};
 
 static btMinkowskiPenetrationDepthSolver	gPenetrationDepthSolver;
 
@@ -169,146 +140,28 @@ void	btConvexConvexAlgorithm::checkPenetrationDepthSolver()
 	
 }
 
-#ifdef USE_HULL
-
-Transform	GetTransformFrombtTransform(const btTransform& trans)
-{
-			//const btVector3& rowA0 = trans.getBasis().getRow(0);
-			////const btVector3& rowA1 = trans.getBasis().getRow(1);
-			//const btVector3& rowA2 = trans.getBasis().getRow(2);
-
-			btVector3 rowA0 = trans.getBasis().getColumn(0);
-			btVector3 rowA1 = trans.getBasis().getColumn(1);
-			btVector3 rowA2 = trans.getBasis().getColumn(2);
-
-
-			Vector3	x(rowA0.getX(),rowA0.getY(),rowA0.getZ());
-			Vector3	y(rowA1.getX(),rowA1.getY(),rowA1.getZ());
-			Vector3	z(rowA2.getX(),rowA2.getY(),rowA2.getZ());
-			
-			Matrix33 ornA(x,y,z);
-	
-			Point3 transA(
-				trans.getOrigin().getX(),
-				trans.getOrigin().getY(),
-				trans.getOrigin().getZ());
-
-			return Transform(ornA,transA);
-}
-
-class btManifoldResultCollector : public HullContactCollector
-{
-public:
-	btManifoldResult& m_manifoldResult;
-
-	btManifoldResultCollector(btManifoldResult& manifoldResult)
-		:m_manifoldResult(manifoldResult)
-	{
-
-	}
-	
-
-	virtual ~btManifoldResultCollector() {};
-
-	virtual int	BatchAddContactGroup(const btSeparation& sep,int numContacts,const Vector3& normalWorld,const Vector3& tangent,const Point3* positionsWorld,const float* depths)
-	{
-		for (int i=0;i<numContacts;i++)
-		{
-			//printf("numContacts = %i\n",numContacts);
-			btVector3 normalOnBInWorld(sep.m_axis.GetX(),sep.m_axis.GetY(),sep.m_axis.GetZ());
-			//normalOnBInWorld.normalize();
-			btVector3 pointInWorld(positionsWorld[i].GetX(),positionsWorld[i].GetY(),positionsWorld[i].GetZ());
-			float depth = -depths[i];
-			m_manifoldResult.addContactPoint(normalOnBInWorld,pointInWorld,depth);
-
-		}
-		return 0;
-	}
-
-	virtual int		GetMaxNumContacts() const
-	{
-		return 4;
-	}
-
-};
-#endif //USE_HULL
-
 
 //
 // Convex-Convex collision algorithm
 //
-void btConvexConvexAlgorithm ::processCollision (btBroadphaseProxy* ,btBroadphaseProxy* ,const btDispatcherInfo& dispatchInfo)
+void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
 
 	if (!m_manifoldPtr)
-		return;
+	{
+		//swapped?
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+		m_ownManifold = true;
+	}
+
 
 	checkPenetrationDepthSolver();
 
-//	printf("btConvexConvexAlgorithm::processCollision\n");
-
-	bool needsCollision = m_dispatcher->needsCollision(m_box0,m_box1);
-	if (!needsCollision)
-		return;
-	
-	btCollisionObject*	col0 = static_cast<btCollisionObject*>(m_box0.m_clientObject);
-	btCollisionObject*	col1 = static_cast<btCollisionObject*>(m_box1.m_clientObject);
-
-#ifdef USE_HULL
-
-
-	if (dispatchInfo.m_enableSatConvex)
-	{
-		if ((col0->m_collisionShape->isPolyhedral()) &&
-			(col1->m_collisionShape->isPolyhedral()))
-		{
-		
-			
-			btPolyhedralConvexShape* polyhedron0 = static_cast<btPolyhedralConvexShape*>(col0->m_collisionShape);
-			btPolyhedralConvexShape* polyhedron1 = static_cast<btPolyhedralConvexShape*>(col1->m_collisionShape);
-			if (polyhedron0->m_optionalHull && polyhedron1->m_optionalHull)
-			{
-				//printf("Hull-Hull");
-
-				//todo: cache this information, rather then initialize
-				btSeparation sep;
-				sep.m_featureA = 0;
-				sep.m_featureB = 0;
-				sep.m_contact = -1;
-				sep.m_separator = 0;
-
-				//convert from btTransform to Transform
-				
-				Transform trA = GetTransformFrombtTransform(col0->m_worldTransform);
-				Transform trB = GetTransformFrombtTransform(col1->m_worldTransform);
-
-				//either use persistent manifold or clear it every time
-				m_dispatcher->clearManifold(m_manifoldPtr);
-				btManifoldResult* resultOut = m_dispatcher->getNewManifoldResult(col0,col1,m_manifoldPtr);
-
-				btManifoldResultCollector hullContactCollector(*resultOut);
-				
-				Hull::ProcessHullHull(sep,*polyhedron0->m_optionalHull,*polyhedron1->m_optionalHull,
-					trA,trB,&hullContactCollector);
-
-				
-				//user provided hull's, so we use SAT Hull collision detection
-				return;
-			}
-		}
-	}
-
-#endif //USE_HULL
-
-	
-	btManifoldResult* resultOut = m_dispatcher->getNewManifoldResult(col0,col1,m_manifoldPtr);
-	
-	btConvexShape* min0 = static_cast<btConvexShape*>(col0->m_collisionShape);
-	btConvexShape* min1 = static_cast<btConvexShape*>(col1->m_collisionShape);
+	btConvexShape* min0 = static_cast<btConvexShape*>(body0->m_collisionShape);
+	btConvexShape* min1 = static_cast<btConvexShape*>(body1->m_collisionShape);
 	
 	btGjkPairDetector::ClosestPointInput input;
 
-
 	//TODO: if (dispatchInfo.m_useContinuous)
 	m_gjkPairDetector.setMinkowskiA(min0);
 	m_gjkPairDetector.setMinkowskiB(min1);
@@ -317,18 +170,18 @@ void btConvexConvexAlgorithm ::processCollision (btBroadphaseProxy* ,btBroadphas
 	
 //	input.m_maximumDistanceSquared = 1e30f;
 	
-	input.m_transformA = col0->m_worldTransform;
-	input.m_transformB = col1->m_worldTransform;
+	input.m_transformA = body0->m_worldTransform;
+	input.m_transformB = body1->m_worldTransform;
     
+	resultOut->setPersistentManifold(m_manifoldPtr);
 	m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 
-	m_dispatcher->releaseManifoldResult(resultOut);
 }
 
 
 
 bool disableCcd = false;
-float	btConvexConvexAlgorithm::calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
+float	btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
 	///Rather then checking ALL pairs, only calculate TOI when motion exceeds treshold
     
@@ -336,8 +189,6 @@ float	btConvexConvexAlgorithm::calculateTimeOfImpact(btBroadphaseProxy* proxy0,b
 	///col0->m_worldTransform,
 	float resultFraction = 1.f;
 
-	btCollisionObject* col1 = static_cast<btCollisionObject*>(m_box1.m_clientObject);
-	btCollisionObject* col0 = static_cast<btCollisionObject*>(m_box0.m_clientObject);
 
 	float squareMot0 = (col0->m_interpolationWorldTransform.getOrigin() - col0->m_worldTransform.getOrigin()).length2();
     
@@ -358,11 +209,6 @@ float	btConvexConvexAlgorithm::calculateTimeOfImpact(btBroadphaseProxy* proxy0,b
 	//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
 	//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
 
-	bool needsCollision = m_dispatcher->needsCollision(m_box0,m_box1);
-
-	if (!needsCollision)
-		return 1.f;
-	
 		
 	/// Convex0 against sphere for Convex1
 	{
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
index 7942ba682..b8e121714 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
@@ -33,8 +33,6 @@ class btConvexConvexAlgorithm : public btCollisionAlgorithm
 	btGjkPairDetector m_gjkPairDetector;
 	bool	m_useEpa;
 public:
-	btBroadphaseProxy	m_box0;
-	btBroadphaseProxy	m_box1;
 
 	bool	m_ownManifold;
 	btPersistentManifold*	m_manifoldPtr;
@@ -46,13 +44,13 @@ public:
 
 public:
 
-	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
 
 	virtual ~btConvexConvexAlgorithm();
 
-	virtual void processCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
-	virtual float calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
+	virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
 	void	setLowLevelOfDetail(bool useLowLevel);
 
@@ -71,9 +69,9 @@ public:
 
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btConvexConvexAlgorithm(0,ci,proxy0,proxy1);
+			return new btConvexConvexAlgorithm(0,ci,body0,body1);
 		}
 	};
 
diff --git a/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp
index 0e0f4a338..9bc106564 100644
--- a/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp
@@ -22,12 +22,12 @@ btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& c
 {
 }
 
-void btEmptyAlgorithm::processCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
+void btEmptyAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
 
 }
 
-float btEmptyAlgorithm::calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
+float btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
 	return 1.f;
 }
diff --git a/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
index 8561cbfc8..e0e136250 100644
--- a/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
@@ -29,13 +29,13 @@ public:
 	
 	btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
 
-	virtual void processCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
-	virtual float calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
+	virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
 			return new btEmptyAlgorithm(ci);
 		}
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
index 114d7fdc0..a9b74ac05 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
@@ -43,25 +43,29 @@ inline btScalar	calculateCombinedRestitution(const btCollisionObject* body0,cons
 
 
 
-btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* manifoldPtr)
-		:m_manifoldPtr(manifoldPtr),
+btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* body1)
+		:m_manifoldPtr(0),
 		m_body0(body0),
 		m_body1(body1)
-	{
-	}
+{
+	m_rootTransA = body0->m_worldTransform;
+	m_rootTransB = body1->m_worldTransform;
+}
 
 
 void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
 {
+	assert(m_manifoldPtr);
+	//order in manifold needs to match
+	
 	if (depth > m_manifoldPtr->getContactBreakingTreshold())
 		return;
 
+	bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
 
-	btTransform transAInv = m_body0->m_worldTransform.inverse();
-	btTransform transBInv= m_body1->m_worldTransform.inverse();
+	btTransform transAInv = isSwapped? m_rootTransB.inverse() : m_rootTransA.inverse();
+	btTransform transBInv = isSwapped? m_rootTransA.inverse() : m_rootTransB.inverse();
 
-	//transAInv = m_body0->m_worldTransform.inverse();
-	//transBInv= m_body1->m_worldTransform.inverse();
 	btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
 	btVector3 localA = transAInv(pointA );
 	btVector3 localB = transBInv(pointInWorld);
@@ -88,8 +92,8 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 		//User can override friction and/or restitution
 		if (gContactAddedCallback &&
 			//and if either of the two bodies requires custom material
-			 ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) ||
-			   (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback)))
+			 ((m_body0->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
+			   (m_body1->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
 		{
 			//experimental feature info, for per-triangle material etc.
 			(*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
index 6b4b3f1e6..f9aae4e9c 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.h
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
@@ -21,6 +21,7 @@ subject to the following restrictions:
 struct btCollisionObject;
 class btPersistentManifold;
 class btManifoldPoint;
+#include "LinearMath/btTransform.h"
 
 typedef bool (*ContactAddedCallback)(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1);
 extern ContactAddedCallback		gContactAddedCallback;
@@ -31,6 +32,11 @@ extern ContactAddedCallback		gContactAddedCallback;
 class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
 {
 	btPersistentManifold* m_manifoldPtr;
+
+	//we need this for compounds
+	btTransform	m_rootTransA;
+	btTransform	m_rootTransB;
+
 	btCollisionObject* m_body0;
 	btCollisionObject* m_body1;
 	int	m_partId0;
@@ -39,10 +45,19 @@ class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
 	int m_index1;
 public:
 
-	btManifoldResult(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* manifoldPtr);
+	btManifoldResult()
+	{
+	}
+
+	btManifoldResult(btCollisionObject* body0,btCollisionObject* body1);
 
 	virtual ~btManifoldResult() {};
 
+	void	setPersistentManifold(btPersistentManifold* manifoldPtr)
+	{
+		m_manifoldPtr = manifoldPtr;
+	}
+
 	virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
 	{
 			m_partId0=partId0;
diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
index cce334d36..04abac49d 100644
--- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
@@ -238,7 +238,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 			((colObj1) && colObj1->GetActivationState() != ISLAND_SLEEPING))
 		{
 			//filtering for response
-			if (dispatcher->needsResponse(*colObj0,*colObj1))
+			if (dispatcher->needsResponse(colObj0,colObj1))
 				islandmanifold.push_back(manifold);
 		}
 	}
diff --git a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
index 0bb254a62..e85a9bd47 100644
--- a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
@@ -20,17 +20,16 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 //#include <stdio.h>
 
-btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
 : btCollisionAlgorithm(ci),
 m_ownManifold(false),
-m_manifoldPtr(mf)
+m_manifoldPtr(mf),
+m_isSwapped(isSwapped)
 {
-	m_sphereColObj = static_cast<btCollisionObject*>(proxy0->m_clientObject);
-	m_boxColObj    = static_cast<btCollisionObject*>(proxy1->m_clientObject);
-
-	if (!m_manifoldPtr && m_dispatcher->needsCollision(*proxy0,*proxy1))
+	
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(col0,col1))
 	{
-		m_manifoldPtr = m_dispatcher->getNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
+		m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
 		m_ownManifold = true;
 	}
 }
@@ -47,55 +46,59 @@ btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
 
 
 
-void btSphereBoxCollisionAlgorithm::processCollision (btBroadphaseProxy*,btBroadphaseProxy*,const btDispatcherInfo& dispatchInfo)
+void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
-	
 	if (!m_manifoldPtr)
 		return;
 
-	btSphereShape* sphere0 = (btSphereShape*)m_sphereColObj ->m_collisionShape;
+	btCollisionObject* sphereObj = m_isSwapped? body1 : body0;
+	btCollisionObject* boxObj = m_isSwapped? body0 : body1;
+
+
+	btSphereShape* sphere0 = (btSphereShape*)sphereObj ->m_collisionShape;
 
 	btVector3 normalOnSurfaceB;
 	btVector3 pOnBox,pOnSphere;
-	btVector3 sphereCenter = m_sphereColObj->m_worldTransform.getOrigin();
+	btVector3 sphereCenter = sphereObj->m_worldTransform.getOrigin();
 	btScalar radius = sphere0->getRadius();
 	
-	float dist = getSphereDistance(pOnBox,pOnSphere,sphereCenter,radius);
+	float dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius);
 
 	if (dist < SIMD_EPSILON)
 	{
 		btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize();
 
 		/// report a contact. internally this will be kept persistent, and contact reduction is done
-		btManifoldResult* resultOut = m_dispatcher->getNewManifoldResult(m_sphereColObj,m_boxColObj,m_manifoldPtr);
+
+		resultOut->setPersistentManifold(m_manifoldPtr);
 		resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist);
-		m_dispatcher->releaseManifoldResult(resultOut);
+		
 	}
 
 	
 
 }
 
-float btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
+float btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
 	//not yet
 	return 1.f;
 }
 
 
-btScalar btSphereBoxCollisionAlgorithm::getSphereDistance( btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) 
+btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) 
 {
 
 	btScalar margins;
 	btVector3 bounds[2];
-	btBoxShape* boxShape= (btBoxShape*)m_boxColObj->m_collisionShape;
+	btBoxShape* boxShape= (btBoxShape*)boxObj->m_collisionShape;
 	
 	bounds[0] = -boxShape->getHalfExtents();
 	bounds[1] = boxShape->getHalfExtents();
 
 	margins = boxShape->getMargin();//also add sphereShape margin?
 
-	const btTransform&	m44T = m_boxColObj->m_worldTransform;
+	const btTransform&	m44T = boxObj->m_worldTransform;
 
 	btVector3	boundsVec[2];
 	btScalar	fPenetration;
@@ -175,7 +178,7 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance( btVector3& pointOnBox
 	//////////////////////////////////////////////////
 	// Deep penetration case
 
-	fPenetration = getSpherePenetration( pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] );
+	fPenetration = getSpherePenetration( boxObj,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] );
 
 	bounds[0] = boundsVec[0];
 	bounds[1] = boundsVec[1];
@@ -186,7 +189,7 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance( btVector3& pointOnBox
 		return 1.0f;
 }
 
-btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) 
+btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) 
 {
 
 	btVector3 bounds[2];
@@ -204,7 +207,7 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3& pointOn
 	n[4].setValue(  0.0f,  1.0f,  0.0f );
 	n[5].setValue(  0.0f,  0.0f,  1.0f );
 
-	const btTransform&	m44T = m_boxColObj->m_worldTransform;
+	const btTransform&	m44T = boxObj->m_worldTransform;
 
 	// convert  point in local space
 	prel = m44T.invXform( sphereCenter);
diff --git a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
index 5a52eb9c3..ec0ae7a23 100644
--- a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
@@ -28,33 +28,32 @@ class btSphereBoxCollisionAlgorithm : public btCollisionAlgorithm
 {
 	bool	m_ownManifold;
 	btPersistentManifold*	m_manifoldPtr;
-	btCollisionObject*	m_boxColObj;
-	btCollisionObject*	m_sphereColObj;
+	bool	m_isSwapped;
 	
 public:
 
-	btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+	btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
 
 	virtual ~btSphereBoxCollisionAlgorithm();
 
-	virtual void processCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const struct btDispatcherInfo& dispatchInfo);
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
-	virtual float calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const struct btDispatcherInfo& dispatchInfo);
+	virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
-	btScalar getSphereDistance( btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius );
+	btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius );
 
-	btScalar getSpherePenetration( btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax);
+	btScalar getSpherePenetration( btCollisionObject* boxObj, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax);
 	
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
 			if (!m_swapped)
 			{
-				return new btSphereBoxCollisionAlgorithm(0,ci,proxy0,proxy1);
+				return new btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false);
 			} else
 			{
-				return new btSphereBoxCollisionAlgorithm(0,ci,proxy1,proxy0);
+				return new btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true);
 			}
 		}
 	};
diff --git a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
index 9fe82da79..1c3ab81b7 100644
--- a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
@@ -18,14 +18,14 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
-btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1)
 : btCollisionAlgorithm(ci),
 m_ownManifold(false),
 m_manifoldPtr(mf)
 {
-	if (!m_manifoldPtr && m_dispatcher->needsCollision(*proxy0,*proxy1))
+	if (!m_manifoldPtr)
 	{
-		m_manifoldPtr = m_dispatcher->getNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
+		m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
 		m_ownManifold = true;
 	}
 }
@@ -39,13 +39,11 @@ btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
 	}
 }
 
-void btSphereSphereCollisionAlgorithm::processCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
+void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
 	if (!m_manifoldPtr)
 		return;
 
-	btCollisionObject*	col0 = static_cast<btCollisionObject*>(proxy0->m_clientObject);
-	btCollisionObject*	col1 = static_cast<btCollisionObject*>(proxy1->m_clientObject);
 	btSphereShape* sphere0 = (btSphereShape*)col0->m_collisionShape;
 	btSphereShape* sphere1 = (btSphereShape*)col1->m_collisionShape;
 
@@ -68,13 +66,12 @@ void btSphereSphereCollisionAlgorithm::processCollision (btBroadphaseProxy* prox
 	btVector3 pos1 = col1->m_worldTransform.getOrigin() + radius1* normalOnSurfaceB;
 
 	/// report a contact. internally this will be kept persistent, and contact reduction is done
-	btManifoldResult* resultOut = m_dispatcher->getNewManifoldResult(col0,col1,m_manifoldPtr);
+	resultOut->setPersistentManifold(m_manifoldPtr);
 	resultOut->addContactPoint(normalOnSurfaceB,pos1,dist);
-	m_dispatcher->releaseManifoldResult(resultOut);
 
 }
 
-float btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
+float btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
 	//not yet
 	return 1.f;
diff --git a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
index 588dcb0c1..425c6739e 100644
--- a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
@@ -30,22 +30,23 @@ class btSphereSphereCollisionAlgorithm : public btCollisionAlgorithm
 	btPersistentManifold*	m_manifoldPtr;
 	
 public:
+	btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
 	btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
 		: btCollisionAlgorithm(ci) {}
 
-	virtual void processCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
-	virtual float calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
-
-	btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+	virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
+	
 	virtual ~btSphereSphereCollisionAlgorithm();
 
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btSphereSphereCollisionAlgorithm(0,ci,proxy0,proxy1);
+			return new btSphereSphereCollisionAlgorithm(0,ci,body0,body1);
 		}
 	};
 
diff --git a/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
index f09efeda1..d31681e6b 100644
--- a/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
@@ -129,14 +129,14 @@ int	btConvexHullShape::getNumVertices() const
 
 int btConvexHullShape::getNumEdges() const
 {
-	return m_points.size()*m_points.size();
+	return m_points.size();
 }
 
 void btConvexHullShape::getEdge(int i,btPoint3& pa,btPoint3& pb) const
 {
 
 	int index0 = i%m_points.size();
-	int index1 = i/m_points.size();
+	int index1 = (i+1)%m_points.size();
 	pa = m_points[index0]*m_localScaling;
 	pb = m_points[index1]*m_localScaling;
 }
diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldContactAddResult.cpp b/src/BulletCollision/NarrowPhaseCollision/btManifoldContactAddResult.cpp
deleted file mode 100644
index 0b0741907..000000000
--- a/src/BulletCollision/NarrowPhaseCollision/btManifoldContactAddResult.cpp
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include "btManifoldContactAddResult.h"
-#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
-
-btManifoldContactAddResult::btManifoldContactAddResult(btTransform transA,btTransform transB,btPersistentManifold* manifoldPtr)
-		:m_manifoldPtr(manifoldPtr)
-{
-	m_transAInv = transA.inverse();
-	m_transBInv = transB.inverse();
-
-}
-
-
-void btManifoldContactAddResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
-{
-	if (depth > m_manifoldPtr->getContactBreakingTreshold())
-		return;
-
-
-	btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
-	btVector3 localA = m_transAInv(pointA );
-	btVector3 localB = m_transBInv(pointInWorld);
-	btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
-
-	int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
-	if (insertIndex >= 0)
-	{
-		m_manifoldPtr->replaceContactPoint(newPt,insertIndex);
-	} else
-	{
-		m_manifoldPtr->AddManifoldPoint(newPt);
-	}
-}
-
diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldContactAddResult.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldContactAddResult.h
deleted file mode 100644
index 9fcc049f5..000000000
--- a/src/BulletCollision/NarrowPhaseCollision/btManifoldContactAddResult.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#ifndef MANIFOLD_CONTACT_ADD_RESULT_H
-#define MANIFOLD_CONTACT_ADD_RESULT_H
-
-#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
-class btPersistentManifold;
-
-class btManifoldContactAddResult : public btDiscreteCollisionDetectorInterface::Result
-{
-	btPersistentManifold* m_manifoldPtr;
-	btTransform	m_transAInv;
-	btTransform	m_transBInv;
-
-public:
-
-	btManifoldContactAddResult(btTransform transA,btTransform transB,btPersistentManifold* manifoldPtr);
-
-	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth);
-
-};
-
-#endif //MANIFOLD_CONTACT_ADD_RESULT_H
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index 4d0b442ab..a8ab1bbad 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -16,7 +16,6 @@ subject to the following restrictions:
 
 #include "btGeneric6DofConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "BulletDynamics/Dynamics/btMassProps.h"
 #include "LinearMath/btTransformUtil.h"
 
 static const btScalar kSign[] = { 1.0f, -1.0f, 1.0f };
diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
index ea58cf0b5..b507e4c7b 100644
--- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
@@ -16,7 +16,6 @@ subject to the following restrictions:
 
 #include "btHingeConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "BulletDynamics/Dynamics/btMassProps.h"
 #include "LinearMath/btTransformUtil.h"
 
 
diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
index 4497e720f..d15bdaad7 100644
--- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
@@ -16,7 +16,6 @@ subject to the following restrictions:
 
 #include "btPoint2PointConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "BulletDynamics/Dynamics/btMassProps.h"
 
 
 
diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
index 4d0da57c1..ea7d5c8b9 100644
--- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
@@ -16,7 +16,6 @@ subject to the following restrictions:
 
 #include "btTypedConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "BulletDynamics/Dynamics/btMassProps.h"
 
 static btRigidBody s_fixed(0, btTransform::getIdentity(),0);
 
diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index 2cb2b8bf1..fb5024820 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -35,6 +35,7 @@ subject to the following restrictions:
 #include "BulletDynamics/Vehicle/btWheelInfo.h"
 #include "LinearMath/btIDebugDraw.h"
 #include "LinearMath/btQuickprof.h"
+#include "LinearMath/btMotionState.h"
 
 
 
@@ -44,6 +45,7 @@ btDiscreteDynamicsWorld::btDiscreteDynamicsWorld()
 :btDynamicsWorld(),
 m_constraintSolver(new btSequentialImpulseConstraintSolver),
 m_debugDrawer(0),
+m_gravity(0,-10,0),
 m_profileTimings(0)
 {
 	m_islandManager = new btSimulationIslandManager();
@@ -56,6 +58,7 @@ btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOver
 :btDynamicsWorld(dispatcher,pairCache),
 m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
 m_debugDrawer(0),
+m_gravity(0,-10,0),
 m_profileTimings(0)
 {
 	m_islandManager = new btSimulationIslandManager();
@@ -73,7 +76,69 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
 		 delete m_constraintSolver;
 }
 
-void	btDiscreteDynamicsWorld::stepSimulation(float timeStep)
+void	btDiscreteDynamicsWorld::saveKinematicState(float timeStep)
+{
+
+	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+				btTransform predictedTrans;
+				if (body->GetActivationState() != ISLAND_SLEEPING)
+				{
+					if (body->isKinematicObject())
+					{
+						//to calculate velocities next frame
+						body->saveKinematicState(timeStep);
+					}
+				}
+		}
+	}
+}
+
+void	btDiscreteDynamicsWorld::synchronizeMotionStates()
+{
+	//todo: iterate over awake simulation islands!
+	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body && body->getMotionState())
+		{
+			if (body->GetActivationState() != ISLAND_SLEEPING)
+			{
+				body->getMotionState()->setWorldOrientation(body->getCenterOfMassTransform().getRotation());
+				body->getMotionState()->setWorldPosition(body->getCenterOfMassTransform().getOrigin());
+			}
+		}
+	}
+
+}
+
+
+void	btDiscreteDynamicsWorld::stepSimulation(float timeStep, int numSubsteps)
+{
+
+	if (!btFuzzyZero(timeStep) && numSubsteps)
+	{
+
+		saveKinematicState(timeStep);
+
+		int i;
+		float subTimeStep = timeStep / float(numSubsteps);
+
+		for (i=0;i<numSubsteps;i++)
+		{
+			internalSingleStepSimulation(subTimeStep);
+		}
+
+		synchronizeMotionStates();
+	} 
+}
+
+void	btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
 {
 	
 	startProfiling(timeStep);
@@ -112,6 +177,35 @@ void	btDiscreteDynamicsWorld::stepSimulation(float timeStep)
 
 }
 
+void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
+{
+	m_gravity = gravity;
+	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->setGravity(gravity);
+		}
+	}
+}
+
+void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
+{
+	body->setGravity(m_gravity);
+	bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
+	short collisionFilterGroup = isDynamic? 
+	btBroadphaseProxy::DefaultFilter : 
+	btBroadphaseProxy::StaticFilter;
+	short collisionFilterMask = isDynamic? 
+	btBroadphaseProxy::AllFilter : 
+	btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
+
+	addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
+}
+
+
 void	btDiscreteDynamicsWorld::updateVehicles(float timeStep)
 {
 	BEGIN_PROFILE("updateVehicles");
@@ -360,7 +454,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(float timeStep)
 		btRigidBody* body = btRigidBody::upcast(colObj);
 		if (body)
 		{
-			if (body->IsActive() && (!body->IsStatic()))
+			if (body->IsActive() && (!body->isStaticObject()))
 			{
 				body->predictIntegratedTransform(timeStep, predictedTrans);
 				body->proceedToTransform( predictedTrans);
@@ -381,12 +475,14 @@ void	btDiscreteDynamicsWorld::predictUnconstraintMotion(float timeStep)
 		btRigidBody* body = btRigidBody::upcast(colObj);
 		if (body)
 		{
-			if (body->IsActive() && (!body->IsStatic()))
+			if (!body->isStaticObject())
 			{
-				body->applyForces( timeStep);
-				body->integrateVelocities( timeStep);
-				body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
-
+				if (body->IsActive())
+				{
+					body->applyForces( timeStep);
+					body->integrateVelocities( timeStep);
+					body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
+				}
 			}
 		}
 	}
diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index 851ea4093..10a460111 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -43,6 +43,8 @@ protected:
 
 	btIDebugDraw*	m_debugDrawer;
 
+	btVector3	m_gravity;
+
 	bool	m_ownsIslandManager;
 	bool	m_ownsConstraintSolver;
 
@@ -65,6 +67,13 @@ protected:
 	void	updateVehicles(float timeStep);
 
 	void	startProfiling(float timeStep);
+
+	virtual void	internalSingleStepSimulation( float timeStep);
+
+	void	synchronizeMotionStates();
+
+	void	saveKinematicState(float timeStep);
+
 public:
 
 
@@ -76,7 +85,7 @@ public:
 		
 	virtual ~btDiscreteDynamicsWorld();
 		
-	virtual void	stepSimulation( float timeStep);
+	virtual void	stepSimulation( float timeStep, int numSubsteps=1);
 
 	virtual void	updateAabbs();
 
@@ -113,6 +122,10 @@ public:
 		return m_debugDrawer;
 	}
 
+	virtual void	setGravity(const btVector3& gravity);
+
+	virtual void	addRigidBody(btRigidBody* body);
+
 };
 
 #endif //BT_DISCRETE_DYNAMICS_WORLD_H
diff --git a/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDynamicsWorld.h
index 38b55e81f..86f25dd00 100644
--- a/src/BulletDynamics/Dynamics/btDynamicsWorld.h
+++ b/src/BulletDynamics/Dynamics/btDynamicsWorld.h
@@ -38,7 +38,7 @@ class btDynamicsWorld : public btCollisionWorld
 		}
 		
 		///stepSimulation proceeds the simulation over timeStep units
-		virtual void	stepSimulation( float timeStep) = 0;
+		virtual void	stepSimulation( float timeStep,int numSubsteps=1) = 0;
 				
 		virtual void	updateAabbs() = 0;
 				
@@ -50,6 +50,12 @@ class btDynamicsWorld : public btCollisionWorld
 
 		virtual btIDebugDraw*	getDebugDrawer() = 0;
 
+		//once a rigidbody is added to the dynamics world, it will get this gravity assigned
+		//existing rigidbodies in the world get gravity assigned too, during this method
+		virtual void	setGravity(const btVector3& gravity) = 0;
+
+		virtual void	addRigidBody(btRigidBody* body) = 0;
+
 };
 
 #endif //BT_DYNAMICS_WORLD_H
diff --git a/src/BulletDynamics/Dynamics/btMassProps.h b/src/BulletDynamics/Dynamics/btMassProps.h
deleted file mode 100644
index 48c6e7ddb..000000000
--- a/src/BulletDynamics/Dynamics/btMassProps.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef MASS_PROPS_H
-#define MASS_PROPS_H
-
-#include <LinearMath/btVector3.h>
-
-struct btMassProps {
-	btMassProps(float mass,const btVector3& inertiaLocal):
-	m_mass(mass),
-		m_inertiaLocal(inertiaLocal)
-	{
-	}
-	float   m_mass;
-	btVector3	m_inertiaLocal;
-};
-
-
-#endif
-
diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp
index baeafd3af..418f80e27 100644
--- a/src/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -14,10 +14,10 @@ subject to the following restrictions:
 */
 
 #include "btRigidBody.h"
-#include "btMassProps.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "LinearMath/btMinMax.h"
 #include <LinearMath/btTransformUtil.h>
+#include <LinearMath/btMotionState.h>
 
 float gLinearAirDamping = 1.f;
 //'temporarily' global variables
@@ -28,6 +28,44 @@ float gLinearSleepingTreshold = 0.8f;
 float gAngularSleepingTreshold = 1.0f;
 static int uniqueId = 0;
 
+btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
+: 
+	m_gravity(0.0f, 0.0f, 0.0f),
+	m_totalForce(0.0f, 0.0f, 0.0f),
+	m_totalTorque(0.0f, 0.0f, 0.0f),
+	m_linearVelocity(0.0f, 0.0f, 0.0f),
+	m_angularVelocity(0.f,0.f,0.f),
+	m_linearDamping(0.f),
+	m_angularDamping(0.5f),
+	m_kinematicTimeStep(0.f),
+	m_optionalMotionState(motionState),
+	m_contactSolverType(0),
+	m_frictionSolverType(0)
+{
+
+	btQuaternion worldOrn;
+	btVector3	worldPos;
+	motionState->getWorldOrientation(worldOrn);
+	motionState->getWorldPosition(worldPos);
+	m_worldTransform = btTransform(worldOrn,worldPos);
+
+	//moved to btCollisionObject
+	m_friction = friction;
+	m_restitution = restitution;
+
+	m_collisionShape = collisionShape;
+	m_debugBodyId = uniqueId++;
+	
+	//m_internalOwner is to allow upcasting from collision object to rigid body
+	m_internalOwner = this;
+
+	setMassProps(mass, localInertia);
+    setDamping(linearDamping, angularDamping);
+	updateInertiaTensor();
+
+}
+
+
 btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
 : 
 	m_gravity(0.0f, 0.0f, 0.0f),
@@ -38,13 +76,11 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi
 	m_linearDamping(0.f),
 	m_angularDamping(0.5f),
 	m_kinematicTimeStep(0.f),
+	m_optionalMotionState(0),
 	m_contactSolverType(0),
 	m_frictionSolverType(0)
 {
-
-	if (mass == 0.f)
-		m_collisionFlags = btCollisionObject::isStatic;
-
+	
 	m_worldTransform = worldTransform;
 
 	//moved to btCollisionObject
@@ -64,11 +100,7 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi
 }
 
 
-void btRigidBody::setLinearVelocity(const btVector3& lin_vel)
-{ 
-	assert (m_collisionFlags != btCollisionObject::isStatic);
-	m_linearVelocity = lin_vel; 
-}
+
 
 
 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) const
@@ -126,9 +158,8 @@ void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
 
 void btRigidBody::applyForces(btScalar step)
 {
-	if (IsStatic())
+	if (isStaticObject() || isKinematicObject())
 		return;
-
 	
 	applyCentralForce(m_gravity);	
 	
@@ -178,15 +209,14 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
 {
 	if (mass == 0.f)
 	{
-		m_collisionFlags = btCollisionObject::isStatic;
+		m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
 		m_inverseMass = 0.f;
 	} else
 	{
-		m_collisionFlags = 0;
+		m_collisionFlags & (~btCollisionObject::CF_STATIC_OBJECT);
 		m_inverseMass = 1.0f / mass;
 	}
 	
-
 	m_invInertiaLocal.setValue(inertia[0] != 0.0f ? 1.0f / inertia[0]: 0.0f,
 						   inertia[1] != 0.0f ? 1.0f / inertia[1]: 0.0f,
 						   inertia[2] != 0.0f ? 1.0f / inertia[2]: 0.0f);
@@ -203,7 +233,7 @@ void btRigidBody::updateInertiaTensor()
 
 void btRigidBody::integrateVelocities(btScalar step) 
 {
-	if (IsStatic())
+	if (isStaticObject() || isKinematicObject())
 		return;
 
 	m_linearVelocity += m_totalForce * (m_inverseMass * step);
diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h
index f1419f270..fab607bee 100644
--- a/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/src/BulletDynamics/Dynamics/btRigidBody.h
@@ -25,7 +25,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 class btCollisionShape;
-struct btMassProps;
+class btMotionState;
+
 typedef btScalar dMatrix3[4*3];
 
 extern float gLinearAirDamping;
@@ -57,9 +58,13 @@ class btRigidBody  : public btCollisionObject
 	
 	btScalar		m_kinematicTimeStep;
 
+	//m_optionalMotionState allows to automatic synchronize the world transform for active objects
+	btMotionState*	m_optionalMotionState;
+
 public:
 
 	btRigidBody(float mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f);
+	btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f);
 
 	void			proceedToTransform(const btTransform& newTrans); 
 	
@@ -138,9 +143,8 @@ public:
 	
   	void applyTorqueImpulse(const btVector3& torque)
 	{
-		if (!IsStatic())
+		if (!isStaticObject()&& !isKinematicObject())
 			m_angularVelocity += m_invInertiaTensorWorld * torque;
-
 	}
 	
 	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) 
@@ -176,9 +180,14 @@ public:
 	}
 	
 
-	void setLinearVelocity(const btVector3& lin_vel);
-	void setAngularVelocity(const btVector3& ang_vel) { 
-		if (!IsStatic())
+	inline void setLinearVelocity(const btVector3& lin_vel)
+	{ 
+		assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT);
+		m_linearVelocity = lin_vel; 
+	}
+
+	inline void setAngularVelocity(const btVector3& ang_vel) { 
+		assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT);
 		{
 			m_angularVelocity = ang_vel; 
 		}
@@ -274,7 +283,21 @@ public:
 	{
 		m_broadphaseHandle = broadphaseProxy;
 	}
-	
+
+	//btMotionState allows to automatic synchronize the world transform for active objects
+	btMotionState*	getMotionState()
+	{
+		return m_optionalMotionState;
+	}
+	const btMotionState*	getMotionState() const
+	{
+		return m_optionalMotionState;
+	}
+	void	setMotionState(btMotionState* motionState)
+	{
+		m_optionalMotionState = motionState;
+	}
+
 	//for experimental overriding of friction/contact solver func
 	int	m_contactSolverType;
 	int	m_frictionSolverType;
diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
index a1674d766..514234e08 100644
--- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -25,7 +25,8 @@ subject to the following restrictions:
 btSimpleDynamicsWorld::btSimpleDynamicsWorld()
 :m_constraintSolver(new btSequentialImpulseConstraintSolver),
 m_ownsConstraintSolver(true),
-m_debugDrawer(0)
+m_debugDrawer(0),
+m_gravity(0,0,-10)
 {
 }
 
@@ -33,7 +34,8 @@ btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlapp
 :btDynamicsWorld(dispatcher,pairCache),
 m_constraintSolver(constraintSolver),
 m_ownsConstraintSolver(false),
-m_debugDrawer(0)
+m_debugDrawer(0),
+m_gravity(0,0,-10)
 {
 
 }
@@ -69,6 +71,26 @@ void	btSimpleDynamicsWorld::stepSimulation(float timeStep)
 }
 
 
+void	btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
+{
+	m_gravity = gravity;
+	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->setGravity(gravity);
+		}
+	}
+}
+
+void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
+{
+	body->setGravity(m_gravity);
+
+	addCollisionObject(body);
+}
 
 void	btSimpleDynamicsWorld::updateAabbs()
 {
@@ -79,7 +101,7 @@ void	btSimpleDynamicsWorld::updateAabbs()
 		btRigidBody* body = btRigidBody::upcast(colObj);
 		if (body)
 		{
-			if (body->IsActive() && (!body->IsStatic()))
+			if (body->IsActive() && (!body->isStaticObject()))
 			{
 				btPoint3 minAabb,maxAabb;
 				colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb);
@@ -99,7 +121,7 @@ void	btSimpleDynamicsWorld::integrateTransforms(float timeStep)
 		btRigidBody* body = btRigidBody::upcast(colObj);
 		if (body)
 		{
-			if (body->IsActive() && (!body->IsStatic()))
+			if (body->IsActive() && (!body->isStaticObject()))
 			{
 				body->predictIntegratedTransform(timeStep, predictedTrans);
 				body->proceedToTransform( predictedTrans);
@@ -118,12 +140,14 @@ void	btSimpleDynamicsWorld::predictUnconstraintMotion(float timeStep)
 		btRigidBody* body = btRigidBody::upcast(colObj);
 		if (body)
 		{
-			if (body->IsActive() && (!body->IsStatic()))
+			if (!body->isStaticObject())
 			{
-				body->applyForces( timeStep);
-				body->integrateVelocities( timeStep);
-				body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
-
+				if (body->IsActive())
+				{
+					body->applyForces( timeStep);
+					body->integrateVelocities( timeStep);
+					body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
+				}
 			}
 		}
 	}
diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
index 52725830c..5a8488c7a 100644
--- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
+++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
@@ -41,6 +41,7 @@ protected:
 	
 	void	integrateTransforms(float timeStep);
 		
+	btVector3	m_gravity;
 	
 public:
 
@@ -65,6 +66,10 @@ public:
 		return m_debugDrawer;
 	}
 
+	virtual void	setGravity(const btVector3& gravity);
+
+	virtual void	addRigidBody(btRigidBody* body);
+
 	virtual void	updateAabbs();
 };
 
diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
index 856fbd007..63615cdb9 100644
--- a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
+++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
@@ -18,7 +18,6 @@
 #include "btWheelInfo.h"
 
 
-#include "BulletDynamics/Dynamics/btMassProps.h"
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
 
 
diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/src/BulletDynamics/Vehicle/btRaycastVehicle.h
index cc38dd9fc..e29d7bf99 100644
--- a/src/BulletDynamics/Vehicle/btRaycastVehicle.h
+++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.h
@@ -14,7 +14,7 @@
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 
-struct btMassProps;
+
 #include "btWheelInfo.h"
 
 struct	btVehicleRaycaster;
diff --git a/src/LinearMath/btQuickprof.h b/src/LinearMath/btQuickprof.h
index 84433c2b4..801ef07a9 100644
--- a/src/LinearMath/btQuickprof.h
+++ b/src/LinearMath/btQuickprof.h
@@ -30,7 +30,7 @@
 #ifndef QUICK_PROF_H
 #define QUICK_PROF_H
 
-//#define USE_QUICKPROF 1
+#define USE_QUICKPROF 1
 
 #ifdef USE_QUICKPROF
 
diff --git a/src/btBulletCollisionCommon.h b/src/btBulletCollisionCommon.h
index 28095757f..2950adc0e 100644
--- a/src/btBulletCollisionCommon.h
+++ b/src/btBulletCollisionCommon.h
@@ -37,7 +37,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
 #include "BulletCollision/CollisionShapes/btEmptyShape.h"
 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
-
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
 
 ///Narrowphase Collision Detector
 #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
diff --git a/src/btBulletDynamicsCommon.h b/src/btBulletDynamicsCommon.h
index 2fe951ddf..3a40fad99 100644
--- a/src/btBulletDynamicsCommon.h
+++ b/src/btBulletDynamicsCommon.h
@@ -22,7 +22,6 @@ subject to the following restrictions:
 #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
 #include "BulletDynamics/Dynamics/btSimpleDynamicsWorld.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "BulletDynamics/Dynamics/btMassProps.h"
 
 #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"