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.

Extras/Serialize/BulletFileLoader/autogenerated/bullet.h

@@ -31,6 +31,8 @@ typedef struct bInvalidHandle {
     class ListBase;
     class btVector3FloatData;
     class btVector3DoubleData;
+    class btQuaternionFloatData;
+    class btQuaternionDoubleData;
     class btMatrix3x3FloatData;
     class btMatrix3x3DoubleData;
     class btTransformFloatData;
@@ -57,6 +59,7 @@ typedef struct bInvalidHandle {
     class btCompoundShapeChildData;
     class btCompoundShapeData;
     class btCylinderShapeData;
+    class btConeShapeData;
     class btCapsuleShapeData;
     class btTriangleInfoData;
     class btTriangleInfoMapData;
@@ -64,18 +67,34 @@ typedef struct bInvalidHandle {
     class btConvexHullShapeData;
     class btCollisionObjectDoubleData;
     class btCollisionObjectFloatData;
+    class btDynamicsWorldDoubleData;
+    class btDynamicsWorldFloatData;
     class btRigidBodyFloatData;
     class btRigidBodyDoubleData;
     class btConstraintInfo1;
+    class btTypedConstraintFloatData;
     class btTypedConstraintData;
+    class btTypedConstraintDoubleData;
     class btPoint2PointConstraintFloatData;
+    class btPoint2PointConstraintDoubleData2;
     class btPoint2PointConstraintDoubleData;
     class btHingeConstraintDoubleData;
     class btHingeConstraintFloatData;
+    class btHingeConstraintDoubleData2;
+    class btConeTwistConstraintDoubleData;
     class btConeTwistConstraintData;
     class btGeneric6DofConstraintData;
+    class btGeneric6DofConstraintDoubleData2;
     class btGeneric6DofSpringConstraintData;
+    class btGeneric6DofSpringConstraintDoubleData2;
+    class btGeneric6DofSpring2ConstraintData;
+    class btGeneric6DofSpring2ConstraintDoubleData2;
     class btSliderConstraintData;
+    class btSliderConstraintDoubleData;
+    class btGearConstraintFloatData;
+    class btGearConstraintDoubleData;
+    class btContactSolverInfoDoubleData;
+    class btContactSolverInfoFloatData;
     class SoftBodyMaterialData;
     class SoftBodyNodeData;
     class SoftBodyLinkData;
@@ -87,6 +106,10 @@ typedef struct bInvalidHandle {
     class SoftBodyClusterData;
     class btSoftBodyJointData;
     class btSoftBodyFloatData;
+    class btMultiBodyLinkDoubleData;
+    class btMultiBodyLinkFloatData;
+    class btMultiBodyDoubleData;
+    class btMultiBodyFloatData;
 // -------------------------------------------------- //
     class PointerArray
     {
@@ -132,6 +155,22 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btQuaternionFloatData
+    {
+    public:
+        float m_floats[4];
+    };
+
+
+// -------------------------------------------------- //
+    class btQuaternionDoubleData
+    {
+    public:
+        double m_floats[4];
+    };
+
+
 // -------------------------------------------------- //
     class btMatrix3x3FloatData
     {
@@ -421,6 +460,16 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btConeShapeData
+    {
+    public:
+        btConvexInternalShapeData m_convexInternalShapeData;
+        int m_upIndex;
+        char m_padding[4];
+    };
+
+
 // -------------------------------------------------- //
     class btCapsuleShapeData
     {
@@ -503,6 +552,7 @@ typedef struct bInvalidHandle {
         double m_contactProcessingThreshold;
         double m_deactivationTime;
         double m_friction;
+        double m_rollingFriction;
         double m_restitution;
         double m_hitFraction;
         double m_ccdSweptSphereRadius;
@@ -534,6 +584,7 @@ typedef struct bInvalidHandle {
         float m_contactProcessingThreshold;
         float m_deactivationTime;
         float m_friction;
+        float m_rollingFriction;
         float m_restitution;
         float m_hitFraction;
         float m_ccdSweptSphereRadius;
@@ -545,6 +596,25 @@ typedef struct bInvalidHandle {
         int m_activationState1;
         int m_internalType;
         int m_checkCollideWith;
+        char m_padding[4];
+    };
+
+
+// -------------------------------------------------- //
+    class btDynamicsWorldDoubleData
+    {
+    public:
+        btContactSolverInfoDoubleData m_solverInfo;
+        btVector3DoubleData m_gravity;
+    };
+
+
+// -------------------------------------------------- //
+    class btDynamicsWorldFloatData
+    {
+    public:
+        btContactSolverInfoFloatData m_solverInfo;
+        btVector3FloatData m_gravity;
     };
 
 
@@ -614,6 +684,26 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btTypedConstraintFloatData
+    {
+    public:
+        btRigidBodyFloatData *m_rbA;
+        btRigidBodyFloatData *m_rbB;
+        char *m_name;
+        int m_objectType;
+        int m_userConstraintType;
+        int m_userConstraintId;
+        int m_needsFeedback;
+        float m_appliedImpulse;
+        float m_dbgDrawSize;
+        int m_disableCollisionsBetweenLinkedBodies;
+        int m_overrideNumSolverIterations;
+        float m_breakingImpulseThreshold;
+        int m_isEnabled;
+    };
+
+
 // -------------------------------------------------- //
     class btTypedConstraintData
     {
@@ -634,6 +724,27 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btTypedConstraintDoubleData
+    {
+    public:
+        btRigidBodyDoubleData *m_rbA;
+        btRigidBodyDoubleData *m_rbB;
+        char *m_name;
+        int m_objectType;
+        int m_userConstraintType;
+        int m_userConstraintId;
+        int m_needsFeedback;
+        double m_appliedImpulse;
+        double m_dbgDrawSize;
+        int m_disableCollisionsBetweenLinkedBodies;
+        int m_overrideNumSolverIterations;
+        double m_breakingImpulseThreshold;
+        int m_isEnabled;
+        char padding[4];
+    };
+
+
 // -------------------------------------------------- //
     class btPoint2PointConstraintFloatData
     {
@@ -644,6 +755,16 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btPoint2PointConstraintDoubleData2
+    {
+    public:
+        btTypedConstraintDoubleData m_typeConstraintData;
+        btVector3DoubleData m_pivotInA;
+        btVector3DoubleData m_pivotInB;
+    };
+
+
 // -------------------------------------------------- //
     class btPoint2PointConstraintDoubleData
     {
@@ -694,6 +815,44 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btHingeConstraintDoubleData2
+    {
+    public:
+        btTypedConstraintDoubleData m_typeConstraintData;
+        btTransformDoubleData m_rbAFrame;
+        btTransformDoubleData m_rbBFrame;
+        int m_useReferenceFrameA;
+        int m_angularOnly;
+        int m_enableAngularMotor;
+        double m_motorTargetVelocity;
+        double m_maxMotorImpulse;
+        double m_lowerLimit;
+        double m_upperLimit;
+        double m_limitSoftness;
+        double m_biasFactor;
+        double m_relaxationFactor;
+        char m_padding1[4];
+    };
+
+
+// -------------------------------------------------- //
+    class btConeTwistConstraintDoubleData
+    {
+    public:
+        btTypedConstraintDoubleData m_typeConstraintData;
+        btTransformDoubleData m_rbAFrame;
+        btTransformDoubleData m_rbBFrame;
+        double m_swingSpan1;
+        double m_swingSpan2;
+        double m_twistSpan;
+        double m_limitSoftness;
+        double m_biasFactor;
+        double m_relaxationFactor;
+        double m_damping;
+    };
+
+
 // -------------------------------------------------- //
     class btConeTwistConstraintData
     {
@@ -728,6 +887,22 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btGeneric6DofConstraintDoubleData2
+    {
+    public:
+        btTypedConstraintDoubleData m_typeConstraintData;
+        btTransformDoubleData m_rbAFrame;
+        btTransformDoubleData m_rbBFrame;
+        btVector3DoubleData m_linearUpperLimit;
+        btVector3DoubleData m_linearLowerLimit;
+        btVector3DoubleData m_angularUpperLimit;
+        btVector3DoubleData m_angularLowerLimit;
+        int m_useLinearReferenceFrameA;
+        int m_useOffsetForConstraintFrame;
+    };
+
+
 // -------------------------------------------------- //
     class btGeneric6DofSpringConstraintData
     {
@@ -740,6 +915,114 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btGeneric6DofSpringConstraintDoubleData2
+    {
+    public:
+        btGeneric6DofConstraintDoubleData2 m_6dofData;
+        int m_springEnabled[6];
+        double m_equilibriumPoint[6];
+        double m_springStiffness[6];
+        double m_springDamping[6];
+    };
+
+
+// -------------------------------------------------- //
+    class btGeneric6DofSpring2ConstraintData
+    {
+    public:
+        btTypedConstraintData m_typeConstraintData;
+        btTransformFloatData m_rbAFrame;
+        btTransformFloatData m_rbBFrame;
+        btVector3FloatData m_linearUpperLimit;
+        btVector3FloatData m_linearLowerLimit;
+        btVector3FloatData m_linearBounce;
+        btVector3FloatData m_linearStopERP;
+        btVector3FloatData m_linearStopCFM;
+        btVector3FloatData m_linearMotorERP;
+        btVector3FloatData m_linearMotorCFM;
+        btVector3FloatData m_linearTargetVelocity;
+        btVector3FloatData m_linearMaxMotorForce;
+        btVector3FloatData m_linearServoTarget;
+        btVector3FloatData m_linearSpringStiffness;
+        btVector3FloatData m_linearSpringDamping;
+        btVector3FloatData m_linearEquilibriumPoint;
+        char m_linearEnableMotor[4];
+        char m_linearServoMotor[4];
+        char m_linearEnableSpring[4];
+        char m_linearSpringStiffnessLimited[4];
+        char m_linearSpringDampingLimited[4];
+        char m_padding1[4];
+        btVector3FloatData m_angularUpperLimit;
+        btVector3FloatData m_angularLowerLimit;
+        btVector3FloatData m_angularBounce;
+        btVector3FloatData m_angularStopERP;
+        btVector3FloatData m_angularStopCFM;
+        btVector3FloatData m_angularMotorERP;
+        btVector3FloatData m_angularMotorCFM;
+        btVector3FloatData m_angularTargetVelocity;
+        btVector3FloatData m_angularMaxMotorForce;
+        btVector3FloatData m_angularServoTarget;
+        btVector3FloatData m_angularSpringStiffness;
+        btVector3FloatData m_angularSpringDamping;
+        btVector3FloatData m_angularEquilibriumPoint;
+        char m_angularEnableMotor[4];
+        char m_angularServoMotor[4];
+        char m_angularEnableSpring[4];
+        char m_angularSpringStiffnessLimited[4];
+        char m_angularSpringDampingLimited[4];
+        int m_rotateOrder;
+    };
+
+
+// -------------------------------------------------- //
+    class btGeneric6DofSpring2ConstraintDoubleData2
+    {
+    public:
+        btTypedConstraintDoubleData m_typeConstraintData;
+        btTransformDoubleData m_rbAFrame;
+        btTransformDoubleData m_rbBFrame;
+        btVector3DoubleData m_linearUpperLimit;
+        btVector3DoubleData m_linearLowerLimit;
+        btVector3DoubleData m_linearBounce;
+        btVector3DoubleData m_linearStopERP;
+        btVector3DoubleData m_linearStopCFM;
+        btVector3DoubleData m_linearMotorERP;
+        btVector3DoubleData m_linearMotorCFM;
+        btVector3DoubleData m_linearTargetVelocity;
+        btVector3DoubleData m_linearMaxMotorForce;
+        btVector3DoubleData m_linearServoTarget;
+        btVector3DoubleData m_linearSpringStiffness;
+        btVector3DoubleData m_linearSpringDamping;
+        btVector3DoubleData m_linearEquilibriumPoint;
+        char m_linearEnableMotor[4];
+        char m_linearServoMotor[4];
+        char m_linearEnableSpring[4];
+        char m_linearSpringStiffnessLimited[4];
+        char m_linearSpringDampingLimited[4];
+        char m_padding1[4];
+        btVector3DoubleData m_angularUpperLimit;
+        btVector3DoubleData m_angularLowerLimit;
+        btVector3DoubleData m_angularBounce;
+        btVector3DoubleData m_angularStopERP;
+        btVector3DoubleData m_angularStopCFM;
+        btVector3DoubleData m_angularMotorERP;
+        btVector3DoubleData m_angularMotorCFM;
+        btVector3DoubleData m_angularTargetVelocity;
+        btVector3DoubleData m_angularMaxMotorForce;
+        btVector3DoubleData m_angularServoTarget;
+        btVector3DoubleData m_angularSpringStiffness;
+        btVector3DoubleData m_angularSpringDamping;
+        btVector3DoubleData m_angularEquilibriumPoint;
+        char m_angularEnableMotor[4];
+        char m_angularServoMotor[4];
+        char m_angularEnableSpring[4];
+        char m_angularSpringStiffnessLimited[4];
+        char m_angularSpringDampingLimited[4];
+        int m_rotateOrder;
+    };
+
+
 // -------------------------------------------------- //
     class btSliderConstraintData
     {
@@ -756,6 +1039,103 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btSliderConstraintDoubleData
+    {
+    public:
+        btTypedConstraintDoubleData m_typeConstraintData;
+        btTransformDoubleData m_rbAFrame;
+        btTransformDoubleData m_rbBFrame;
+        double m_linearUpperLimit;
+        double m_linearLowerLimit;
+        double m_angularUpperLimit;
+        double m_angularLowerLimit;
+        int m_useLinearReferenceFrameA;
+        int m_useOffsetForConstraintFrame;
+    };
+
+
+// -------------------------------------------------- //
+    class btGearConstraintFloatData
+    {
+    public:
+        btTypedConstraintFloatData m_typeConstraintData;
+        btVector3FloatData m_axisInA;
+        btVector3FloatData m_axisInB;
+        float m_ratio;
+        char m_padding[4];
+    };
+
+
+// -------------------------------------------------- //
+    class btGearConstraintDoubleData
+    {
+    public:
+        btTypedConstraintDoubleData m_typeConstraintData;
+        btVector3DoubleData m_axisInA;
+        btVector3DoubleData m_axisInB;
+        double m_ratio;
+    };
+
+
+// -------------------------------------------------- //
+    class btContactSolverInfoDoubleData
+    {
+    public:
+        double m_tau;
+        double m_damping;
+        double m_friction;
+        double m_timeStep;
+        double m_restitution;
+        double m_maxErrorReduction;
+        double m_sor;
+        double m_erp;
+        double m_erp2;
+        double m_globalCfm;
+        double m_splitImpulsePenetrationThreshold;
+        double m_splitImpulseTurnErp;
+        double m_linearSlop;
+        double m_warmstartingFactor;
+        double m_maxGyroscopicForce;
+        double m_singleAxisRollingFrictionThreshold;
+        int m_numIterations;
+        int m_solverMode;
+        int m_restingContactRestitutionThreshold;
+        int m_minimumSolverBatchSize;
+        int m_splitImpulse;
+        char m_padding[4];
+    };
+
+
+// -------------------------------------------------- //
+    class btContactSolverInfoFloatData
+    {
+    public:
+        float m_tau;
+        float m_damping;
+        float m_friction;
+        float m_timeStep;
+        float m_restitution;
+        float m_maxErrorReduction;
+        float m_sor;
+        float m_erp;
+        float m_erp2;
+        float m_globalCfm;
+        float m_splitImpulsePenetrationThreshold;
+        float m_splitImpulseTurnErp;
+        float m_linearSlop;
+        float m_warmstartingFactor;
+        float m_maxGyroscopicForce;
+        float m_singleAxisRollingFrictionThreshold;
+        int m_numIterations;
+        int m_solverMode;
+        int m_restingContactRestitutionThreshold;
+        int m_minimumSolverBatchSize;
+        int m_splitImpulse;
+        char m_padding[4];
+    };
+
+
 // -------------------------------------------------- //
     class SoftBodyMaterialData
     {
@@ -964,5 +1344,82 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btMultiBodyLinkDoubleData
+    {
+    public:
+        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;
+        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];
+    };
+
+
+// -------------------------------------------------- //
+    class btMultiBodyLinkFloatData
+    {
+    public:
+        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;
+        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;
+    };
+
+
+// -------------------------------------------------- //
+    class btMultiBodyDoubleData
+    {
+    public:
+        char *m_baseName;
+        btMultiBodyLinkDoubleData *m_links;
+        btCollisionObjectDoubleData *m_baseCollider;
+        btTransformDoubleData m_baseWorldTransform;
+        btVector3DoubleData m_baseInertia;
+        int m_numLinks;
+        double m_baseMass;
+        char m_padding[4];
+    };
+
+
+// -------------------------------------------------- //
+    class btMultiBodyFloatData
+    {
+    public:
+        char *m_baseName;
+        btMultiBodyLinkFloatData *m_links;
+        btCollisionObjectFloatData *m_baseCollider;
+        btTransformFloatData m_baseWorldTransform;
+        btVector3FloatData m_baseInertia;
+        float m_baseMass;
+        int m_numLinks;
+    };
+
+
 }
 #endif//__BULLET_H__

Extras/Serialize/HeaderGenerator/apiGen.cpp

@@ -220,7 +220,7 @@ int main(int argc,char** argv)
 	fprintf(dump, "%s\n", data);
 
 	
-	char* filename = "../../../Demos/SerializeDemo/testFile.bullet";
+	char* filename = "../../../../data/slope.bullet";
 	
 	if (argc==2)
 		filename = argv[1];

Extras/Serialize/makesdna/makesdna.cpp

@@ -120,6 +120,7 @@ typedef unsigned long uintptr_t;
 // include files for automatic dependancies
 #include "DNA_rigidbody.h"
 #include "LinearMath/btVector3.h"
+#include "LinearMath/btQuaternion.h"
 #include "LinearMath/btMatrix3x3.h"
 #include "LinearMath/btTransform.h"
 #include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h"
@@ -152,6 +153,7 @@ typedef unsigned long uintptr_t;
 
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "BulletSoftBody/btSoftBodyData.h"
+#include "BulletDynamics/Featherstone/btMultiBody.h"
 
 #ifdef HAVE_CONFIG_H
 #include <config.h>
@@ -167,6 +169,7 @@ char *includefiles[] = {
 	// of makesdna.c (this file) as well
 	"../makesdna/DNA_rigidbody.h",
 	"../../../src/LinearMath/btVector3.h",
+	"../../../src/LinearMath/btQuaternion.h",
 	"../../../src/LinearMath/btMatrix3x3.h",
 	"../../../src/LinearMath/btTransform.h",
 	"../../../src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h",
@@ -197,7 +200,8 @@ char *includefiles[] = {
 	"../../../src/BulletDynamics/ConstraintSolver/btSliderConstraint.h",
 	"../../../src/BulletDynamics/ConstraintSolver/btGearConstraint.h",
 	"../../../src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h",
-	"../../../src/BulletSoftBody/btSoftBodyData.h",	
+	"../../../src/BulletSoftBody/btSoftBodyData.h",
+	"../../../src/BulletDynamics/Featherstone/btMultiBody.h",
 	// empty string to indicate end of includefiles
 	""
 };

VERSION

@@ -1 +1 @@
-2.83
+2.84

examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp

@@ -9,6 +9,8 @@
 #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
 #include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../../Utils/b3ResourcePath.h"
+
 #ifdef ENABLE_ROS_URDF
 #include "ROSURDFImporter.h"
 #endif
@@ -35,6 +37,7 @@ class ImportUrdfSetup : public CommonMultiBodyBase
 
     struct ImportUrdfInternalData* m_data;
 	bool m_useMultiBody;
+	btAlignedObjectArray<std::string* > m_nameMemory;
 
 public:
     ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
@@ -143,6 +146,11 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
 
 ImportUrdfSetup::~ImportUrdfSetup()
 {
+	for (int i=0;i<m_nameMemory.size();i++)
+	{
+		delete m_nameMemory[i];
+	}
+	m_nameMemory.clear();
     delete m_data;
 }
 
@@ -194,7 +202,7 @@ void ImportUrdfSetup::initPhysics()
 
 	m_dynamicsWorld->setGravity(gravity);
 
-
+	
 
     //now print the tree using the new interface
     URDFImporterInterface* bla=0;
@@ -215,6 +223,9 @@ void ImportUrdfSetup::initPhysics()
 #endif//USE_ROS_URDF
 	URDFImporterInterface& u2b = *bla;
 	bool loadOk =  u2b.loadURDF(m_fileName);
+	
+	//test to serialize a multibody to disk or shared memory, with base, link and joint names
+	btSerializer* s = new btDefaultSerializer;
 
 	if (loadOk)
 	{
@@ -240,25 +251,40 @@ void ImportUrdfSetup::initPhysics()
 			ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
 			mb = creation.getBulletMultiBody();
 
-			if (m_useMultiBody)
+			if (m_useMultiBody && mb )
 			{
+				std::string*   name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
+				m_nameMemory.push_back(name);
+				s->registerNameForPointer(name->c_str(),name->c_str());
 
+				mb->setBaseName(name->c_str());
 				//create motors for each btMultiBody joint
 				int numLinks = mb->getNumLinks();
 				for (int i=0;i<numLinks;i++)
 				{
 					int mbLinkIndex = i;
+					int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
+
+					std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
+					std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
+					s->registerNameForPointer(jointName->c_str(),jointName->c_str());
+					s->registerNameForPointer(linkName->c_str(),linkName->c_str());
+
+					m_nameMemory.push_back(jointName);
+					m_nameMemory.push_back(linkName);
+
+					mb->getLink(i).m_linkName = linkName->c_str();
+					mb->getLink(i).m_jointName = jointName->c_str();
+							
 					if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
 					    ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
 					)
 					{
 						if (m_data->m_numMotors<MAX_NUM_MOTORS)
 						{
-							int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
+							
-
-							std::string jointName = u2b.getJointName(urdfLinkIndex);
 							char motorName[1024];
-							sprintf(motorName,"%s q'", jointName.c_str());
+							sprintf(motorName,"%s q'", jointName->c_str());
 							btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
 							*motorVel = 0.f;
 							SliderParams slider(motorName,motorVel);
@@ -349,6 +375,17 @@ void ImportUrdfSetup::initPhysics()
 		///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
 		m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
 	}
+
+	
+	m_dynamicsWorld->serialize(s);
+	b3ResourcePath p;
+	char resourcePath[1024];
+	if (p.findResourcePath("r2d2_multibody.bullet",resourcePath,1024))
+	{
+		FILE* f = fopen(resourcePath,"wb");
+		fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
+		fclose(f);
+	}
 }
 
 void ImportUrdfSetup::stepSimulation(float deltaTime)

examples/MultiBody/TestJointTorqueSetup.cpp

@@ -5,6 +5,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
 							
 #include "../CommonInterfaces/CommonMultiBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
 
 static btScalar radius(0.2);
 
@@ -366,6 +367,16 @@ void TestJointTorqueSetup::initPhysics()
         }
     }
 
+	btSerializer* s = new btDefaultSerializer;
+	m_dynamicsWorld->serialize(s);
+	b3ResourcePath p;
+	char resourcePath[1024];
+	if (p.findResourcePath("multibody.bullet",resourcePath,1024))
+	{
+		FILE* f = fopen(resourcePath,"wb");
+		fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
+		fclose(f);
+	}
 }
 
 void TestJointTorqueSetup::stepSimulation(float deltaTime)

examples/RollingFrictionDemo/RollingFrictionDemo.cpp

@@ -35,7 +35,7 @@ subject to the following restrictions:
 
 
 #include "../CommonInterfaces/CommonRigidBodyBase.h"
-
+#include "../Utils/b3ResourcePath.h"
 
 ///The RollingFrictionDemo shows the use of rolling friction.
 ///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same.
@@ -231,12 +231,17 @@ void	RollingFrictionDemo::initPhysics()
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 	btSerializer* s = new btDefaultSerializer;
 	m_dynamicsWorld->serialize(s);
-	FILE* f = fopen("slope.bullet","wb");
+	b3ResourcePath p;
-	fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
+	char resourcePath[1024];
-	fclose(f);
+	if (p.findResourcePath("slope.bullet",resourcePath,1024))
+	{
+		FILE* f = fopen(resourcePath,"wb");
+		fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
+		fclose(f);
+	}
 }
 
-	
+
 
 void	RollingFrictionDemo::exitPhysics()
 {

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -1533,7 +1533,7 @@ void	btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
 	for (i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
-		if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
+		if ((colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK))
 		{
 			colObj->serializeSingleObject(serializer);
 		}

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);

src/LinearMath/btQuaternion.h

@@ -22,6 +22,13 @@ subject to the following restrictions:
 #include "btQuadWord.h"
 
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btQuaternionData btQuaternionDoubleData
+#define btQuaternionDataName "btQuaternionDoubleData"
+#else
+#define btQuaternionData btQuaternionFloatData
+#define btQuaternionDataName "btQuaternionFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
 
 
 
@@ -560,7 +567,18 @@ public:
 
 	SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
 
-	
+	SIMD_FORCE_INLINE	void	serialize(struct	btQuaternionData& dataOut) const;
+
+	SIMD_FORCE_INLINE	void	deSerialize(const struct	btQuaternionData& dataIn);
+
+	SIMD_FORCE_INLINE	void	serializeFloat(struct	btQuaternionFloatData& dataOut) const;
+
+	SIMD_FORCE_INLINE	void	deSerializeFloat(const struct	btQuaternionFloatData& dataIn);
+
+	SIMD_FORCE_INLINE	void	serializeDouble(struct	btQuaternionDoubleData& dataOut) const;
+
+	SIMD_FORCE_INLINE	void	deSerializeDouble(const struct	btQuaternionDoubleData& dataIn);
+
 };
 
 
@@ -903,6 +921,62 @@ shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
 	return shortestArcQuat(v0,v1);
 }
 
+
+
+
+struct	btQuaternionFloatData
+{
+	float	m_floats[4];
+};
+
+struct	btQuaternionDoubleData
+{
+	double	m_floats[4];
+
+};
+
+SIMD_FORCE_INLINE	void	btQuaternion::serializeFloat(struct	btQuaternionFloatData& dataOut) const
+{
+	///could also do a memcpy, check if it is worth it
+	for (int i=0;i<4;i++)
+		dataOut.m_floats[i] = float(m_floats[i]);
+}
+
+SIMD_FORCE_INLINE void	btQuaternion::deSerializeFloat(const struct	btQuaternionFloatData& dataIn)
+{
+	for (int i=0;i<4;i++)
+		m_floats[i] = btScalar(dataIn.m_floats[i]);
+}
+
+
+SIMD_FORCE_INLINE	void	btQuaternion::serializeDouble(struct	btQuaternionDoubleData& dataOut) const
+{
+	///could also do a memcpy, check if it is worth it
+	for (int i=0;i<4;i++)
+		dataOut.m_floats[i] = double(m_floats[i]);
+}
+
+SIMD_FORCE_INLINE void	btQuaternion::deSerializeDouble(const struct	btQuaternionDoubleData& dataIn)
+{
+	for (int i=0;i<4;i++)
+		m_floats[i] = btScalar(dataIn.m_floats[i]);
+}
+
+
+SIMD_FORCE_INLINE	void	btQuaternion::serialize(struct	btQuaternionData& dataOut) const
+{
+	///could also do a memcpy, check if it is worth it
+	for (int i=0;i<4;i++)
+		dataOut.m_floats[i] = m_floats[i];
+}
+
+SIMD_FORCE_INLINE void	btQuaternion::deSerialize(const struct	btQuaternionData& dataIn)
+{
+	for (int i=0;i<4;i++)
+		m_floats[i] = dataIn.m_floats[i];
+}
+
+
 #endif //BT_SIMD__QUATERNION_H_
 
 

src/LinearMath/btScalar.h

@@ -28,7 +28,7 @@ subject to the following restrictions:
 #include <float.h>
 
 /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
-#define BT_BULLET_VERSION 283
+#define BT_BULLET_VERSION 284
 
 inline int	btGetVersion()
 {

src/LinearMath/btSerializer.h

@@ -113,6 +113,8 @@ public:
 #	define BT_MAKE_ID(a,b,c,d) ( (int)(d)<<24 | (int)(c)<<16 | (b)<<8 | (a) )
 #endif
 
+
+#define BT_MULTIBODY_CODE       BT_MAKE_ID('M','B','D','Y')
 #define BT_SOFTBODY_CODE		BT_MAKE_ID('S','B','D','Y')
 #define BT_COLLISIONOBJECT_CODE BT_MAKE_ID('C','O','B','J')
 #define BT_RIGIDBODY_CODE		BT_MAKE_ID('R','B','D','Y')
@@ -465,7 +467,7 @@ public:
 
 			buffer[9] = '2';
 			buffer[10] = '8';
-			buffer[11] = '3';
+			buffer[11] = '4';
 
 		}