made some progress in saving and restoring the state during the simulation, with identical results.

Option to de/serialize btPersistentContactManifolds and fix lossy conversion during btMultiBody de/serialization of base world transform
(serialize the quaternion, not the converted 3x3 matrix)
There are still several caches not taken into account, and btMultiBody links/constraints are not deserialized yet etc.
See examples\pybullet\examples\saveRestoreState.py for an example.

Extras/Serialize/BulletFileLoader/autogenerated/bullet.h

@@ -63,6 +63,8 @@ typedef struct bInvalidHandle {
     class btCapsuleShapeData;
     class btTriangleInfoData;
     class btTriangleInfoMapData;
+    class btPersistentManifoldDoubleData;
+    class btPersistentManifoldFloatData;
     class btGImpactMeshShapeData;
     class btConvexHullShapeData;
     class btCollisionObjectDoubleData;
@@ -514,6 +516,96 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btPersistentManifoldDoubleData
+    {
+    public:
+        btVector3DoubleData m_pointCacheLocalPointA[4];
+        btVector3DoubleData m_pointCacheLocalPointB[4];
+        btVector3DoubleData m_pointCachePositionWorldOnA[4];
+        btVector3DoubleData m_pointCachePositionWorldOnB[4];
+        btVector3DoubleData m_pointCacheNormalWorldOnB[4];
+        btVector3DoubleData m_pointCacheLateralFrictionDir1[4];
+        btVector3DoubleData m_pointCacheLateralFrictionDir2[4];
+        double m_pointCacheDistance[4];
+        double m_pointCacheAppliedImpulse[4];
+        double m_pointCacheCombinedFriction[4];
+        double m_pointCacheCombinedRollingFriction[4];
+        double m_pointCacheCombinedSpinningFriction[4];
+        double m_pointCacheCombinedRestitution[4];
+        int m_pointCachePartId0[4];
+        int m_pointCachePartId1[4];
+        int m_pointCacheIndex0[4];
+        int m_pointCacheIndex1[4];
+        int m_pointCacheContactPointFlags[4];
+        double m_pointCacheAppliedImpulseLateral1[4];
+        double m_pointCacheAppliedImpulseLateral2[4];
+        double m_pointCacheContactMotion1[4];
+        double m_pointCacheContactMotion2[4];
+        double m_pointCacheContactCFM[4];
+        double m_pointCacheCombinedContactStiffness1[4];
+        double m_pointCacheContactERP[4];
+        double m_pointCacheCombinedContactDamping1[4];
+        double m_pointCacheFrictionCFM[4];
+        int m_pointCacheLifeTime[4];
+        int m_numCachedPoints;
+        int m_companionIdA;
+        int m_companionIdB;
+        int m_index1a;
+        int m_objectType;
+        double m_contactBreakingThreshold;
+        double m_contactProcessingThreshold;
+        int m_padding;
+        void *m_body0;
+        void *m_body1;
+    };
+
+
+// -------------------------------------------------- //
+    class btPersistentManifoldFloatData
+    {
+    public:
+        btVector3FloatData m_pointCacheLocalPointA[4];
+        btVector3FloatData m_pointCacheLocalPointB[4];
+        btVector3FloatData m_pointCachePositionWorldOnA[4];
+        btVector3FloatData m_pointCachePositionWorldOnB[4];
+        btVector3FloatData m_pointCacheNormalWorldOnB[4];
+        btVector3FloatData m_pointCacheLateralFrictionDir1;
+        btVector3FloatData m_pointCacheLateralFrictionDir2;
+        float m_pointCacheDistance[4];
+        float m_pointCacheAppliedImpulse[4];
+        float m_pointCacheCombinedFriction[4];
+        float m_pointCacheCombinedRollingFriction[4];
+        float m_pointCacheCombinedSpinningFriction[4];
+        float m_pointCacheCombinedRestitution[4];
+        int m_pointCachePartId0[4];
+        int m_pointCachePartId1[4];
+        int m_pointCacheIndex0[4];
+        int m_pointCacheIndex1[4];
+        int m_pointCacheContactPointFlags[4];
+        float m_pointCacheAppliedImpulseLateral1[4];
+        float m_pointCacheAppliedImpulseLateral2[4];
+        float m_pointCacheContactMotion1[4];
+        float m_pointCacheContactMotion2[4];
+        float m_pointCacheContactCFM[4];
+        float m_pointCacheCombinedContactStiffness1[4];
+        float m_pointCacheContactERP[4];
+        float m_pointCacheCombinedContactDamping1[4];
+        float m_pointCacheFrictionCFM[4];
+        int m_pointCacheLifeTime[4];
+        int m_numCachedPoints;
+        int m_companionIdA;
+        int m_companionIdB;
+        int m_index1a;
+        int m_objectType;
+        float m_contactBreakingThreshold;
+        float m_contactProcessingThreshold;
+        int m_padding;
+        void *m_body0;
+        void *m_body1;
+    };
+
+
 // -------------------------------------------------- //
     class btGImpactMeshShapeData
     {
@@ -568,7 +660,9 @@ typedef struct bInvalidHandle {
         int m_activationState1;
         int m_internalType;
         int m_checkCollideWith;
-        char m_padding[4];
+        int m_collisionFilterGroup;
+        int m_collisionFilterMask;
+        int m_uniqueId;
     };
 
 
@@ -602,7 +696,9 @@ typedef struct bInvalidHandle {
         int m_activationState1;
         int m_internalType;
         int m_checkCollideWith;
-        char m_padding[4];
+        int m_collisionFilterGroup;
+        int m_collisionFilterMask;
+        int m_uniqueId;
     };
 
 
@@ -1416,15 +1512,17 @@ typedef struct bInvalidHandle {
     class btMultiBodyDoubleData
     {
     public:
-        btTransformDoubleData m_baseWorldTransform;
+        btVector3DoubleData m_baseWorldPosition;
+        btQuaternionDoubleData m_baseWorldOrientation;
+        btVector3DoubleData m_baseLinearVelocity;
+        btVector3DoubleData m_baseAngularVelocity;
         btVector3DoubleData m_baseInertia;
         double m_baseMass;
+        int m_numLinks;
+        char m_padding[4];
         char *m_baseName;
         btMultiBodyLinkDoubleData *m_links;
         btCollisionObjectDoubleData *m_baseCollider;
-        char *m_paddingPtr;
-        int m_numLinks;
-        char m_padding[4];
     };
 
 
@@ -1432,13 +1530,16 @@ typedef struct bInvalidHandle {
     class btMultiBodyFloatData
     {
     public:
-        char *m_baseName;
+        btVector3FloatData m_baseWorldPosition;
-        btMultiBodyLinkFloatData *m_links;
+        btQuaternionFloatData m_baseWorldOrientation;
-        btCollisionObjectFloatData *m_baseCollider;
+        btVector3FloatData m_baseLinearVelocity;
-        btTransformFloatData m_baseWorldTransform;
+        btVector3FloatData m_baseAngularVelocity;
         btVector3FloatData m_baseInertia;
         float m_baseMass;
         int m_numLinks;
+        char *m_baseName;
+        btMultiBodyLinkFloatData *m_links;
+        btCollisionObjectFloatData *m_baseCollider;
     };
 
 

Extras/Serialize/BulletFileLoader/btBulletFile.cpp

@@ -176,6 +176,10 @@ void btBulletFile::parseData()
 					//	listID->push_back((bStructHandle*)id);
 				}
 
+				if (dataChunk.code == BT_CONTACTMANIFOLD_CODE)
+				{
+					m_contactManifolds.push_back((bStructHandle*)id);
+				}
 				if (dataChunk.code == BT_MULTIBODY_CODE)
 				{
 					m_multiBodies.push_back((bStructHandle*)id);

Extras/Serialize/BulletFileLoader/btBulletFile.h

@@ -59,6 +59,8 @@ namespace bParse {
 
 		btAlignedObjectArray<bStructHandle*>	m_dynamicsWorldInfo;
 
+		btAlignedObjectArray<bStructHandle*>	m_contactManifolds;
+
 		btAlignedObjectArray<char*>				m_dataBlocks;
 		btBulletFile();
 

Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp

@@ -52,10 +52,11 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 		result = true;
 
 		//convert all multibodies
-		for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
+		if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
 		{
 
-			if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
+			//for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
+			for (int i = bulletFile2->m_multiBodies.size()-1; i >=0; i--)
 			{
 				btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
 				bool isFixedBase = mbd->m_baseMass == 0;
@@ -63,9 +64,20 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 				btVector3 baseInertia;
 				baseInertia.deSerializeDouble(mbd->m_baseInertia);
 
-				btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
 				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
-				mb->setBaseWorldTransform(tr);
+				btVector3 baseWorldPos;
+				baseWorldPos.deSerializeDouble(mbd->m_baseWorldPosition);
+				mb->setBasePos(baseWorldPos);
+				btQuaternion baseWorldRot;
+				baseWorldRot.deSerializeDouble(mbd->m_baseWorldOrientation);
+				mb->setWorldToBaseRot(baseWorldRot.inverse());
+				btVector3 baseLinVal;
+				baseLinVal.deSerializeDouble(mbd->m_baseLinearVelocity);
+				btVector3 baseAngVel;
+				baseAngVel.deSerializeDouble(mbd->m_baseAngularVelocity);
+				mb->setBaseVel(baseLinVal);
+				mb->setBaseOmega(baseAngVel);
+
 				for (int i = 0; i < mbd->m_numLinks; i++)
 				{
 				}
@@ -74,6 +86,74 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 				mb->forwardKinematics(scratchQ, scratchM);
 				mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
 			}
+
+			//todo: check why body1 pointer is not properly deserialized
+			for (int i = 0; i < bulletFile2->m_contactManifolds.size(); i++)
+			{
+				btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[i];
+				void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
+				if (ptr)
+				{
+					manifoldData->m_body1 = ptr;
+					btCollisionObjectDoubleData* cdd = (btCollisionObjectDoubleData*)ptr;
+				}
+			}
+
+			btDispatcherInfo& dispatchInfo = m_data->m_mbDynamicsWorld->getDispatchInfo();
+
+			{
+				m_data->m_mbDynamicsWorld->updateAabbs();
+				m_data->m_mbDynamicsWorld->computeOverlappingPairs();
+				btDispatcher* dispatcher = m_data->m_mbDynamicsWorld->getDispatcher();
+				
+				if (dispatcher)
+				{
+					btOverlappingPairCache* pairCache = m_data->m_mbDynamicsWorld->getBroadphase()->getOverlappingPairCache();
+					if (dispatcher)
+					{
+						dispatcher->dispatchAllCollisionPairs(pairCache, dispatchInfo, dispatcher);
+					}
+					int numExistingManifolds = m_data->m_mbDynamicsWorld->getDispatcher()->getNumManifolds();
+					btManifoldArray manifoldArray;
+					for (int i = 0; i < pairCache->getNumOverlappingPairs(); i++)
+					{
+						btBroadphasePair& pair = pairCache->getOverlappingPairArray()[i];
+						if (pair.m_algorithm)
+						{
+							pair.m_algorithm->getAllContactManifolds(manifoldArray);
+							//for each existing manifold, search a matching manifoldData and reconstruct
+							for (int m = 0; m < manifoldArray.size(); m++)
+							{
+								btPersistentManifold* existingManifold = manifoldArray[m];
+								int uid0 = existingManifold->getBody0()->getBroadphaseHandle()->m_uniqueId;
+								int uid1 = existingManifold->getBody1()->getBroadphaseHandle()->m_uniqueId;
+								int matchingManifoldIndex = -1;
+								for (int q= 0; q < bulletFile2->m_contactManifolds.size(); q++)
+								{
+									btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[q];
+									btCollisionObjectDoubleData* cdd0 = (btCollisionObjectDoubleData*)manifoldData->m_body0;
+									btCollisionObjectDoubleData* cdd1 = (btCollisionObjectDoubleData*)manifoldData->m_body1;
+									if (uid0 == cdd0->m_uniqueId && uid1 == cdd1->m_uniqueId)
+									{
+										matchingManifoldIndex = q;
+									}
+								}
+								if (matchingManifoldIndex >= 0)
+								{
+									btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[matchingManifoldIndex];
+									existingManifold->deSerializeDouble(manifoldData);
+								}
+								else
+								{
+									printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1);
+								}
+
+								manifoldArray.clear();
+							}
+						}
+					}
+				}
+			}
 		}
 	}
 	else
@@ -94,8 +174,14 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 				baseInertia.deSerializeDouble(mbd->m_baseInertia);
 				btMultiBody* mb = new btMultiBody(mbd->m_numLinks, mbd->m_baseMass, baseInertia, isFixedBase, canSleep);
 				mb->setHasSelfCollision(false);
-				btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
+				
-				mb->setBaseWorldTransform(tr);
+				btVector3 baseWorldPos;
+				baseWorldPos.deSerializeDouble(mbd->m_baseWorldPosition);
+
+				btQuaternion baseWorldOrn;
+				baseWorldOrn.deSerializeDouble(mbd->m_baseWorldOrientation);
+				mb->setBasePos(baseWorldPos);
+				mb->setWorldToBaseRot(baseWorldOrn.inverse());
 
 				m_data->m_mbMap.insert(mbd, mb);
 				for (int i = 0; i < mbd->m_numLinks; i++)

Extras/Serialize/makesdna/makesdna.cpp

@@ -132,6 +132,7 @@ typedef unsigned __int64 uint64_t;
 #include "BulletCollision/CollisionShapes/btConeShape.h"
 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
 #include "BulletCollision/CollisionShapes/btTriangleInfoMap.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 #include "BulletCollision/Gimpact/btGImpactShape.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
@@ -181,6 +182,7 @@ char *includefiles[] = {
 	"../../../src/BulletCollision/CollisionShapes/btConeShape.h",
 	"../../../src/BulletCollision/CollisionShapes/btCapsuleShape.h",
 	"../../../src/BulletCollision/CollisionShapes/btTriangleInfoMap.h",
+	"../../../src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h",
 	"../../../src/BulletCollision/Gimpact/btGImpactShape.h",
 	"../../../src/BulletCollision/CollisionShapes/btConvexHullShape.h",
 	"../../../src/BulletCollision/CollisionDispatch/btCollisionObject.h",

data/planeMesh.urdf

@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="plane">
+  <link name="planeLink">
+  <contact>
+      <lateral_friction value="1"/>
+  </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="plane.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 	<mesh filename="plane.obj" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2173,8 +2173,11 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 	
 	m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback);
 
+	//int maxProxies = 32768;
+	//m_data->m_broadphase = new btSimpleBroadphase(maxProxies, m_data->m_pairCache);
     m_data->m_broadphase = new btDbvtBroadphase(m_data->m_pairCache);
     
+
     m_data->m_solver = new btMultiBodyConstraintSolver;
     
 #ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
@@ -2183,7 +2186,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
     m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
 #endif
     
-    //Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
+    //Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
     m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(32768);
     
     m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
@@ -8305,6 +8308,9 @@ bool PhysicsServerCommandProcessor::processSaveBulletCommand(const struct Shared
 	if (f)
 	{
 		btDefaultSerializer* ser = new btDefaultSerializer();
+		int currentFlags = ser->getSerializationFlags();
+		ser->setSerializationFlags(currentFlags | BT_SERIALIZE_CONTACT_MANIFOLDS);
+
 		m_data->m_dynamicsWorld->serialize(ser);
 		fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f);
 		fclose(f);

examples/pybullet/examples/saveRestoreState.py

@@ -1,30 +1,95 @@
 import pybullet as p
 import math, time
+import difflib,sys
 
+numSteps = 500
+numSteps2 = 3
 p.connect(p.GUI, options="--width=1024 --height=768")
+numObjects = 10
+verbose = 0
 
-p.loadURDF("plane.urdf")
+def setupWorld():
-for i in range (10):
+	p.resetSimulation()
-	cube = p.loadURDF("cube_small.urdf",0,i*0.01,i*0.5)
+	p.loadURDF("planeMesh.urdf")
-p.setGravity(0,0,-10)
+	for i in range (numObjects):
-
+		cube = p.loadURDF("cube_small.urdf",0,0,(i+1)*0.5)
-
-#for i in range (500):
-#	p.stepSimulation()
-
-
-
-saveState = 1
-
-if saveState:
-	for i in range (500):
 	p.stepSimulation()
-	p.saveBullet("state.bullet")
+	p.setGravity(0,0,-10)
-else:
+
-	p.restoreState(fileName="state.bullet")
+
+def dumpStateToFile(file):
+	for i in  range (p.getNumBodies()):
+		pos,orn = p.getBasePositionAndOrientation(i)
+		linVel,angVel = p.getBaseVelocity(i)
+		txtPos = "pos="+str(pos)+"\n"
+		txtOrn = "orn="+str(orn)+"\n"
+		txtLinVel = "linVel"+str(linVel)+"\n"
+		txtAngVel = "angVel"+str(angVel)+"\n"
+		file.write(txtPos)
+		file.write(txtOrn)
+		file.write(txtLinVel)
+		file.write(txtAngVel)
+		
+def compareFiles(file1,file2):
+	diff = difflib.unified_diff(
+            file1.readlines(),
+            file2.readlines(),
+            fromfile='saveFile.txt',
+            tofile='restoreFile.txt',
+        )
+	for line in diff:
+		sys.stdout.write(line)
+	
+setupWorld()
+for i in range (numSteps):
+	p.stepSimulation()	
+p.saveBullet("state.bullet")
+if verbose:
+	p.setInternalSimFlags(1)
+p.stepSimulation()
+if verbose:
+	p.setInternalSimFlags(0)
+	print("contact points=")
+	for q in p.getContactPoints():
+		print(q)
+
+for i in range (numSteps2):
+	p.stepSimulation()
+
+
+file = open("saveFile.txt","w") 
+dumpStateToFile(file)
+file.close() 
+
+#################################
+setupWorld()
+p.restoreState(fileName="state.bullet")
+if verbose:
+	p.setInternalSimFlags(1)
+p.stepSimulation()
+if verbose:
+	p.setInternalSimFlags(0)
+	print("contact points restored=")
+	for q in p.getContactPoints():
+		print(q)
+for i in range (numSteps2):
+	p.stepSimulation()
+
+	
+file = open("restoreFile.txt","w")
+dumpStateToFile(file)
+file.close() 
+
+file1 = open("saveFile.txt","r") 
+file2 = open("restoreFile.txt","r") 
+#file3 = open("saveFile.txt","r")#saveFileReorder.txt","r") 
+
+compareFiles(file1,file2)
+#compareFiles(file1,file3)
             
-for i in range (10):
-	print("pos[",i,"]=",p.getBasePositionAndOrientation(i))
 
 while (p.getConnectionInfo()["isConnected"]):
 	time.sleep(1)
+
+file1.close()
+file2.close()

examples/pybullet/pybullet.c

@@ -7876,8 +7876,8 @@ static PyMethodDef SpamMethods[] = {
 	 { "restoreState", (PyCFunction)pybullet_restoreState, METH_VARARGS | METH_KEYWORDS,
 	"Restore the full state of an existing world." },
 
-//	{ "saveState", (PyCFunction)pybullet_saveState, METH_VARARGS | METH_KEYWORDS,
+	{ "saveState", (PyCFunction)pybullet_saveState, METH_VARARGS | METH_KEYWORDS,
-//	"Save the full state of the world to memory." },
+	"Save the full state of the world to memory." },
 
 	{"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS,
 	 "Load multibodies from an MJCF file."},

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -16,6 +16,7 @@ subject to the following restrictions:
 
 #include "btCollisionObject.h"
 #include "LinearMath/btSerializer.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 
 btCollisionObject::btCollisionObject()
 	:	m_interpolationLinearVelocity(0.f, 0.f, 0.f),
@@ -114,10 +115,18 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
 	dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius;
 	dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
 	dataOut->m_checkCollideWith = m_checkCollideWith;
-
+	if (m_broadphaseHandle)
-	// Fill padding with zeros to appease msan.
+	{
-	memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
+		dataOut->m_collisionFilterGroup = m_broadphaseHandle->m_collisionFilterGroup;
-
+		dataOut->m_collisionFilterMask = m_broadphaseHandle->m_collisionFilterMask;
+		dataOut->m_uniqueId = m_broadphaseHandle->m_uniqueId;
+	}
+	else
+	{
+		dataOut->m_collisionFilterGroup = 0;
+		dataOut->m_collisionFilterMask = 0;
+		dataOut->m_uniqueId = -1;
+	}
 	return btCollisionObjectDataName;
 }
 

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -621,7 +621,6 @@ struct	btCollisionObjectDoubleData
 	double					m_hitFraction; 
 	double					m_ccdSweptSphereRadius;
 	double					m_ccdMotionThreshold;
-
 	int						m_hasAnisotropicFriction;
 	int						m_collisionFlags;
 	int						m_islandTag1;
@@ -629,8 +628,9 @@ struct	btCollisionObjectDoubleData
 	int						m_activationState1;
 	int						m_internalType;
 	int						m_checkCollideWith;
-
+	int						m_collisionFilterGroup;
-	char	m_padding[4];
+	int						m_collisionFilterMask;
+	int						m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
@@ -656,7 +656,6 @@ struct	btCollisionObjectFloatData
 	float					m_hitFraction; 
 	float					m_ccdSweptSphereRadius;
 	float					m_ccdMotionThreshold;
-
 	int						m_hasAnisotropicFriction;
 	int						m_collisionFlags;
 	int						m_islandTag1;
@@ -664,7 +663,9 @@ struct	btCollisionObjectFloatData
 	int						m_activationState1;
 	int						m_internalType;
 	int						m_checkCollideWith;
-	char					m_padding[4];
+	int						m_collisionFilterGroup;
+	int						m_collisionFilterMask;
+	int						m_uniqueId;
 };
 
 

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -1654,6 +1654,28 @@ void	btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
 }
 
 
+
+void btCollisionWorld::serializeContactManifolds(btSerializer* serializer)
+{
+	if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS)
+	{
+		int numManifolds = getDispatcher()->getNumManifolds();
+		for (int i = 0; i < numManifolds; i++)
+		{
+			const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i];
+			//don't serialize empty manifolds, they just take space 
+			//(may have to do it anyway if it destroys determinism)
+			if (manifold->getNumContacts() == 0)
+				continue;
+
+			btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1);
+			const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk, structType, BT_CONTACTMANIFOLD_CODE, (void*)manifold);
+		}
+	}
+}
+
+
 void	btCollisionWorld::serialize(btSerializer* serializer)
 {
 
@@ -1661,6 +1683,8 @@ void	btCollisionWorld::serialize(btSerializer* serializer)
 	
 	serializeCollisionObjects(serializer);
 
+	serializeContactManifolds(serializer);
+	
 	serializer->finishSerialization();
 }
 

