Implement saveState/restoreState for deformable objects

Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp

@@ -7,6 +7,8 @@
 #include "BulletDynamics/Featherstone/btMultiBody.h"
 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyData.h"
 
 struct btMultiBodyWorldImporterInternalData
 {
@@ -364,6 +366,7 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
 			{
 				syncContactManifolds((btPersistentManifoldDoubleData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data);
 			}
+
 		}
 		else
 		{
@@ -449,6 +452,57 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
 				syncContactManifolds((btPersistentManifoldFloatData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data);
 			}
 		}
+
+		// Deserialize and update the softbody. Note: This is outside of the check for float/double precision 
+		// since currently the softbody implementation is only configured for float serialization
+		for (int i = bulletFile2->m_softBodies.size() - 1; i >= 0; i--)
+		{
+			btSoftBodyData* sbd = (btSoftBodyData*)bulletFile2->m_softBodies[i];
+			int foundSb = -1;
+			int uid = sbd->m_collisionObjectData.m_uniqueId;
+			for (int i = 0; i < m_data->m_mbDynamicsWorld->getNumCollisionObjects(); i++)
+			{
+				if (uid == m_data->m_mbDynamicsWorld->getCollisionObjectArray()[i]->getBroadphaseHandle()->m_uniqueId)
+				{
+					foundSb = i;
+					break;
+				}
+			}
+			if (foundSb >= 0)
+			{
+				btSoftBody* sb = btSoftBody::upcast(m_data->m_mbDynamicsWorld->getCollisionObjectArray()[foundSb]);
+				if (sb)
+				{
+					// Deserialize data for each node and update the softbody's nodes with the saved info
+					for (int i = 0; i < sbd->m_numNodes; i++){
+						btVector3 pos, prev_pos, vel, force, normal; 
+						pos.deSerializeFloat(sbd->m_nodes[i].m_position);
+						prev_pos.deSerializeFloat(sbd->m_nodes[i].m_previousPosition);
+						vel.deSerializeFloat(sbd->m_nodes[i].m_velocity);
+						force.deSerializeFloat(sbd->m_nodes[i].m_accumulatedForce);
+						normal.deSerializeFloat(sbd->m_nodes[i].m_normal);
+						// TODO: see if the other serialized attributes for the nodes need to be reset. i.e.
+						// m_pad, m_material, m_inverseMass, m_area, m_attach
+						sb->m_nodes[i].m_x = pos; 
+						sb->m_nodes[i].m_q = prev_pos;
+						sb->m_nodes[i].m_v = vel; 
+						sb->m_nodes[i].m_f = force;
+						sb->m_nodes[i].m_n = normal; 
+					}		
+				}
+				else
+				{
+					printf("btMultiBodyWorldImporter::convertAllObjects error: cannot find btSoftBody with bodyUniqueId %d\n", uid);
+					result = false;
+				}
+			}
+			else
+			{
+				printf("Error in btMultiBodyWorldImporter::convertAllObjects: didn't find bodyUniqueId: %d\n", uid);
+				result = false;
+			}
+		}
+
 	}
 	else
 	{

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -743,3 +743,37 @@ int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int ma
 
 	return numSimulationSubSteps;
 }
+
+
+void btDeformableMultiBodyDynamicsWorld::serializeSoftBodies(btSerializer* serializer)
+{
+	int i;
+	//serialize all collision objects
+	for (i = 0; i < m_softBodies.size(); i++)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		int len = psb->calculateSerializeBufferSize();
+		btChunk* chunk = serializer->allocate(len, 1);
+		const char* structType = psb->serialize(chunk->m_oldPtr, serializer);
+		serializer->finalizeChunk(chunk, structType, BT_SOFTBODY_CODE, psb);
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
+{
+	serializer->startSerialization();
+
+	serializeDynamicsWorldInfo(serializer);
+
+	serializeSoftBodies(serializer);
+
+	serializeMultiBodies(serializer);
+
+	serializeRigidBodies(serializer);
+
+	serializeCollisionObjects(serializer);
+
+	serializeContactManifolds(serializer);
+
+	serializer->finishSerialization();
+}

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -175,6 +175,10 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
 
 	void performGeometricCollisions(btScalar timeStep);
 
+	void serializeSoftBodies(btSerializer* serializer); 
+
+	void serialize(btSerializer* serializer);
+
 	struct btDeformableSingleRayCallback : public btBroadphaseRayCallback
 	{
 		btVector3 m_rayFromWorld;

src/BulletSoftBody/btSoftBody.cpp

@@ -4339,6 +4339,8 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ
 
 	btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);
 
+	sbd->m_collisionObjectData.m_uniqueId = m_broadphaseHandle->m_uniqueId;
+
 	btHashMap<btHashPtr, int> m_nodeIndexMap;
 
 	sbd->m_numMaterials = m_materials.size();