change btMultiBody to allow removal/changing of links

(still not easy to modify the link structure of a btMultiBody -> You have to manually re-link parents, copying links around etc)

Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp

@@ -213,9 +213,17 @@ void	FeatherstoneMultiBodyDemo::initPhysics()
 	
 	
 	mb = createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false,true);
-	
+	bool testRemoveLinks = false;
-	
+	if (testRemoveLinks)
-
+	{
+		while (mb->getNumLinks())
+		{
+			btCollisionObject* col = mb->getLink(mb->getNumLinks()-1).m_collider;
+			m_dynamicsWorld->removeCollisionObject(col);
+			delete col;
+			mb->setNumLinks(mb->getNumLinks()-1);
+		}
+	}
 }
 
 btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep)
@@ -304,21 +312,21 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
 			{
 				btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2]));
 				btRigidBody* body = new btRigidBody(mass,0,box,inertia);
-				btMultiBodyLinkCollider* multiBody= new btMultiBodyLinkCollider(bod,-1);
+				btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(bod,-1);
 
 				body->setCollisionShape(box);
-				multiBody->setCollisionShape(box);
+				col->setCollisionShape(box);
 								
 				btTransform tr;
 				tr.setIdentity();
 				tr.setOrigin(local_origin[0]);
 				tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
 				body->setWorldTransform(tr);
-				multiBody->setWorldTransform(tr);
+				col->setWorldTransform(tr);
 				
-				world->addCollisionObject(multiBody, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);
+				world->addCollisionObject(col, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);
-				multiBody->setFriction(1);
+				col->setFriction(1);
-				bod->addLinkCollider(multiBody);
+				bod->setBaseCollider(col);
 				
 			}
 		}
@@ -351,8 +359,8 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
 			col->setWorldTransform(tr);
 			col->setFriction(1);
 			world->addCollisionObject(col,btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);//,2,1);
-				
+			
-			bod->addLinkCollider(col);
+			bod->getLink(i).m_collider=col;
 			//app->drawBox(halfExtents, pos,quat);
 		}
 

src/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://bulletphysics.com/Bullet/
+Copyright (c) 2003-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.
@@ -18,13 +18,11 @@ subject to the following restrictions:
  * @mainpage Bullet Documentation
  *
  * @section intro_sec Introduction
- * Bullet Collision Detection & Physics SDK
- *
  * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
  *
  * The main documentation is Bullet_User_Manual.pdf, included in the source code distribution.
  * There is the Physics Forum for feedback and general Collision Detection and Physics discussions.
- * Please visit http://www.bulletphysics.com
+ * Please visit http://www.bulletphysics.org
  *
  * @section install_sec Installation
  *
@@ -32,7 +30,16 @@ subject to the following restrictions:
  * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list
  *
  * @subsection step2 Step 2: Building
- * Bullet main build system for all platforms is cmake, you can download http://www.cmake.org
+ * Bullet has multiple build systems, including premake, cmake and autotools. Premake and cmake support all platforms.
+ * Premake is included in the Bullet/build folder for Windows, Mac OSX and Linux. 
+ * Under Windows you can click on Bullet/build/vs2010.bat to create Microsoft Visual Studio projects. 
+ * On Mac OSX and Linux you can open a terminal and generate Makefile, codeblocks or Xcode4 projects:
+ * cd Bullet/build
+ * ./premake4_osx gmake or ./premake4_linux gmake or ./premake4_linux64 gmake or (for Mac) ./premake4_osx xcode4
+ * cd Bullet/build/gmake
+ * make
+ * 
+ * An alternative to premake is cmake. You can download cmake from http://www.cmake.org
  * cmake can autogenerate projectfiles for Microsoft Visual Studio, Apple Xcode, KDevelop and Unix Makefiles.
  * The easiest is to run the CMake cmake-gui graphical user interface and choose the options and generate projectfiles.
  * You can also use cmake in the command-line. Here are some examples for various platforms:

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -81,15 +81,15 @@ btMultiBody::btMultiBody(int n_links,
                      const btVector3 &inertia,
                      bool fixed_base_,
                      bool can_sleep_)
-    : num_links(n_links),
+    : base_quat(0, 0, 0, 1),
-      base_quat(0, 0, 0, 1),
       base_mass(mass),
       base_inertia(inertia),
     
       fixed_base(fixed_base_),
       awake(true),
       can_sleep(can_sleep_),
-      sleep_timer(0)
+      sleep_timer(0),
+	  m_baseCollider(0)
 {
 	 links.resize(n_links);
 
@@ -146,24 +146,8 @@ void btMultiBody::setupRevolute(int i,
     links[i].updateCache();
 }
 
-void btMultiBody::addLinkCollider(btMultiBodyLinkCollider* collider)
-{
-	m_colliders.push_back(collider);
-}
 
-btMultiBodyLinkCollider* btMultiBody::getLinkCollider(int index)
-{
-	return m_colliders[index];
-}
-const btMultiBodyLinkCollider* btMultiBody::getLinkCollider(int index) const
-{
-	return m_colliders[index];
-}
 
-int btMultiBody::getNumLinkColliders() const
-{
-	return m_colliders.size();
-}
 
 	
 int btMultiBody::getParent(int i) const
@@ -262,6 +246,7 @@ btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
 
 void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
 {
+	int num_links = getNumLinks();
     // Calculates the velocities of each link (and the base) in its local frame
     omega[0] = quatRotate(base_quat ,getBaseOmega());
     vel[0] = quatRotate(base_quat ,getBaseVel());
@@ -282,6 +267,7 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
 
 btScalar btMultiBody::getKineticEnergy() const
 {
+	int num_links = getNumLinks();
     // TODO: would be better not to allocate memory here
     btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
 	btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
@@ -301,6 +287,7 @@ btScalar btMultiBody::getKineticEnergy() const
 
 btVector3 btMultiBody::getAngularMomentum() const
 {
+	int num_links = getNumLinks();
     // TODO: would be better not to allocate memory here
     btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
 	btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
@@ -324,7 +311,7 @@ void btMultiBody::clearForcesAndTorques()
     base_force.setValue(0, 0, 0);
     base_torque.setValue(0, 0, 0);
 
-    for (int i = 0; i < num_links; ++i) {
+    for (int i = 0; i < getNumLinks(); ++i) {
         links[i].applied_force.setValue(0, 0, 0);
         links[i].applied_torque.setValue(0, 0, 0);
         links[i].joint_torque = 0;
@@ -333,7 +320,7 @@ void btMultiBody::clearForcesAndTorques()
 
 void btMultiBody::clearVelocities()
 {
-	for (int i = 0; i < 6 + num_links; ++i) 
+	for (int i = 0; i < 6 + getNumLinks(); ++i) 
 	{
 		real_buf[i] = 0.f;
 	}
@@ -406,6 +393,8 @@ void btMultiBody::stepVelocities(btScalar dt,
     // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
     // num_links joint acceleration values.
     
+	int num_links = getNumLinks();
+
     const btScalar DAMPING_K1 = btScalar(0.0);
     //const btScalar DAMPING_K2 = btScalar(0);
 	const btScalar DAMPING_K2 = btScalar(0.0);
@@ -670,6 +659,7 @@ void btMultiBody::stepVelocities(btScalar dt,
 
 void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
 {
+	int num_links = getNumLinks();
 	///solve I * x = rhs, so the result = invI * rhs
     if (num_links == 0) 
 	{
@@ -722,7 +712,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
 {
     // Temporary matrices/vectors -- use scratch space from caller
     // so that we don't have to keep reallocating every frame
-
+	int num_links = getNumLinks();
     scratch_r.resize(num_links);
     scratch_v.resize(4*num_links + 4);
 
@@ -843,6 +833,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
 
 void btMultiBody::stepPositions(btScalar dt)
 {
+	int num_links = getNumLinks();
     // step position by adding dt * velocity
 	btVector3 v = getBaseVel();
     base_pos += dt * v;
@@ -885,6 +876,7 @@ void btMultiBody::fillContactJacobian(int link,
                                     btAlignedObjectArray<btMatrix3x3> &scratch_m) const
 {
     // temporary space
+	int num_links = getNumLinks();
     scratch_v.resize(2*num_links + 2);
     scratch_m.resize(num_links + 1);
 
@@ -967,6 +959,8 @@ void btMultiBody::goToSleep()
 
 void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
 {
+	int num_links = getNumLinks();
+
     if (!can_sleep) return;
 
     // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -69,11 +69,31 @@ public:
                        const btVector3 &joint_axis,    // in my frame
                        const btVector3 &parent_axis_position,    // vector from parent COM to joint axis, in PARENT frame
                        const btVector3 &my_axis_position);       // vector from joint axis to my COM, in MY frame
+	
+	const btMultibodyLink& getLink(int index) const
+	{
+		return links[index];
+	}
+
+	btMultibodyLink& getLink(int index)
+	{
+		return links[index];
+	}
+
+
+	void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base
+	{
+		m_baseCollider = collider;
+	}
+	const btMultiBodyLinkCollider* getBaseCollider() const
+	{
+		return m_baseCollider;
+	}
+	btMultiBodyLinkCollider* getBaseCollider()
+	{
+		return m_baseCollider;
+	}
 
-	void addLinkCollider(btMultiBodyLinkCollider* collider);
-	btMultiBodyLinkCollider* getLinkCollider(int index);
-	const btMultiBodyLinkCollider* getLinkCollider(int index) const;
-	int getNumLinkColliders() const;
     //
     // get parent
     // input: link num from 0 to num_links-1
@@ -86,7 +106,7 @@ public:
     // get number of links, masses, moments of inertia
     //
 
-    int getNumLinks() const { return num_links; }
+    int getNumLinks() const { return links.size(); }
     btScalar getBaseMass() const { return base_mass; }
     const btVector3 & getBaseInertia() const { return base_inertia; }
     btScalar getLinkMass(int i) const;
@@ -238,12 +258,12 @@ public:
     // apply a delta-vee directly. used in sequential impulses code.
     void applyDeltaVee(const btScalar * delta_vee) 
 	{
-        for (int i = 0; i < 6 + num_links; ++i) 
+        for (int i = 0; i < 6 + getNumLinks(); ++i) 
 			real_buf[i] += delta_vee[i];
     }
     void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) 
 	{
-        for (int i = 0; i < 6 + num_links; ++i) 
+        for (int i = 0; i < 6 + getNumLinks(); ++i) 
 			real_buf[i] += delta_vee[i] * multiplier;
     }
 
@@ -290,6 +310,11 @@ public:
 		//printf("for %p setCompanionId(%d)\n",this, id);
 		m_companionId = id;
 	}
+
+	void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links
+	{
+		links.resize(numLinks);
+	}
 private:
     btMultiBody(const btMultiBody &);  // not implemented
     void operator=(const btMultiBody &);  // not implemented
@@ -300,7 +325,8 @@ private:
     
 	
 private:
-    int num_links;  // includes base.
+
+	btMultiBodyLinkCollider* m_baseCollider;//can be NULL
 
     btVector3 base_pos;       // position of COM of base (world frame)
     btQuaternion base_quat;   // rotates world points into base frame
@@ -311,7 +337,7 @@ private:
     btVector3 base_force;     // external force applied to base. World frame.
     btVector3 base_torque;    // external torque applied to base. World frame.
     
-    btAlignedObjectArray<btMultibodyLink> links;    // array of links, excluding the base. index from 0 to num_links-2.
+    btAlignedObjectArray<btMultibodyLink> links;    // array of links, excluding the base. index from 0 to num_links-1.
 	btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
     
     //

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -78,17 +78,17 @@ void	btMultiBodyDynamicsWorld::calculateSimulationIslands()
 		}
 	}
 
-	//merge islands linked by Featherstone links
+	//merge islands linked by Featherstone link colliders
 	for (int i=0;i<m_multiBodies.size();i++)
 	{
 		btMultiBody* body = m_multiBodies[i];
-		if (body->getNumLinkColliders()>1)
 		{
-			btMultiBodyLinkCollider* prev = body->getLinkCollider(0);
+			btMultiBodyLinkCollider* prev = body->getBaseCollider();
-			for (int b=1;b<body->getNumLinkColliders();b++)
-			{
-				btMultiBodyLinkCollider* cur = body->getLinkCollider(b);
 
+			for (int b=0;b<body->getNumLinks();b++)
+			{
+				btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
+				
 				if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
 					((prev) && (!(prev)->isStaticOrKinematicObject())))
 				{
@@ -96,7 +96,8 @@ void	btMultiBodyDynamicsWorld::calculateSimulationIslands()
 					int tagCur = cur->getIslandTag();
 					getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
 				}
-				prev = cur;
+				if (cur && !cur->isStaticOrKinematicObject())
+					prev = cur;
 				
 			}
 		}
@@ -122,10 +123,16 @@ void	btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 			body->checkMotionAndSleepIfRequired(timeStep);
 			if (!body->isAwake())
 			{
-				for (int b=0;b<body->getNumLinkColliders();b++)
+				btMultiBodyLinkCollider* col = body->getBaseCollider();
+				if (col && col->getActivationState() == ACTIVE_TAG)
 				{
-					btMultiBodyLinkCollider* col = body->getLinkCollider(b);
+					col->setActivationState( WANTS_DEACTIVATION);
-					if (col->getActivationState() == ACTIVE_TAG)
+					col->setDeactivationTime(0.f);
+				}
+				for (int b=0;b<body->getNumLinks();b++)
+				{
+					btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
+					if (col && col->getActivationState() == ACTIVE_TAG)
 					{
 						col->setActivationState( WANTS_DEACTIVATION);
 						col->setDeactivationTime(0.f);
@@ -133,10 +140,14 @@ void	btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 				}
 			} else
 			{
-				for (int b=0;b<body->getNumLinkColliders();b++)
+				btMultiBodyLinkCollider* col = body->getBaseCollider();
+				if (col && col->getActivationState() != DISABLE_DEACTIVATION)
+					col->setActivationState( ACTIVE_TAG );
+
+				for (int b=0;b<body->getNumLinks();b++)
 				{
-					btMultiBodyLinkCollider* col = body->getLinkCollider(b);
+					btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
-					if (col->getActivationState() != DISABLE_DEACTIVATION)
+					if (col && col->getActivationState() != DISABLE_DEACTIVATION)
 						col->setActivationState( ACTIVE_TAG );
 				}
 			}
@@ -415,10 +426,17 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 			btMultiBody* bod = m_multiBodies[i];
 
 			bool isSleeping = false;
-			if (bod->getNumLinkColliders() && bod->getLinkCollider(0)->getActivationState()==ISLAND_SLEEPING)
+			
+			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
 			{
 				isSleeping = true;
 			} 
+			for (int b=0;b<bod->getNumLinks();b++)
+			{
+				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+					isSleeping = true;
+			} 
+
 			if (!isSleeping)
 			{
 				scratch_r.resize(bod->getNumLinks()+1);
@@ -458,10 +476,17 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 		{
 			btMultiBody* bod = m_multiBodies[b];
 			bool isSleeping = false;
-			if (bod->getNumLinkColliders() && bod->getLinkCollider(0)->getActivationState()==ISLAND_SLEEPING)
+			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
 			{
 				isSleeping = true;
 			} 
+			for (int b=0;b<bod->getNumLinks();b++)
+			{
+				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+					isSleeping = true;
+			}
+
+
 			if (!isSleeping)
 			{
 				int nLinks = bod->getNumLinks();
@@ -476,6 +501,20 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 
 				world_to_local[0] = bod->getWorldToBaseRot();
 				local_origin[0] = bod->getBasePos();
+
+				if (bod->getBaseCollider())
+				{
+					btVector3 posr = local_origin[0];
+					float pos[4]={posr.x(),posr.y(),posr.z(),1};
+					float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
+					btTransform tr;
+					tr.setIdentity();
+					tr.setOrigin(posr);
+					tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+
+					bod->getBaseCollider()->setWorldTransform(tr);
+
+				}
       
 				for (int k=0;k<bod->getNumLinks();k++)
 				{
@@ -484,25 +523,27 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 					local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
 				}
 
-				for (int m=0;m<bod->getNumLinkColliders();m++)
+
+				for (int m=0;m<bod->getNumLinks();m++)
 				{
-					btMultiBodyLinkCollider* col = bod->getLinkCollider(m);
+					btMultiBodyLinkCollider* col = bod->getLink(m).m_collider;
-					int link = col->m_link;
+					if (col)
-					int index = link+1;
+					{
+						int link = col->m_link;
+						btAssert(link == m);
 
-					float halfExtents[3]={7.5,0.05,4.5};
+						int index = link+1;
-					btVector3 posr = local_origin[index];
-					float pos[4]={posr.x(),posr.y(),posr.z(),1};
-			
-					float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
-					//app->drawBox(halfExtents, pos,quat);
 
-					btTransform tr;
+						btVector3 posr = local_origin[index];
-					tr.setIdentity();
+						float pos[4]={posr.x(),posr.y(),posr.z(),1};
-					tr.setOrigin(posr);
+						float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
-					tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+						btTransform tr;
+						tr.setIdentity();
+						tr.setOrigin(posr);
+						tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
 
-					col->setWorldTransform(tr);
+						col->setWorldTransform(tr);
+					}
 				}
 			} else
 			{

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -28,12 +28,30 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
 
 int btMultiBodyJointLimitConstraint::getIslandIdA() const
 {
-	return m_bodyA->getLinkCollider(0)->getIslandTag();
+	btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+	if (col)
+		return col->getIslandTag();
+	for (int i=0;i<m_bodyA->getNumLinks();i++)
+	{
+		if (m_bodyA->getLink(i).m_collider)
+			return m_bodyA->getLink(i).m_collider->getIslandTag();
+	}
+	return -1;
 }
 
 int btMultiBodyJointLimitConstraint::getIslandIdB() const
 {
-	return m_bodyB->getLinkCollider(0)->getIslandTag();
+	btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+	if (col)
+		return col->getIslandTag();
+
+	for (int i=0;i<m_bodyB->getNumLinks();i++)
+	{
+		col = m_bodyB->getLink(i).m_collider;
+		if (col)
+			return col->getIslandTag();
+	}
+	return -1;
 }
 
 

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -63,6 +63,8 @@ struct btMultibodyLink
     btVector3 applied_torque;   // In WORLD frame
     btScalar joint_torque;
 
+	class btMultiBodyLinkCollider* m_collider;
+
     // ctor: set some sensible defaults
 	btMultibodyLink()
 		: joint_pos(0),
@@ -71,7 +73,8 @@ struct btMultibodyLink
 			zero_rot_parent_to_this(1, 0, 0, 0),
 			is_revolute(false),
 			cached_rot_parent_to_this(1, 0, 0, 0),
-			joint_torque(0)
+			joint_torque(0),
+			m_collider(0)
 	{
 		inertia.setValue(1, 1, 1);
 		axis_top.setValue(0, 0, 0);