src/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -107,6 +107,9 @@ protected:
 
 	void	serializeCollisionObjects(btSerializer* serializer);
 
+	void serializeContactManifolds(btSerializer* serializer);
+
+
 public:
 
 	//this constructor doesn't own the dispatcher and paircache/broadphase

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -199,6 +199,22 @@ class btPersistentManifoldSortPredicate
 		}
 };
 
+class btPersistentManifoldSortPredicateDeterministic
+{
+public:
+
+	SIMD_FORCE_INLINE bool operator() (const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
+	{
+		return (
+			(getIslandId(lhs) < getIslandId(rhs))
+							|| ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) 
+						||((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) &&
+					(lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId))
+			);
+
+	}
+};
+
 
 void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
 {
@@ -318,6 +334,8 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
 	for (i=0;i<maxNumManifolds ;i++)
 	{
 		 btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
+		 if (manifold->getNumContacts() == 0)
+			 continue;
 
 		 const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
 		 const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
@@ -379,7 +397,13 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 
 		//tried a radix sort, but quicksort/heapsort seems still faster
 		//@todo rewrite island management
-		m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+
+		//m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+
+		//btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid,
+		//but also based on object0 unique id and object1 unique id
+		m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic());
+
 		//m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
 
 		//now process all active islands (sets of manifolds for now)

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

@@ -16,6 +16,7 @@ subject to the following restrictions:
 
 #include "btPersistentManifold.h"
 #include "LinearMath/btTransform.h"
+#include "LinearMath/btSerializer.h"
 
 
 btScalar					gContactBreakingThreshold = btScalar(0.02);
@@ -33,6 +34,8 @@ btPersistentManifold::btPersistentManifold()
 m_body0(0),
 m_body1(0),
 m_cachedPoints (0),
+m_companionIdA(0),
+m_companionIdB(0),
 m_index1a(0)
 {
 }
@@ -303,6 +306,103 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT
 }
 
 
