Merge pull request #3132 from WenlongLu/kinematic_multibody_base

Kinematic Multibody Base

examples/ExampleBrowser/CMakeLists.txt

@@ -350,7 +350,8 @@ SET(BulletExampleBrowser_SRCS
   ../MultiBody/TestJointTorqueSetup.h
   ../MultiBody/InvertedPendulumPDControl.cpp
   ../MultiBody/InvertedPendulumPDControl.h
- ../MultiBody/MultiBodyConstraintFeedback.cpp
+  ../MultiBody/MultiBodyConstraintFeedback.cpp
+  ../MultiBody/KinematicMultiBodyExample.cpp
   ../SoftDemo/SoftDemo.cpp
   ../SoftDemo/SoftDemo.h
   ../DeformableDemo/DeformableContact.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -33,6 +33,7 @@
 #include "../MultiBody/MultiBodyConstraintFeedback.h"
 #include "../MultiBody/MultiDofDemo.h"
 #include "../MultiBody/InvertedPendulumPDControl.h"
+#include "../MultiBody/KinematicMultiBodyExample.h"
 
 #include "../RigidBody/RigidBodySoftContact.h"
 #include "../RigidBody/KinematicRigidBodyExample.h"
@@ -156,6 +157,7 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
 		ExampleEntry(1, "Inverted Pendulum PD", "Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc),
 		ExampleEntry(1, "MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", MultiBodySoftContactCreateFunc, 0),
+    ExampleEntry(1, "Kinematic MultiBody", "Let the user set the transform, the physics engine computes the velocity for one-way contact and friction interaction.", KinematicMultiBodyExampleCreateFunc),
 
 		ExampleEntry(0, "Physics Client-Server"),
 		ExampleEntry(1, "Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.",

examples/MultiBody/KinematicMultiBodyExample.cpp

@@ -0,0 +1,178 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+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 "KinematicMultiBodyExample.h"
+//#define USE_MOTIONSTATE 1
+#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btTransformUtil.h"
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Z 5
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "../OpenGLWindow/ShapeData.h"
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+namespace {
+
+void kinematicPreTickCallback(btDynamicsWorld* world, btScalar deltaTime)
+{
+	btMultiBody* groundBody = (btMultiBody*)world->getWorldUserInfo();
+	btTransform predictedTrans;
+	btVector3 linearVelocity(0, 0, 0);
+	btVector3 angularVelocity(0, 0.1, 0);
+	btTransformUtil::integrateTransform(groundBody->getBaseWorldTransform(), linearVelocity, angularVelocity, deltaTime, predictedTrans);
+	groundBody->setBaseWorldTransform(predictedTrans);
+}
+
+struct KinematicMultiBodyExample : public CommonMultiBodyBase
+{
+	btMultiBody* m_groundBody;
+
+	KinematicMultiBodyExample(struct GUIHelperInterface* helper)
+		: CommonMultiBodyBase(helper),
+		  m_groundBody(0)
+	{
+	}
+
+	virtual void stepSimulation(float deltaTime)
+	{
+		if (m_dynamicsWorld)
+		{
+			m_dynamicsWorld->stepSimulation(deltaTime);
+		}
+	}
+
+	virtual ~KinematicMultiBodyExample() {}
+	virtual void initPhysics();
+	void resetCamera()
+	{
+		float dist = 4;
+		float pitch = -30;
+		float yaw = 50;
+		float targetPos[3] = {0, 0, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+void KinematicMultiBodyExample::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	createEmptyDynamicsWorld();
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+	if (m_dynamicsWorld->getDebugDrawer())
+		m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints);
+
+	///create a few basic rigid bodies
+	btScalar halfExtentsX = 10.0;
+	btScalar halfExtentsY = 0.1;
+	btScalar halfExtentsZ = 10.0;
+
+	btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10.), btScalar(0.1), btScalar(10.)));
+	btTransform groundTransform;
+	groundTransform.setIdentity();
+	groundTransform.setOrigin(btVector3(0, -halfExtentsY, 0));
+	m_collisionShapes.push_back(groundShape);
+
+
+
+	{
+		bool floating = false;
+		int numLinks = 0;
+		bool canSleep = false;
+		btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
+		float baseMass = 1.f;
+
+		if (baseMass)
+		{
+			btCollisionShape* pTempBox = new btBoxShape(btVector3(10, 10, 10));
+			pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
+			delete pTempBox;
+		}
+		btTransform startTransform;
+		startTransform.setIdentity();
+
+		m_groundBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
+		m_groundBody->setBasePos(startTransform.getOrigin());
+		m_groundBody->setWorldToBaseRot(startTransform.getRotation());
+		m_groundBody->finalizeMultiDof();
+		m_dynamicsWorld->addMultiBody(m_groundBody);
+
+
+		btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(m_groundBody, -1);
+		col->setCollisionShape(groundShape);
+		bool isDynamic = (baseMass > 0 && floating);
+		int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
+		int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+		m_dynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);  //, 2,1+2);
+		m_groundBody->setBaseCollider(col);
+		m_groundBody->setBaseDynamicType(btCollisionObject::CF_KINEMATIC_OBJECT);
+
+	}
+	m_dynamicsWorld->setInternalTickCallback(kinematicPreTickCallback, m_groundBody, true);
+
+	{
+		//create a few dynamic rigidbodies
+		// Re-using the same collision is better for memory usage and performance
+		btBoxShape* colShape = createBoxShape(btVector3(.1, .1, .1));
+
+		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
+		m_collisionShapes.push_back(colShape);
+
+		/// Create Dynamic Objects
+		btTransform startTransform;
+		startTransform.setIdentity();
+
+		btScalar mass(1.f);
+
+		//rigidbody is dynamic if and only if mass is non zero, otherwise static
+		bool isDynamic = (mass != 0.f);
+
+		btVector3 localInertia(0, 0, 0);
+		if (isDynamic)
+			colShape->calculateLocalInertia(mass, localInertia);
+
+		for (int k = 0; k < ARRAY_SIZE_Y; k++)
+		{
+			for (int i = 0; i < ARRAY_SIZE_X; i++)
+			{
+				for (int j = 0; j < ARRAY_SIZE_Z; j++)
+				{
+					startTransform.setOrigin(btVector3(
+						btScalar(0.2 * i),
+						btScalar(2 + .2 * k),
+						btScalar(0.2 * j)));
+
+					createRigidBody(mass, startTransform, colShape);
+				}
+			}
+		}
+	}
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+}
+
+CommonExampleInterface* KinematicMultiBodyExampleCreateFunc(CommonExampleOptions& options)
+{
+	return new KinematicMultiBodyExample(options.m_guiHelper);
+}
+
+B3_STANDALONE_EXAMPLE(KinematicMultiBodyExampleCreateFunc)

examples/MultiBody/KinematicMultiBodyExample.h

@@ -0,0 +1,21 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+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 KINEMATIC_MULTI_BODY_EXAMPLE_H
+#define KINEMATIC_MULTI_BODY_EXAMPLE_H
+
+class CommonExampleInterface* KinematicMultiBodyExampleCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //KINEMATIC_MULTI_BODY_EXAMPLE_H

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -127,6 +127,7 @@ public:
 
 	enum CollisionFlags
 	{
+		CF_DYNAMIC_OBJECT = 0,
 		CF_STATIC_OBJECT = 1,
 		CF_KINEMATIC_OBJECT = 2,
 		CF_NO_CONTACT_RESPONSE = 4,

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -673,6 +673,30 @@ btScalar *btMultiBody::getJointTorqueMultiDof(int i)
 	return &m_links[i].m_jointTorque[0];
 }
 
+bool btMultiBody::hasFixedBase() const
+{
+	return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticObject());
+}
+
+bool btMultiBody::isBaseStaticOrKinematic() const
+{
+	return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticOrKinematicObject());
+}
+
+bool btMultiBody::isBaseKinematic() const
+{
+	return getBaseCollider() && getBaseCollider()->isKinematicObject();
+}
+
+void btMultiBody::setBaseDynamicType(int dynamicType)
+{
+	if(getBaseCollider()) {
+		int oldFlags = getBaseCollider()->getCollisionFlags();
+		oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT);
+		getBaseCollider()->setCollisionFlags(oldFlags | dynamicType);
+	}
+}
+
 inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)  //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
 {
 	btVector3 row0 = btVector3(
@@ -798,7 +822,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	//create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
 	spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
 
-	if (m_fixedBase)
+	if (isBaseStaticOrKinematic())
 	{
 		zeroAccSpatFrc[0].setZero();
 	}
@@ -1049,7 +1073,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	// Second 'upward' loop
 	// (part of TreeForwardDynamics in Mirtich)
 
-	if (m_fixedBase)
+	if (isBaseStaticOrKinematic())
 	{
 		spatAcc[0].setZero();
 	}
@@ -1434,7 +1458,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 
 	// Fill in zero_acc
 	// -- set to force/torque on the base, zero otherwise
-	if (m_fixedBase)
+	if (isBaseStaticOrKinematic())
 	{
 		zeroAccSpatFrc[0].setZero();
 	}
@@ -1496,7 +1520,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	// Second 'upward' loop
 	// (part of TreeForwardDynamics in Mirtich)
 
-	if (m_fixedBase)
+	if (isBaseStaticOrKinematic())
 	{
 		spatAcc[0].setZero();
 	}
@@ -1552,23 +1576,26 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 void btMultiBody::predictPositionsMultiDof(btScalar dt)
 {
     int num_links = getNumLinks();
-    // step position by adding dt * velocity
-    //btVector3 v = getBaseVel();
-    //m_basePos += dt * v;
-    //
-    btScalar *pBasePos;
-    btScalar *pBaseVel = &m_realBuf[3];  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+		if(!isBaseKinematic())
+		{
+      // step position by adding dt * velocity
+      //btVector3 v = getBaseVel();
+      //m_basePos += dt * v;
+      //
+      btScalar *pBasePos;
+      btScalar *pBaseVel = &m_realBuf[3];  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
     
-    // reset to current position
-    for (int i = 0; i < 3; ++i)
-    {
-        m_basePos_interpolate[i] = m_basePos[i];
-    }
-    pBasePos = m_basePos_interpolate;
-    
-    pBasePos[0] += dt * pBaseVel[0];
-    pBasePos[1] += dt * pBaseVel[1];
-    pBasePos[2] += dt * pBaseVel[2];
+    	// reset to current position
+    	for (int i = 0; i < 3; ++i)
+    	{
+    	    m_basePos_interpolate[i] = m_basePos[i];
+    	}
+    	pBasePos = m_basePos_interpolate;
+    	
+    	pBasePos[0] += dt * pBaseVel[0];
+    	pBasePos[1] += dt * pBaseVel[1];
+    	pBasePos[2] += dt * pBaseVel[2];
+		}
     
     ///////////////////////////////
     //local functor for quaternion integration (to avoid error prone redundancy)
@@ -1619,26 +1646,29 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
     
     //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
     //
-    btScalar *pBaseQuat;
+		if(!isBaseKinematic())
+		{
+        btScalar *pBaseQuat;
 
-    // reset to current orientation
-    for (int i = 0; i < 4; ++i)
-    {
-        m_baseQuat_interpolate[i] = m_baseQuat[i];
-    }
-    pBaseQuat = m_baseQuat_interpolate;
+        // reset to current orientation
+        for (int i = 0; i < 4; ++i)
+        {
+            m_baseQuat_interpolate[i] = m_baseQuat[i];
+        }
+        pBaseQuat = m_baseQuat_interpolate;
 
-    btScalar *pBaseOmega = &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
-    //
-    btQuaternion baseQuat;
-    baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
-    btVector3 baseOmega;
-    baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
-    pQuatUpdateFun(baseOmega, baseQuat, true, dt);
-    pBaseQuat[0] = baseQuat.x();
-    pBaseQuat[1] = baseQuat.y();
-    pBaseQuat[2] = baseQuat.z();
-    pBaseQuat[3] = baseQuat.w();
+        btScalar *pBaseOmega = &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
+        //
+        btQuaternion baseQuat;
+        baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+        btVector3 baseOmega;
+        baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+        pQuatUpdateFun(baseOmega, baseQuat, true, dt);
+        pBaseQuat[0] = baseQuat.x();
+        pBaseQuat[1] = baseQuat.y();
+        pBaseQuat[2] = baseQuat.z();
+        pBaseQuat[3] = baseQuat.w();
+		}
 
     // Finally we can update m_jointPos for each of the m_links
     for (int i = 0; i < num_links; ++i)
@@ -1705,16 +1735,19 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
 void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
 {
 	int num_links = getNumLinks();
-	// step position by adding dt * velocity
-	//btVector3 v = getBaseVel();
-	//m_basePos += dt * v;
-	//
-    btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
-    btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
-    
-	pBasePos[0] += dt * pBaseVel[0];
-	pBasePos[1] += dt * pBaseVel[1];
-	pBasePos[2] += dt * pBaseVel[2];
+	if(!isBaseKinematic())
+	{
+		// step position by adding dt * velocity
+		//btVector3 v = getBaseVel();
+		//m_basePos += dt * v;
+		//
+  	  btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
+  	  btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+  	  
+		pBasePos[0] += dt * pBaseVel[0];
+		pBasePos[1] += dt * pBaseVel[1];
+		pBasePos[2] += dt * pBaseVel[2];
+	}
 
 	///////////////////////////////
 	//local functor for quaternion integration (to avoid error prone redundancy)
@@ -1765,22 +1798,25 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 
 	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
 	//
-    btScalar *pBaseQuat = pq ? pq : m_baseQuat;
-	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
-	//
-	btQuaternion baseQuat;
-	baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
-	btVector3 baseOmega;
-	baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
-	pQuatUpdateFun(baseOmega, baseQuat, true, dt);
-	pBaseQuat[0] = baseQuat.x();
-	pBaseQuat[1] = baseQuat.y();
-	pBaseQuat[2] = baseQuat.z();
-	pBaseQuat[3] = baseQuat.w();
+	if(!isBaseKinematic())
+	{
+		btScalar *pBaseQuat = pq ? pq : m_baseQuat;
+		btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
+		//
+		btQuaternion baseQuat;
+		baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+		btVector3 baseOmega;
+		baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+		pQuatUpdateFun(baseOmega, baseQuat, true, dt);
+		pBaseQuat[0] = baseQuat.x();
+		pBaseQuat[1] = baseQuat.y();
+		pBaseQuat[2] = baseQuat.z();
+		pBaseQuat[3] = baseQuat.w();
 
-	//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
-	//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
-	//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
+		//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
+		//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
+		//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
+	}
 
 	if (pq)
 		pq += 7;
@@ -2137,8 +2173,15 @@ void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObj
     world_to_local.resize(getNumLinks() + 1);
     local_origin.resize(getNumLinks() + 1);
     
-    world_to_local[0] = getInterpolateWorldToBaseRot();
-    local_origin[0] = getInterpolateBasePos();
+		if(isBaseKinematic()){
+        world_to_local[0] = getWorldToBaseRot();
+        local_origin[0] = getBasePos();
+		}
+		else
+		{
+        world_to_local[0] = getInterpolateWorldToBaseRot();
+        local_origin[0] = getInterpolateBasePos();
+		}
     
     if (getBaseCollider())
     {
@@ -2284,3 +2327,16 @@ const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *seriali
 
 	return btMultiBodyDataName;
 }
+
+void btMultiBody::saveKinematicState(btScalar timeStep)
+{
+	//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
+	if (timeStep != btScalar(0.))
+	{
+		btVector3 linearVelocity, angularVelocity;
+		btTransformUtil::calculateVelocity(getInterpolateBaseWorldTransform(), getBaseWorldTransform(), timeStep, linearVelocity, angularVelocity);
+		setBaseVel(linearVelocity);
+		setBaseOmega(angularVelocity);
+		setInterpolateBaseWorldTransform(getBaseWorldTransform());
+	}
+}

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -210,7 +210,13 @@ public:
 	void setBasePos(const btVector3 &pos)
 	{
 		m_basePos = pos;
-        m_basePos_interpolate = pos;
+		if(!isBaseKinematic())
+			m_basePos_interpolate = pos;
+	}
+
+	void setInterpolateBasePos(const btVector3 &pos)
+	{
+		m_basePos_interpolate = pos;
 	}
 
 	void setBaseWorldTransform(const btTransform &tr)
@@ -227,17 +233,39 @@ public:
 		return tr;
 	}
 
+	void setInterpolateBaseWorldTransform(const btTransform &tr)
+	{
+		setInterpolateBasePos(tr.getOrigin());
+		setInterpolateWorldToBaseRot(tr.getRotation().inverse());
+	}
+
+	btTransform getInterpolateBaseWorldTransform() const
+	{
+		btTransform tr;
+		tr.setOrigin(getInterpolateBasePos());
+		tr.setRotation(getInterpolateWorldToBaseRot().inverse());
+		return tr;
+	}
+
 	void setBaseVel(const btVector3 &vel)
 	{
 		m_realBuf[3] = vel[0];
 		m_realBuf[4] = vel[1];
 		m_realBuf[5] = vel[2];
 	}
+
 	void setWorldToBaseRot(const btQuaternion &rot)
 	{
 		m_baseQuat = rot;  //m_baseQuat asumed to ba alias!?
-        m_baseQuat_interpolate = rot;
+		if(!isBaseKinematic())
+			m_baseQuat_interpolate = rot;
 	}
+
+	void setInterpolateWorldToBaseRot(const btQuaternion &rot)
+	{
+		m_baseQuat_interpolate = rot;
+	}
+
 	void setBaseOmega(const btVector3 &omega)
 	{
 		m_realBuf[0] = omega[0];
@@ -245,6 +273,8 @@ public:
 		m_realBuf[2] = omega[2];
 	}
 
+	void saveKinematicState(btScalar timeStep);
+
 	//
 	// get/set pos/vel for child m_links (i = 0 to num_links-1)
 	//
@@ -520,14 +550,22 @@ public:
 	void goToSleep();
 	void checkMotionAndSleepIfRequired(btScalar timestep);
 
-	bool hasFixedBase() const
-	{
-		return m_fixedBase;
-	}
+	bool hasFixedBase() const;
+
+	bool isBaseKinematic() const;
+
+	bool isBaseStaticOrKinematic() const;
+
+	// set the dynamic type in the base's collision flags.
+	void setBaseDynamicType(int dynamicType);
 
 	void setFixedBase(bool fixedBase)
 	{
 		m_fixedBase = fixedBase;
+		if(m_fixedBase)
+			setBaseDynamicType(btCollisionObject::CF_STATIC_OBJECT);
+		else
+			setBaseDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT);
 	}
 
 	int getCompanionId() const

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -869,6 +869,18 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
 		}
 	}
 }
+
+void btMultiBodyDynamicsWorld::saveKinematicState(btScalar timeStep)
+{
+	btDiscreteDynamicsWorld::saveKinematicState(timeStep);
+	for(int i = 0; i < m_multiBodies.size(); i++)
+	{
+		btMultiBody* body = m_multiBodies[i];
+		if(body->isBaseKinematic())
+			body->saveKinematicState(timeStep);
+	}
+}
+
 //
 //void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
 //{

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -120,5 +120,7 @@ public:
     virtual void solveExternalForces(btContactSolverInfo& solverInfo);
     virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
     void buildIslands();
+
+	virtual void saveKinematicState(btScalar timeStep);
 };
 #endif  //BT_MULTIBODY_DYNAMICS_WORLD_H