improvements/bugfixes related to kinematic(animated) objects, synchronizeMotionStates

Demos/BasicDemo/BasicDemo.cpp

@@ -38,7 +38,7 @@ int main(int argc,char** argv)
 
 	BasicDemo ccdDemo;
 	ccdDemo.initPhysics();
-	ccdDemo.setCameraDistance(10.f);
+	ccdDemo.setCameraDistance(50.f);
 
 #ifdef CHECK_MEMORY_LEAKS
 	ccdDemo.exitPhysics();

Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp

@@ -13,10 +13,10 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-//#define USER_DEFINED_FRICTION_MODEL 1
-//#define PRINT_CONTACT_STATISTICS 1
 //#define USE_KINEMATIC_GROUND 1
+//#define PRINT_CONTACT_STATISTICS 1
 //#define REGISTER_CUSTOM_COLLISION_ALGORITHM 1
+//#define USER_DEFINED_FRICTION_MODEL 1
 
 //following define allows to compare/replace Bullet's constraint solver with ODE quickstep
 //this define requires to either add the libquickstep library (win32, see msvc/8/libquickstep.vcproj) or manually add the files in Extras/quickstep
@@ -137,9 +137,21 @@ void CcdPhysicsDemo::clientMoveAndDisplay()
 
 #ifdef USE_KINEMATIC_GROUND
 	//btQuaternion kinRotation(btVector3(0,0,1),0.);
-	btVector3 kinTranslation(0,0,0.01);
+	btVector3 kinTranslation(-0.01,0,0);
 	//kinematic object
+	btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[0];
+	//is this a rigidbody with a motionstate? then use the motionstate to update positions!
+	if (btRigidBody::upcast(colObj) && btRigidBody::upcast(colObj)->getMotionState())
+	{
+		btTransform newTrans;
+		btRigidBody::upcast(colObj)->getMotionState()->getWorldTransform(newTrans);
+		newTrans.getOrigin()+=kinTranslation;
+		btRigidBody::upcast(colObj)->getMotionState()->setWorldTransform(newTrans);
+	} else
+	{
 		m_dynamicsWorld->getCollisionObjectArray()[0]->m_worldTransform.getOrigin() += kinTranslation;
+	}
+
 #endif //USE_KINEMATIC_GROUND
 
 	float dt = m_clock.getTimeMilliseconds() * 0.001f;
@@ -247,15 +259,17 @@ void	CcdPhysicsDemo::initPhysics()
 		
 
 #ifdef USER_DEFINED_FRICTION_MODEL
-	btSequentialImpulseConstraintSolver* solver = (btSequentialImpulseConstraintSolver*) m_physicsEnvironmentPtr->GetConstraintSolver();
+	{
 		//solver->setContactSolverFunc(ContactSolverFunc func,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE);
 		solver->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE);
 		solver->SetFrictionSolverFunc(myFrictionModel,DEFAULT_CONTACT_SOLVER_TYPE,USER_CONTACT_SOLVER_TYPE1);
 		solver->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,USER_CONTACT_SOLVER_TYPE1);
 		//m_physicsEnvironmentPtr->setNumIterations(2);
+	}
 #endif //USER_DEFINED_FRICTION_MODEL
 
 
+
 	int i;
 
 	btTransform tr;
@@ -331,7 +345,6 @@ void	CcdPhysicsDemo::initPhysics()
 		{
 			body->m_collisionFlags = btCollisionObject::CF_KINEMATIC_OJBECT;
 			body->SetActivationState(DISABLE_DEACTIVATION);
-			body->setLinearVelocity(btVector3(0,0,1));
 		}
 #endif //USE_KINEMATIC_GROUND
 		
@@ -344,7 +357,7 @@ void	CcdPhysicsDemo::initPhysics()
 
 #ifdef USER_DEFINED_FRICTION_MODEL	
 		///Advanced use: override the friction solver
-		ctrl->getRigidBody()->m_frictionSolverType = USER_CONTACT_SOLVER_TYPE1;
+		body->m_frictionSolverType = USER_CONTACT_SOLVER_TYPE1;
 #endif //USER_DEFINED_FRICTION_MODEL
 
 	}