+int	btPersistentManifold::calculateSerializeBufferSize()	const
+{
+	return sizeof(btPersistentManifoldData);
+}
 
+const char*	btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const
+{
+	btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer;
+	memset(dataOut, 0, sizeof(btPersistentManifoldData));
 
+	dataOut->m_body0 = serializer->getUniquePointer((void*)manifold->getBody0());
+	dataOut->m_body1 = serializer->getUniquePointer((void*)manifold->getBody1());
+	dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold();
+	dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold();
+	dataOut->m_numCachedPoints = manifold->getNumContacts();
+	dataOut->m_companionIdA = manifold->m_companionIdA;
+	dataOut->m_companionIdB = manifold->m_companionIdB;
+	dataOut->m_index1a = manifold->m_index1a;
+	dataOut->m_objectType = manifold->m_objectType;
 
+	for (int i = 0; i < this->getNumContacts(); i++)
+	{
+		const btManifoldPoint& pt = manifold->getContactPoint(i);
+		dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
+		dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
+		dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
+		pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
+		pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]);
+		pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]);
+		dataOut->m_pointCacheDistance[i] = pt.m_distance1;
+		dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1;
+		dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1;
+		dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime;
+		dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM;
+		dataOut->m_pointCacheContactERP[i] = pt.m_contactERP;
+		dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM;
+		dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags;
+		dataOut->m_pointCacheIndex0[i] = pt.m_index0;
+		dataOut->m_pointCacheIndex1[i] = pt.m_index1;
+		dataOut->m_pointCachePartId0[i] = pt.m_partId0;
+		dataOut->m_pointCachePartId1[i] = pt.m_partId1;
+		pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]);
+		pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]);
+		dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction;
+		pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]);
+		pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]);
+		dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction;
+		dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction;
+		dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution;
+		dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1;
+		dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2;
+	}
+	return btPersistentManifoldDataName;
+}
+
+void btPersistentManifold::deSerializeDouble(const struct btPersistentManifoldDoubleData* manifoldDataPtr)
+{
+	m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
+	m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
+	m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
+	m_companionIdA = manifoldDataPtr->m_companionIdA;
+	m_companionIdB = manifoldDataPtr->m_companionIdB;
+	//m_index1a = manifoldDataPtr->m_index1a;
+	m_objectType = manifoldDataPtr->m_objectType;
+
+	for (int i = 0; i < this->getNumContacts(); i++)
+	{
+		btManifoldPoint& pt = m_pointCache[i];
+		
+		pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
+		pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
+		pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
+		pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]);
+		pt.m_localPointB.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointB[i]);
+		pt.m_normalWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]);
+		pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
+		pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i];
+		pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i];
+		pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
+		pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
+		pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
+		pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
+		pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
+		pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
+		pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
+		pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
+		pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
+		pt.m_positionWorldOnA.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
+		pt.m_positionWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
+		pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
+		pt.m_lateralFrictionDir1.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
+		pt.m_lateralFrictionDir2.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]);
+		pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i];
+		pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i];
+		pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i];
+		pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
+		pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
+	}
+
+}

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -95,7 +95,10 @@ public:
 		: btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
 	m_body0(body0),m_body1(body1),m_cachedPoints(0),
 		m_contactBreakingThreshold(contactBreakingThreshold),
