First draft of btMultiBody serialization, including optional names for base, link and joints (see ImportURDFDemo/ImportURDFSetup.cpp how this is done)

Bump up version number to 2.84 because of new serialization data.

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -26,7 +26,7 @@
 #include "btMultiBodyLinkCollider.h"
 #include "btMultiBodyJointFeedback.h"
 #include "LinearMath/btTransformUtil.h"
-
+#include "LinearMath/btSerializer.h"
 #include "Bullet3Common/b3Logging.h"
 // #define INCLUDE_GYRO_TERM 
 
@@ -95,6 +95,7 @@ btMultiBody::btMultiBody(int n_links,
 					 bool multiDof)
     : 
     	m_baseCollider(0),
+		m_baseName(0),
     	m_basePos(0,0,0),
     	m_baseQuat(0, 0, 0, 1),
       m_baseMass(mass),
@@ -400,6 +401,17 @@ btScalar * btMultiBody::getJointVelMultiDof(int i)
 	return &m_realBuf[6 + m_links[i].m_dofOffset];
 }
 
+const btScalar * btMultiBody::getJointPosMultiDof(int i) const 
+{
+	return &m_links[i].m_jointPos[0];
+}
+
+const btScalar * btMultiBody::getJointVelMultiDof(int i) const 
+{
+	return &m_realBuf[6 + m_links[i].m_dofOffset];
+}
+
+
 void btMultiBody::setJointPos(int i, btScalar q)
 {
     m_links[i].m_jointPos[0] = q;
@@ -2412,3 +2424,85 @@ void	btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to
 
 }
 
+
+int	btMultiBody::calculateSerializeBufferSize()	const
+{
+	int sz = sizeof(btMultiBodyData);
+	return sz;
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
+{
+		btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
+		getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
+		mbd->m_baseMass = this->getBaseMass();
+		getBaseInertia().serialize(mbd->m_baseInertia);
+		{
+			char* name = (char*) serializer->findNameForPointer(m_baseName);
+			mbd->m_baseName = (char*)serializer->getUniquePointer(name);
+			if (mbd->m_baseName)
+			{
+				serializer->serializeName(name);
+			}
+		}
+		mbd->m_numLinks = this->getNumLinks();
+		mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
+		if (mbd->m_links)
+		{
+			int sz = sizeof(btMultiBodyLinkData);
+			int numElem = mbd->m_numLinks;
+			btChunk* chunk = serializer->allocate(sz,numElem);
+			btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr;
+			for (int i=0;i<numElem;i++,memPtr++)
+			{
+
+				memPtr->m_jointType = getLink(i).m_jointType;
+				memPtr->m_dofCount = getLink(i).m_dofCount;
+				getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
+				memPtr->m_linkMass = getLink(i).m_mass;
+				memPtr->m_parentIndex = getLink(i).m_parent;
+				getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
+				getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
+				getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
+				btAssert(memPtr->m_dofCount<=3);
+				for (int dof = 0;dof<getLink(i).m_dofCount;dof++)
+				{
+					getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
+					getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
+					
+					memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
+					memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
+
+				}
+				int numPosVar = getLink(i).m_posVarCount;
+				for (int posvar = 0; posvar < numPosVar;posvar++)
+				{
+					memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
+				}
+				
+				
+				{
+					char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName);
+					memPtr->m_linkName = (char*)serializer->getUniquePointer(name);
+					if (memPtr->m_linkName)
+					{
+						serializer->serializeName(name);
+					}
+				}
+				{
+					char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName);
+					memPtr->m_jointName = (char*)serializer->getUniquePointer(name);
+					if (memPtr->m_jointName)
+					{
+						serializer->serializeName(name);
+					}
+				}
+				memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider);
+
+			}
+			serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]);
+		}
+		
+		return btMultiBodyDataName;
+}

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -30,6 +30,17 @@
 #include "LinearMath/btMatrix3x3.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+	#define btMultiBodyData	btMultiBodyDoubleData
+	#define btMultiBodyDataName	"btMultiBodyDoubleData"
+	#define btMultiBodyLinkData btMultiBodyLinkDoubleData
+	#define btMultiBodyLinkDataName	"btMultiBodyLinkDoubleData"
+#else
+	#define btMultiBodyData	btMultiBodyFloatData
+	#define btMultiBodyDataName	"btMultiBodyFloatData"
+	#define btMultiBodyLinkData btMultiBodyLinkFloatData
+	#define btMultiBodyLinkDataName	"btMultiBodyLinkFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
 
 #include "btMultiBodyLink.h"
 class btMultiBodyLinkCollider;
@@ -220,6 +231,9 @@ public:
 	btScalar * getJointVelMultiDof(int i);
 	btScalar * getJointPosMultiDof(int i);
 
+	const btScalar * getJointVelMultiDof(int i) const ;
+	const btScalar * getJointPosMultiDof(int i) const ;
+
     void setJointPos(int i, btScalar q);
     void setJointVel(int i, btScalar qdot);
 	void setJointPosMultiDof(int i, btScalar *q);
@@ -586,6 +600,20 @@ void addJointTorque(int i, btScalar Q);
 	}
 	void	forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
 	
+	virtual	int	calculateSerializeBufferSize()	const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer,  class btSerializer* serializer) const;
+
+	const char*				getBaseName() const
+	{
+		return m_baseName;
+	}
+	///memory of setBaseName needs to be manager by user
+	void	setBaseName(const char* name)
+	{
+		m_baseName = name;
+	}
 
 private:
     btMultiBody(const btMultiBody &);  // not implemented