Demos/ColladaDemo/ColladaConverter.cpp

@@ -545,8 +545,7 @@ void	ColladaConverter::prepareConstraints(ConstraintInput& input)
 					{
 						if (m_rigidBodies[i]->m_userObjectPointer)
 						{
-							btDefaultMotionState* ms = (btDefaultMotionState*)m_rigidBodies[i]->m_userObjectPointer;
+							char* bodyName = (char*)m_rigidBodies[i]->m_userObjectPointer;
-							char* bodyName = (char*)ms->m_userPointer;
 							if (!strcmp(bodyName,orgUri0))
 							{
 								body0=m_rigidBodies[i];
@@ -707,9 +706,8 @@ void	ColladaConverter::PreparePhysicsObject(struct btRigidBodyInput& input, bool
 	btRigidBody* body= createRigidBody(isDynamics,mass,startTransform,colShape);
 	if (body && body->m_userObjectPointer)
 	{
-		//for bodyName lookup in constraints
+		//bodyName is used as identifier for constraints
-		btDefaultMotionState* ms = (btDefaultMotionState*)body->m_userObjectPointer;
+		body->m_userObjectPointer = (void*)input.m_bodyName;
-		ms->m_userPointer = (void*)input.m_bodyName;
 		m_rigidBodies[m_numObjects] = body;
 		m_numObjects++;
 	}

Demos/OpenGL/DemoApplication.cpp

@@ -650,10 +650,16 @@ btRigidBody*	DemoApplication::localCreateRigidBody(float mass, const btTransform
 	if (isDynamic)
 		shape->calculateLocalInertia(mass,localInertia);
 
+	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+
+#define USE_MOTIONSTATE 1
+#ifdef USE_MOTIONSTATE
 	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
 	btRigidBody* body = new btRigidBody(mass,myMotionState,shape,localInertia);
-	body->m_userObjectPointer = myMotionState;
 
+#else
+	btRigidBody* body = new btRigidBody(mass,startTransform,shape,localInertia);	
+#endif//
 	m_dynamicsWorld->addRigidBody(body);
 	
 	return body;
@@ -675,10 +681,11 @@ void DemoApplication::renderme()
 		for (int i=0;i<numObjects;i++)
 		{
 			btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[i];
+			btRigidBody* body = btRigidBody::upcast(colObj);
 
-			if (colObj->m_userObjectPointer)
+			if (body && body->getMotionState())
 			{
-				btDefaultMotionState* myMotionState = (btDefaultMotionState*)colObj->m_userObjectPointer;
+				btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
 				myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);
 			} else
 			{
@@ -819,10 +826,10 @@ void	DemoApplication::clientResetScene()
 	for (int i=0;i<numObjects;i++)
 	{
 		btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[i];
-		
+		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (colObj->m_userObjectPointer)
+		if (body && body->getMotionState())
 		{
-			btDefaultMotionState* myMotionState = (btDefaultMotionState*)colObj->m_userObjectPointer;
+			btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
 			myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans;
 			colObj->m_worldTransform = myMotionState->m_graphicsWorldTrans;
 			colObj->m_interpolationWorldTransform = myMotionState->m_startWorldTrans;

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -92,6 +92,11 @@ struct	btCollisionObject
 		return m_collisionFlags & CF_KINEMATIC_OJBECT;
 	}
 
+	inline bool		isStaticOrKinematicObject() const
+	{
+		return m_collisionFlags & (CF_KINEMATIC_OJBECT | CF_STATIC_OBJECT);
+	}
+
 	inline bool		hasContactResponse() const {
 		return !(m_collisionFlags & CF_NO_CONTACT_RESPONSE);
 	}

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -237,6 +237,16 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 		 if (((colObj0) && colObj0->GetActivationState() != ISLAND_SLEEPING) ||
 			((colObj1) && colObj1->GetActivationState() != ISLAND_SLEEPING))
 		{
+			//kinematic objects don't merge islands, but wake up all connected objects
+			if (colObj0->isKinematicObject() && colObj0->GetActivationState() != ISLAND_SLEEPING)
+			{
+				colObj1->SetActivationState(ACTIVE_TAG);
+			}
+			if (colObj1->isKinematicObject() && colObj1->GetActivationState() != ISLAND_SLEEPING)
+			{
+				colObj0->SetActivationState(ACTIVE_TAG);
+			}
+
 			//filtering for response
 			if (dispatcher->needsResponse(colObj0,colObj1))
 				islandmanifold.push_back(manifold);

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -145,7 +145,7 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 			debugDrawObject(colObj->m_worldTransform,colObj->m_collisionShape,color);
 		}
 		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body && body->getMotionState())
+		if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
 		{
 			if (body->GetActivationState() != ISLAND_SLEEPING)
 			{
@@ -524,7 +524,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(float timeStep)
 		btRigidBody* body = btRigidBody::upcast(colObj);
 		if (body)
 		{
-			if (body->IsActive() && (!body->isStaticObject()))
+			if (body->IsActive() && (!body->isStaticOrKinematicObject()))
 			{
 				body->predictIntegratedTransform(timeStep, predictedTrans);
 				body->proceedToTransform( predictedTrans);
@@ -545,7 +545,7 @@ void	btDiscreteDynamicsWorld::predictUnconstraintMotion(float timeStep)
 		btRigidBody* body = btRigidBody::upcast(colObj);
 		if (body)
 		{
-			if (!body->isStaticObject())
+			if (!body->isStaticOrKinematicObject())
 			{
 				if (body->IsActive())
 				{

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -37,7 +37,6 @@ btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShap
 	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)
@@ -73,7 +72,6 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi
 	m_angularVelocity(0.f,0.f,0.f),
 	m_linearDamping(0.f),
 	m_angularDamping(0.5f),
-	m_kinematicTimeStep(0.f),
 	m_optionalMotionState(0),
 	m_contactSolverType(0),
 	m_frictionSolverType(0)
@@ -109,15 +107,17 @@ void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& pred
 
 void			btRigidBody::saveKinematicState(btScalar timeStep)
 {
-	
+	//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
-	if (m_kinematicTimeStep)
+	if (timeStep != 0.f)
 	{
+		//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
+		if (getMotionState())
+			getMotionState()->getWorldTransform(m_worldTransform);
 		btVector3 linVel,angVel;
-		btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,m_kinematicTimeStep,m_linearVelocity,m_angularVelocity);
+		btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
+		m_interpolationWorldTransform = m_worldTransform;
 		//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
 	}
-	
-	m_kinematicTimeStep = timeStep;
 }
 	
 void	btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
@@ -154,7 +154,7 @@ void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
 
 void btRigidBody::applyForces(btScalar step)
 {
-	if (isStaticObject() || isKinematicObject())
+	if (isStaticOrKinematicObject())
 		return;
 	
 	applyCentralForce(m_gravity);	
@@ -229,7 +229,7 @@ void btRigidBody::updateInertiaTensor()
 
 void btRigidBody::integrateVelocities(btScalar step) 
 {
-	if (isStaticObject() || isKinematicObject())
+	if (isStaticOrKinematicObject())
 		return;
 
 	m_linearVelocity += m_totalForce * (m_inverseMass * step);

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -56,7 +56,6 @@ class btRigidBody  : public btCollisionObject
 	btScalar		m_linearDamping;
 	btScalar		m_angularDamping;
 	
-	btScalar		m_kinematicTimeStep;
 
 	//m_optionalMotionState allows to automatic synchronize the world transform for active objects
 	btMotionState*	m_optionalMotionState;
@@ -148,7 +147,6 @@ public:
 	
   	void applyTorqueImpulse(const btVector3& torque)
 	{
-		if (!isStaticObject()&& !isKinematicObject())
 			m_angularVelocity += m_invInertiaTensorWorld * torque;
 	}