From 31e0d0fd384b1c4c25d9bd373d3dfe8cd79c1f95 Mon Sep 17 00:00:00 2001
From: erwin coumans <erwin.coumans@gmail.com>
Date: Mon, 3 Jun 2013 15:17:06 -0700
Subject: [PATCH] add support for motion state and internal pre-tick callback

---
 demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp | 38 +++++++++---
 demos3/BasicGpuDemo/b3GpuDynamicsWorld.h   | 69 +++++++++++-----------
 2 files changed, 66 insertions(+), 41 deletions(-)

diff --git a/demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp b/demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp
index 9def1e17e..fa6936d80 100644
--- a/demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp
+++ b/demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp
@@ -18,7 +18,7 @@
 
 
 #ifdef _WIN32
-	#include <wiNdOws.h>
+	#include <windows.h>
 #endif
 
 
@@ -42,7 +42,7 @@ b3GpuDynamicsWorld::~b3GpuDynamicsWorld()
 
 
 
-int		b3GpuDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
+int		b3GpuDynamicsWorld::stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
 {
 #ifndef BT_NO_PROFILE
 	CProfileManager::Reset();
@@ -62,6 +62,11 @@ int		b3GpuDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btSc
 		m_rigidBodyPipeline->writeAllInstancesToGpu();
 	}
 
+	// dispatch preTick callback
+	if(0 != m_internalPreTickCallback) {
+		(*m_internalPreTickCallback)(this, fixedTimeStep);
+	}	
+
 	m_rigidBodyPipeline->stepSimulation(fixedTimeStep);
 
 	{
@@ -71,7 +76,6 @@ int		b3GpuDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btSc
 			m_np->readbackAllBodiesToCpu();
 		}
 
-		
 		{
 			BT_PROFILE("scatter transforms into rigidbody (CPU)");
 			for (int i=0;i<this->m_collisionObjects.size();i++)
@@ -82,7 +86,14 @@ int		b3GpuDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btSc
 				btTransform newTrans;
 				newTrans.setOrigin(pos);
 				newTrans.setRotation(orn);
-				this->m_collisionObjects[i]->setWorldTransform(newTrans);
+
+				btCollisionObject* colObj = this->m_collisionObjects[i];
+				colObj->setWorldTransform(newTrans);
+
+				// synchronize motionstate
+				btRigidBody* body = btRigidBody::upcast(colObj);
+				if (body)
+					this->synchronizeSingleMotionState(body);
 			}
 		}
 	}
@@ -120,7 +131,7 @@ int b3GpuDynamicsWorld::findOrRegisterCollisionShape(const btCollisionShape* col
 			for (int i=0;i<numVertices;i++)
 				convex->getVertex(i,tmpVertices[i]);
 			const float scaling[4]={1,1,1,1};
-			bool noHeightField=true;
+			//bool noHeightField=true;
 			
 			int gpuShapeIndex = m_np->registerConvexHullShape(&tmpVertices[0].getX(), strideInBytes, numVertices, scaling);
 			m_uniqueShapeMapping.push_back(gpuShapeIndex);
@@ -190,7 +201,7 @@ int b3GpuDynamicsWorld::findOrRegisterCollisionShape(const btCollisionShape* col
 				//GraphicsShape* gfxShape = b3BulletDataExtractor::createGraphicsShapeFromConvexHull(&sUnitSpherePoints[0],MY_UNITSPHERE_POINTS);
 				float meshScaling[4] = {1,1,1,1};
 				//int shapeIndex = renderer.registerShape(gfxShape->m_vertices,gfxShape->m_numvertices,gfxShape->m_indices,gfxShape->m_numIndices);
-				float groundPos[4] = {0,0,0,0};
+				//float groundPos[4] = {0,0,0,0};
 				
 				//renderer.registerGraphicsInstance(shapeIndex,groundPos,rotOrn,color,meshScaling);
 				if (vertices.size() && indices.size())
@@ -278,7 +289,7 @@ int b3GpuDynamicsWorld::findOrRegisterCollisionShape(const btCollisionShape* col
 void	b3GpuDynamicsWorld::addRigidBody(btRigidBody* body)
 {
 	m_cpuGpuSync = true;
-	body->setMotionState(0);
+	//body->setMotionState(0);
 	
 
 	int index = findOrRegisterCollisionShape(body->getCollisionShape());
@@ -308,3 +319,16 @@ void	b3GpuDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3&
 
 }
 
+void	b3GpuDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
+{
+	btAssert(body);
+
+	if (body->getMotionState() && !body->isStaticOrKinematicObject())
+	{
+		//we need to call the update at least once, even for sleeping objects
+		//otherwise the 'graphics' transform never updates properly
+		const btTransform& centerOfMassWorldTrans = body->getWorldTransform();
+		body->getMotionState()->setWorldTransform(centerOfMassWorldTrans);
+	}
+}
+
diff --git a/demos3/BasicGpuDemo/b3GpuDynamicsWorld.h b/demos3/BasicGpuDemo/b3GpuDynamicsWorld.h
index 1658385b9..befc79905 100644
--- a/demos3/BasicGpuDemo/b3GpuDynamicsWorld.h
+++ b/demos3/BasicGpuDemo/b3GpuDynamicsWorld.h
@@ -64,50 +64,51 @@ public:
 		return m_gravity;
 	}
 
-		virtual void	addRigidBody(btRigidBody* body, short group, short mask)
-		{
-			addRigidBody(body);
-		}
+	virtual void	addRigidBody(btRigidBody* body, short group, short mask)
+	{
+		addRigidBody(body);
+	}
 
-		virtual void	removeRigidBody(btRigidBody* body)
-		{
-			btAssert(0);
-		}
+	virtual void	removeRigidBody(btRigidBody* body)
+	{
+		btAssert(0);
+	}
 
 
-		virtual void	addAction(btActionInterface* action) 
-		{
-			btAssert(0);
-		}
+	virtual void	addAction(btActionInterface* action) 
+	{
+		btAssert(0);
+	}
 
-		virtual void	removeAction(btActionInterface* action)
-		{
-			btAssert(0);
-		}
+	virtual void	removeAction(btActionInterface* action)
+	{
+		btAssert(0);
+	}
 
-		virtual void	setConstraintSolver(btConstraintSolver* solver)
-		{
-			btAssert(0);
-		}
+	virtual void	setConstraintSolver(btConstraintSolver* solver)
+	{
+		btAssert(0);
+	}
 
-		virtual btConstraintSolver* getConstraintSolver()
-		{
-			btAssert(0);
-			return 0;
-		}
+	virtual btConstraintSolver* getConstraintSolver()
+	{
+		btAssert(0);
+		return 0;
+	}
 		
 
-		virtual void	clearForces()
-		{
-			btAssert(0);
-		}
+	virtual void	clearForces()
+	{
+		btAssert(0);
+	}
 
-		virtual btDynamicsWorldType	getWorldType() const
-		{
-			return BT_GPU_DYNAMICS_WORLD;
-		}
+	virtual btDynamicsWorldType	getWorldType() const
+	{
+		return BT_GPU_DYNAMICS_WORLD;
+	}
 
+	///this can be useful to synchronize a single rigid body -> graphics object
+	void	synchronizeSingleMotionState(btRigidBody* body);
 };
 
-
 #endif //B3_GPU_DYNAMICS_WORLD_H