Support kinematic multibody joints/links

6 files changed, 237 insertions, 128 deletions

diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt
index 3332440f2..b85e3851b 100644
--- a/src/BulletDynamics/CMakeLists.txt
+++ b/src/BulletDynamics/CMakeLists.txt
@@ -38,6 +38,7 @@ SET(BulletDynamics_SRCS
 	Featherstone/btMultiBodyGearConstraint.cpp
 	Featherstone/btMultiBodyJointLimitConstraint.cpp
 	Featherstone/btMultiBodyJointMotor.cpp
+	Featherstone/btMultiBodyLink.cpp
 	Featherstone/btMultiBodyMLCPConstraintSolver.cpp
 	Featherstone/btMultiBodyPoint2Point.cpp
 	Featherstone/btMultiBodySliderConstraint.cpp
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index 8c361cf8b..b98e8d5e5 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -898,31 +898,53 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 		// calculate zhat_i^A
 		//
-		//external forces
-		btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
-		btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
+		if (isLinkStaticOrKinematic(i))
+		{
+			zeroAccSpatFrc[i].setZero();
+		}
+		else{
+			//external forces
+			btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
+			btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
 
-		zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
+			zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
 
 #if 0	
-		{
+			{
 
-			b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
-			i+1,
-			zeroAccSpatFrc[i+1].m_topVec[0],
-			zeroAccSpatFrc[i+1].m_topVec[1],
-			zeroAccSpatFrc[i+1].m_topVec[2],
+				b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
+				i+1,
+				zeroAccSpatFrc[i+1].m_topVec[0],
+				zeroAccSpatFrc[i+1].m_topVec[1],
+				zeroAccSpatFrc[i+1].m_topVec[2],
 
-			zeroAccSpatFrc[i+1].m_bottomVec[0],
-			zeroAccSpatFrc[i+1].m_bottomVec[1],
-			zeroAccSpatFrc[i+1].m_bottomVec[2]);
-		}
+				zeroAccSpatFrc[i+1].m_bottomVec[0],
+				zeroAccSpatFrc[i+1].m_bottomVec[1],
+				zeroAccSpatFrc[i+1].m_bottomVec[2]);
+			}
 #endif
-		//
-		//adding damping terms (only)
-		btScalar linDampMult = 1., angDampMult = 1.;
-		zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
-										linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
+			//
+			//adding damping terms (only)
+			btScalar linDampMult = 1., angDampMult = 1.;
+			zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
+											linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
+			//p += vhat x Ihat vhat - done in a simpler way
+			if (m_useGyroTerm)
+				zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
+			//
+			zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
+			//
+			//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
+			////clamp parent's omega
+			//btScalar parOmegaMod = temp.length();
+			//btScalar parOmegaModMax = 1000;
+			//if(parOmegaMod > parOmegaModMax)
+			//	temp *= parOmegaModMax / parOmegaMod;
+			//zeroAccSpatFrc[i+1].addLinear(temp);
+			//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
+			//temp = spatCoriolisAcc[i].getLinear();
+			//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
+		}
 
 		// calculate Ihat_i^A
 		//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
@@ -935,22 +957,6 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 									 btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
 												 0, m_links[i].m_inertiaLocal[1], 0,
 												 0, 0, m_links[i].m_inertiaLocal[2]));
-		//
-		//p += vhat x Ihat vhat - done in a simpler way
-		if (m_useGyroTerm)
-			zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
-		//
-		zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
-		//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
-		////clamp parent's omega
-		//btScalar parOmegaMod = temp.length();
-		//btScalar parOmegaModMax = 1000;
-		//if(parOmegaMod > parOmegaModMax)
-		//	temp *= parOmegaModMax / parOmegaMod;
-		//zeroAccSpatFrc[i+1].addLinear(temp);
-		//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
-		//temp = spatCoriolisAcc[i].getLinear();
-		//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
 
 		//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
 		//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