@@ -608,11 +636,11 @@ private:
 
 	void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
 	
-
 	
 private:
 
 	btMultiBodyLinkCollider* m_baseCollider;//can be NULL
+	const char*				m_baseName;//memory needs to be manager by user!
 
     btVector3 m_basePos;       // position of COM of base (world frame)
     btQuaternion m_baseQuat;   // rotates world points into base frame
@@ -679,4 +707,96 @@ private:
 	bool m_internalNeedsJointFeedback;
 };
 
+struct btMultiBodyLinkDoubleData
+{
+	btQuaternionDoubleData	m_zeroRotParentToThis;
+	btVector3DoubleData		m_parentComToThisComOffset;
+	btVector3DoubleData		m_thisPivotToThisComOffset;
+	btVector3DoubleData		m_jointAxisTop[6];
+	btVector3DoubleData		m_jointAxisBottom[6];
+
+
+	char					*m_linkName;
+	char					*m_jointName;
+	btCollisionObjectDoubleData	*m_linkCollider;
+	
+	btVector3DoubleData		m_linkInertia;   // inertia of the base (in local frame; diagonal)
+	double					m_linkMass;
+	int						m_parentIndex;
+	int						m_jointType;
+	
+
+	
+
+	int						m_dofCount;
+	int						m_posVarCount;
+	double					m_jointPos[7];
+	double					m_jointVel[6];
+	double					m_jointTorque[6];
+	
+	
+	
+};
+
+struct btMultiBodyLinkFloatData
+{
+	btQuaternionFloatData	m_zeroRotParentToThis;
+	btVector3FloatData		m_parentComToThisComOffset;
+	btVector3FloatData		m_thisPivotToThisComOffset;
+	btVector3FloatData		m_jointAxisTop[6];
+	btVector3FloatData		m_jointAxisBottom[6];
+	
+
+	char				*m_linkName;
+	char				*m_jointName;
+	btCollisionObjectFloatData	*m_linkCollider;
+	
+	btVector3FloatData	m_linkInertia;   // inertia of the base (in local frame; diagonal)
+	int						m_dofCount;
+	float				m_linkMass;
+	int					m_parentIndex;
+	int					m_jointType;
+	
+
+		
+	float					m_jointPos[7];
+	float					m_jointVel[6];
+	float					m_jointTorque[6];
+	int						m_posVarCount;
+	
+
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btMultiBodyDoubleData
+{
+	char	*m_baseName;
+	btMultiBodyLinkDoubleData	*m_links;
+	btCollisionObjectDoubleData	*m_baseCollider;
+
+	btTransformDoubleData m_baseWorldTransform;
+	btVector3DoubleData m_baseInertia;   // inertia of the base (in local frame; diagonal)
+	
+	int		m_numLinks;
+	double	m_baseMass;
+
+	char m_padding[4];
+	
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btMultiBodyFloatData
+{
+	char	*m_baseName;
+	btMultiBodyLinkFloatData	*m_links;
+	btCollisionObjectFloatData	*m_baseCollider;
+	btTransformFloatData m_baseWorldTransform;
+	btVector3FloatData m_baseInertia;   // inertia of the base (in local frame; diagonal)
+	
+	float	m_baseMass;
+	int		m_numLinks;
+};
+
+
+
 #endif

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -21,7 +21,7 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "btMultiBodyConstraint.h"
 #include "LinearMath/btIDebugDraw.h"
-	
+#include "LinearMath/btSerializer.h"
 
 
 void	btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
@@ -1011,3 +1011,37 @@ void btMultiBodyDynamicsWorld::clearForces()
 }
 
 
+
+
+void	btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
+{
+
+	serializer->startSerialization();
+
+	serializeDynamicsWorldInfo( serializer);
+
+	serializeMultiBodies(serializer);
+
+	serializeRigidBodies(serializer);
+
+	serializeCollisionObjects(serializer);
+
+	serializer->finishSerialization();
+}
+
+void	btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
+{
+	int i;
+	//serialize all collision objects
+	for (i=0;i<m_multiBodies.size();i++)
+	{
+		btMultiBody* mb = m_multiBodies[i];
+		{
+			int len = mb->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(len,1);
+			const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
+		}
+	}
+
+}

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -40,6 +40,8 @@ protected:
 	virtual void	updateActivationState(btScalar timeStep);
 	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
 	
+	virtual void	serializeMultiBodies(btSerializer* serializer);
+
 public:
 
 	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
@@ -91,5 +93,7 @@ public:
 	virtual void clearMultiBodyForces();
 	virtual void applyGravity();
 	
+	virtual	void	serialize(btSerializer* serializer);
+
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -127,6 +127,9 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 
 	btTransform	m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
 
+	const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
+	const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
+
     // ctor: set some sensible defaults
 	btMultibodyLink()
 		: 	m_mass(1),
@@ -138,8 +141,9 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 			m_dofCount(0),
 			m_posVarCount(0),
 			m_jointType(btMultibodyLink::eInvalid),
-			m_jointFeedback(0)
-			
+			m_jointFeedback(0),
+			m_linkName(0),
+			m_jointName(0)
 	{
 		
 		m_inertiaLocal.setValue(1, 1, 1);