-		m_contactProcessingThreshold(contactProcessingThreshold)
+		m_contactProcessingThreshold(contactProcessingThreshold),
+		m_companionIdA(0),
+		m_companionIdB(0),
+		m_index1a(0)
 	{
 	}
 
@@ -256,10 +259,114 @@ public:
 		m_cachedPoints = 0;
 	}
 
+	int	calculateSerializeBufferSize()	const;
+	const char*	serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const;
+	void deSerializeDouble(const struct btPersistentManifoldDoubleData* manifoldDataPtr);
 
 
-}
+};
-;
+
+
+
+struct btPersistentManifoldDoubleData
+{
+	btVector3DoubleData m_pointCacheLocalPointA[4];
+	btVector3DoubleData m_pointCacheLocalPointB[4];
+	btVector3DoubleData m_pointCachePositionWorldOnA[4];
+	btVector3DoubleData m_pointCachePositionWorldOnB[4];
+	btVector3DoubleData m_pointCacheNormalWorldOnB[4];
+	btVector3DoubleData	m_pointCacheLateralFrictionDir1[4];
+	btVector3DoubleData	m_pointCacheLateralFrictionDir2[4];
+	double m_pointCacheDistance[4];
+	double m_pointCacheAppliedImpulse[4];
+	double m_pointCacheCombinedFriction[4];
+	double m_pointCacheCombinedRollingFriction[4];
+	double m_pointCacheCombinedSpinningFriction[4];
+	double m_pointCacheCombinedRestitution[4];
+	int	m_pointCachePartId0[4];
+	int	m_pointCachePartId1[4];
+	int	m_pointCacheIndex0[4];
+	int	m_pointCacheIndex1[4];
+	int m_pointCacheContactPointFlags[4];
+	double m_pointCacheAppliedImpulseLateral1[4];
+	double m_pointCacheAppliedImpulseLateral2[4];
+	double m_pointCacheContactMotion1[4];
+	double m_pointCacheContactMotion2[4];
+	double m_pointCacheContactCFM[4];
+	double m_pointCacheCombinedContactStiffness1[4];
+	double m_pointCacheContactERP[4];
+	double m_pointCacheCombinedContactDamping1[4];
+	double m_pointCacheFrictionCFM[4];
+	int m_pointCacheLifeTime[4];
+
+	int m_numCachedPoints;
+	int m_companionIdA;
+	int m_companionIdB;
+	int m_index1a;
+
+	int m_objectType;
+	double	m_contactBreakingThreshold;
+	double	m_contactProcessingThreshold;
+	int m_padding;
+
+	void *m_body0;
+	void *m_body1;
+};
+
+
+struct btPersistentManifoldFloatData
+{
+	btVector3FloatData m_pointCacheLocalPointA[4];
+	btVector3FloatData m_pointCacheLocalPointB[4];
+	btVector3FloatData m_pointCachePositionWorldOnA[4];
+	btVector3FloatData m_pointCachePositionWorldOnB[4];
+	btVector3FloatData m_pointCacheNormalWorldOnB[4];
+	btVector3FloatData	m_pointCacheLateralFrictionDir1;
+	btVector3FloatData	m_pointCacheLateralFrictionDir2;
+	float m_pointCacheDistance[4];
+	float m_pointCacheAppliedImpulse[4];
+	float m_pointCacheCombinedFriction[4];
+	float m_pointCacheCombinedRollingFriction[4];
+	float m_pointCacheCombinedSpinningFriction[4];
+	float m_pointCacheCombinedRestitution[4];
+	int	m_pointCachePartId0[4];
+	int	m_pointCachePartId1[4];
+	int	m_pointCacheIndex0[4];
+	int	m_pointCacheIndex1[4];
+	int m_pointCacheContactPointFlags[4];
+	float m_pointCacheAppliedImpulseLateral1[4];
+	float m_pointCacheAppliedImpulseLateral2[4];
+	float m_pointCacheContactMotion1[4];
+	float m_pointCacheContactMotion2[4];
+	float m_pointCacheContactCFM[4];
+	float m_pointCacheCombinedContactStiffness1[4];
+	float m_pointCacheContactERP[4];
+	float m_pointCacheCombinedContactDamping1[4];
+	float m_pointCacheFrictionCFM[4];
+	int m_pointCacheLifeTime[4];
+
+	int m_numCachedPoints;
+	int m_companionIdA;
+	int m_companionIdB;
+	int m_index1a;
+
+	int m_objectType;
+	float	m_contactBreakingThreshold;
+	float	m_contactProcessingThreshold;
+	int m_padding;
+
+	void *m_body0;
+	void *m_body1;
+};
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btPersistentManifoldData	btPersistentManifoldDoubleData
+#define btPersistentManifoldDataName	"btPersistentManifoldDoubleData"
+#else
+#define btPersistentManifoldData	btPersistentManifoldFloatData
+#define btPersistentManifoldDataName	"btPersistentManifoldFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+
 
 
 

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -1994,7 +1994,11 @@ int	btMultiBody::calculateSerializeBufferSize()	const
 const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
 {
 		btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
-		getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
+		getBasePos().serialize(mbd->m_baseWorldPosition);
+		getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
+		getBaseVel().serialize(mbd->m_baseLinearVelocity);
+		getBaseOmega().serialize(mbd->m_baseAngularVelocity);
+
 		mbd->m_baseMass = this->getBaseMass();
 		getBaseInertia().serialize(mbd->m_baseInertia);
 		{

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -784,29 +784,38 @@ struct btMultiBodyLinkFloatData
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btMultiBodyDoubleData
 {
-	btTransformDoubleData m_baseWorldTransform;
+	btVector3DoubleData m_baseWorldPosition;
+	btQuaternionDoubleData m_baseWorldOrientation;
+	btVector3DoubleData m_baseLinearVelocity;
+	btVector3DoubleData m_baseAngularVelocity;
 	btVector3DoubleData m_baseInertia;   // inertia of the base (in local frame; diagonal)
 	double	m_baseMass;
+	int		m_numLinks;
+	char	m_padding[4];
 
 	char	*m_baseName;
 	btMultiBodyLinkDoubleData	*m_links;
 	btCollisionObjectDoubleData	*m_baseCollider;
-	char	*m_paddingPtr;
+	
-	int		m_numLinks;
+	
-	char	m_padding[4];
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btMultiBodyFloatData
 {
+	btVector3FloatData m_baseWorldPosition;
+	btQuaternionFloatData m_baseWorldOrientation;
+	btVector3FloatData m_baseLinearVelocity;
+	btVector3FloatData m_baseAngularVelocity;
+
+	btVector3FloatData m_baseInertia;   // inertia of the base (in local frame; diagonal)
+	float	m_baseMass;
+	int		m_numLinks;
+
 	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;
 };
 
 

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -972,6 +972,8 @@ void	btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeCollisionObjects(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
 

src/LinearMath/btSerializer.h

@@ -62,7 +62,8 @@ enum	btSerializationFlags
 {
 	BT_SERIALIZE_NO_BVH = 1,
 	BT_SERIALIZE_NO_TRIANGLEINFOMAP = 2,
-	BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4
+	BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4,
+	BT_SERIALIZE_CONTACT_MANIFOLDS = 8,
 };
 
 class	btSerializer
@@ -128,9 +129,9 @@ public:
 #define BT_SBMATERIAL_CODE		BT_MAKE_ID('S','B','M','T')
 #define BT_SBNODE_CODE			BT_MAKE_ID('S','B','N','D')
 #define BT_DYNAMICSWORLD_CODE	BT_MAKE_ID('D','W','L','D')
+#define BT_CONTACTMANIFOLD_CODE BT_MAKE_ID('C','O','N','T')
 #define BT_DNA_CODE				BT_MAKE_ID('D','N','A','1')
 
-
 struct	btPointerUid
 {
 	union