@@ -961,6 +967,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	// (part of TreeForwardDynamics in Mirtich.)
 	for (int i = num_links - 1; i >= 0; --i)
 	{
+		if(isLinkStaticOrKinematic(i))
+			continue;
 		const int parent = m_links[i].m_parent;
 		fromParent.m_rotMat = rot_from_parent[i + 1];
 		fromParent.m_trnVec = m_links[i].m_cachedRVector;
@@ -1107,21 +1115,23 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 		fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
 
-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		if(!isLinkStaticOrKinematic(i))
 		{
-			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
-			//
-			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
-		}
-
-		btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
-		//D^{-1} * (Y - h^{T}*apar)
-		mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
+			for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+			{
+				const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+				//
+				Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
+			}
+			btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
+			//D^{-1} * (Y - h^{T}*apar)
+			mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
 
-		spatAcc[i + 1] += spatCoriolisAcc[i];
+			spatAcc[i + 1] += spatCoriolisAcc[i];
 
-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-			spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
+			for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+				spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
+		}
 
 		if (m_links[i].m_jointFeedback)
 		{
@@ -1477,6 +1487,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	// (part of TreeForwardDynamics in Mirtich.)
 	for (int i = num_links - 1; i >= 0; --i)
 	{
+		if(isLinkStaticOrKinematic(i))
+			continue;
 		const int parent = m_links[i].m_parent;
 		fromParent.m_rotMat = rot_from_parent[i + 1];
 		fromParent.m_trnVec = m_links[i].m_cachedRVector;
@@ -1533,6 +1545,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	// now do the loop over the m_links
 	for (int i = 0; i < num_links; ++i)
 	{
+		if(isLinkStaticOrKinematic(i))
+			continue;
 		const int parent = m_links[i].m_parent;
 		fromParent.m_rotMat = rot_from_parent[i + 1];
 		fromParent.m_trnVec = m_links[i].m_cachedRVector;
@@ -1676,55 +1690,88 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
         btScalar *pJointPos;
         pJointPos = &m_links[i].m_jointPos_interpolate[0];
         
-        btScalar *pJointVel = getJointVelMultiDof(i);
-        
-        switch (m_links[i].m_jointType)
-        {
-            case btMultibodyLink::ePrismatic:
-            case btMultibodyLink::eRevolute:
-            {
-                //reset to current pos
-                pJointPos[0] = m_links[i].m_jointPos[0];
-                btScalar jointVel = pJointVel[0];
-                pJointPos[0] += dt * jointVel;
-                break;
-            }
-            case btMultibodyLink::eSpherical:
-            {
-                //reset to current pos
-
-                for (int j = 0; j < 4; ++j)
+        if(m_links[i].isStaticOrKinematic()) 
+				{
+            switch (m_links[i].m_jointType) 
+						{
+                case btMultibodyLink::ePrismatic:
+                case btMultibodyLink::eRevolute:
                 {
-                    pJointPos[j] = m_links[i].m_jointPos[j];
+                    pJointPos[0] = m_links[i].m_jointPos[0];
+                    break;
                 }
-                
-                btVector3 jointVel;
-                jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
-                btQuaternion jointOri;
-                jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
-                pQuatUpdateFun(jointVel, jointOri, false, dt);
-                pJointPos[0] = jointOri.x();
-                pJointPos[1] = jointOri.y();
-                pJointPos[2] = jointOri.z();
-                pJointPos[3] = jointOri.w();
-                break;
-            }
-            case btMultibodyLink::ePlanar:
-            {
-                for (int j = 0; j < 3; ++j)
+                case btMultibodyLink::eSpherical:
+                {
+                    for (int j = 0; j < 4; ++j)
+                    {
+                        pJointPos[j] = m_links[i].m_jointPos[j];
+                    }
+                    break;
+                }
+                case btMultibodyLink::ePlanar:
                 {
-                    pJointPos[j] = m_links[i].m_jointPos[j];
+                    for (int j = 0; j < 3; ++j)
+                    {
+                        pJointPos[j] = m_links[i].m_jointPos[j];
+                    }
+                    break;
                 }
-                pJointPos[0] += dt * getJointVelMultiDof(i)[0];
-                
-                btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
-                btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
-                pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
-                pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
-                break;
+                default:
+                   break;
             }
-            default:
+        }
+        else
+        {
+            btScalar *pJointVel = getJointVelMultiDof(i); 
+
+            switch (m_links[i].m_jointType)
             {
+                case btMultibodyLink::ePrismatic:
+                case btMultibodyLink::eRevolute:
+                {
+                    //reset to current pos
+                    pJointPos[0] = m_links[i].m_jointPos[0];
+                    btScalar jointVel = pJointVel[0];
+                    pJointPos[0] += dt * jointVel;
+                    break;
+                }
+                case btMultibodyLink::eSpherical:
+                {
+                    //reset to current pos
+
+                    for (int j = 0; j < 4; ++j)
+                    {
+                        pJointPos[j] = m_links[i].m_jointPos[j];
+                    }
+                    
+                    btVector3 jointVel;
+                    jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+                    btQuaternion jointOri;
+                    jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+                    pQuatUpdateFun(jointVel, jointOri, false, dt);
+                    pJointPos[0] = jointOri.x();
+                    pJointPos[1] = jointOri.y();
+                    pJointPos[2] = jointOri.z();
+                    pJointPos[3] = jointOri.w();
+                    break;
+                }
+                case btMultibodyLink::ePlanar:
+                {
+                    for (int j = 0; j < 3; ++j)
+                    {
+                        pJointPos[j] = m_links[i].m_jointPos[j];
+                    }
+                    pJointPos[0] += dt * getJointVelMultiDof(i)[0];
+                    
+                    btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
+                    btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
+                    pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
+                    pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
+                    break;
+                }
+                default:
+                {
+                }
             }
         }
         
@@ -1826,48 +1873,51 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 	// Finally we can update m_jointPos for each of the m_links
 	for (int i = 0; i < num_links; ++i)
 	{
-        btScalar *pJointPos;
-        pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
-        
-		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
-
-		switch (m_links[i].m_jointType)
+		if(!m_links[i].isStaticOrKinematic())
 		{
-			case btMultibodyLink::ePrismatic:
-			case btMultibodyLink::eRevolute:
-			{
-                //reset to current pos
-				btScalar jointVel = pJointVel[0];
-				pJointPos[0] += dt * jointVel;
-				break;
-			}
-			case btMultibodyLink::eSpherical:
-			{
-                //reset to current pos
-				btVector3 jointVel;
-				jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
-				btQuaternion jointOri;
-				jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
-				pQuatUpdateFun(jointVel, jointOri, false, dt);
-				pJointPos[0] = jointOri.x();
-				pJointPos[1] = jointOri.y();
-				pJointPos[2] = jointOri.z();
-				pJointPos[3] = jointOri.w();
-				break;
-			}
-			case btMultibodyLink::ePlanar:
+			btScalar *pJointPos;
+			pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
+		
+			btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
+
+			switch (m_links[i].m_jointType)
 			{
-				pJointPos[0] += dt * getJointVelMultiDof(i)[0];
+				case btMultibodyLink::ePrismatic:
+				case btMultibodyLink::eRevolute:
+				{
+    	            //reset to current pos
+					btScalar jointVel = pJointVel[0];
+					pJointPos[0] += dt * jointVel;
+					break;
+				}
+				case btMultibodyLink::eSpherical:
+				{
+    	            //reset to current pos
+					btVector3 jointVel;
+					jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+					btQuaternion jointOri;
+					jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+					pQuatUpdateFun(jointVel, jointOri, false, dt);
+					pJointPos[0] = jointOri.x();
+					pJointPos[1] = jointOri.y();
+					pJointPos[2] = jointOri.z();
+					pJointPos[3] = jointOri.w();
+					break;
+				}
+				case btMultibodyLink::ePlanar:
+				{
+					pJointPos[0] += dt * getJointVelMultiDof(i)[0];
 
-				btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
-				btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
-				pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
-				pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
+					btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
+					btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
+					pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
+					pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
 
-				break;
-			}
-			default:
-			{
+					break;
+				}
+				default:
+				{
+				}
 			}
 		}
 
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index cf965047e..94e9b317f 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -716,6 +716,29 @@ public:
 		btVector3 &top_out,         // top part of output vector
 		btVector3 &bottom_out);      // bottom part of output vector
 
+	void setLinkDynamicType(const int i, int type)
+  {
+  	if(i == -1)
+  	{
+  		setBaseDynamicType(type);
+  	}
+  	else if (i >= 0 && i < getNumLinks())
+  	{
+  		m_links[i].setDynamicType(type);
+  	}
+  }
+
+	bool isLinkStaticOrKinematic(const int i) const
+	{
+		if(i == -1)
+		{
+			return isBaseStaticOrKinematic();
+		}
+		else
+		{
+			return m_links[i].isStaticOrKinematic();
+		}
+	}
 
 
 private:
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.cpp b/src/BulletDynamics/Featherstone/btMultiBodyLink.cpp
new file mode 100644
index 000000000..cd508b344
--- /dev/null
+++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.cpp
@@ -0,0 +1,31 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  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 "btMultiBodyLink.h"
+#include "btMultiBodyLinkCollider.h"
+
+bool btMultibodyLink::isStaticOrKinematic() const
+{
+	return m_collider && m_collider->isStaticOrKinematicObject();
+}
+
+void btMultibodyLink::setDynamicType(int dynamicType)
+{
+	if(m_collider) {
+	  int oldFlags = m_collider->getCollisionFlags();
+	  oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT);
+	  m_collider->setCollisionFlags(oldFlags | dynamicType);
+	}
+}
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
index 01d5583c2..23cd2ba4f 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -295,6 +295,9 @@ struct btMultibodyLink
             }
         }
     }
+
+    bool isStaticOrKinematic() const;
+    void setDynamicType(int dynamicType);
 };
 
 #endif  //BT_MULTIBODY_LINK_H
diff --git a/src/btBulletDynamicsAll.cpp b/src/btBulletDynamicsAll.cpp
index a8069e30a..de182fa51 100644
--- a/src/btBulletDynamicsAll.cpp
+++ b/src/btBulletDynamicsAll.cpp
@@ -30,6 +30,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.cpp"
 #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodyLink.cpp"
 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp"
 #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp"