added some template to restore (syncMultiBody, syncContactManifolds) for single float and double precision, in 'pybullet.restoreState'

Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp

@@ -33,6 +33,230 @@ void btMultiBodyWorldImporter::deleteAllData()
 }
 
 
+static btCollisionObjectDoubleData* getBody0FromContactManifold(btPersistentManifoldDoubleData* manifold)
+{
+	return (btCollisionObjectDoubleData*)manifold->m_body0;
+}
+static btCollisionObjectDoubleData* getBody1FromContactManifold(btPersistentManifoldDoubleData* manifold)
+{
+	return (btCollisionObjectDoubleData*)manifold->m_body1;
+}
+static btCollisionObjectFloatData* getBody0FromContactManifold(btPersistentManifoldFloatData* manifold)
+{
+	return (btCollisionObjectFloatData*)manifold->m_body0;
+}
+static btCollisionObjectFloatData* getBody1FromContactManifold(btPersistentManifoldFloatData* manifold)
+{
+	return (btCollisionObjectFloatData*)manifold->m_body1;
+}
+
+
+template<class T> void syncContactManifolds(T** contactManifolds, int numContactManifolds, btMultiBodyWorldImporterInternalData* m_data)
+{
+	m_data->m_mbDynamicsWorld->updateAabbs();
+	m_data->m_mbDynamicsWorld->computeOverlappingPairs();
+	btDispatcher* dispatcher = m_data->m_mbDynamicsWorld->getDispatcher();
+
+	btDispatcherInfo& dispatchInfo = m_data->m_mbDynamicsWorld->getDispatchInfo();
+
+
+	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 < numContactManifolds; q++)
+					{
+						if (uid0 == getBody0FromContactManifold(contactManifolds[q])->m_uniqueId && uid1 == getBody1FromContactManifold(contactManifolds[q])->m_uniqueId)
+						{
+							matchingManifoldIndex = q;
+						}
+					}
+					if (matchingManifoldIndex >= 0)
+					{
+						existingManifold->deSerialize(contactManifolds[matchingManifoldIndex]);
+					}
+					else
+					{
+						existingManifold->setNumContacts(0);
+						//printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1);
+					}
+
+					manifoldArray.clear();
+				}
+			}
+		}
+	}
+
+}
+
+template<class T>  void syncMultiBody(T* mbd, btMultiBody* mb, btMultiBodyWorldImporterInternalData* m_data, btAlignedObjectArray<btQuaternion>& scratchQ, btAlignedObjectArray<btVector3>& scratchM)
+{
+	bool isFixedBase = mbd->m_baseMass == 0;
+	bool canSleep = false;
+	btVector3 baseInertia;
+	baseInertia.deSerialize(mbd->m_baseInertia);
+
+	btVector3 baseWorldPos;
+	baseWorldPos.deSerialize(mbd->m_baseWorldPosition);
+	mb->setBasePos(baseWorldPos);
+	btQuaternion baseWorldRot;
+	baseWorldRot.deSerialize(mbd->m_baseWorldOrientation);
+	mb->setWorldToBaseRot(baseWorldRot.inverse());
+	btVector3 baseLinVal;
+	baseLinVal.deSerialize(mbd->m_baseLinearVelocity);
+	btVector3 baseAngVel;
+	baseAngVel.deSerialize(mbd->m_baseAngularVelocity);
+	mb->setBaseVel(baseLinVal);
+	mb->setBaseOmega(baseAngVel);
+
+	for (int i = 0; i < mbd->m_numLinks; i++)
+	{
+		switch (mbd->m_links[i].m_jointType)
+		{
+		case btMultibodyLink::eFixed:
+		{
+			break;
+		}
+		case btMultibodyLink::ePrismatic:
+		{
+			mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
+			mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
+			break;
+		}
+		case btMultibodyLink::eRevolute:
+		{
+			mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
+			mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
+			break;
+		}
+		case btMultibodyLink::eSpherical:
+		{
+			btScalar jointPos[3] = { (btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2] };
+			btScalar jointVel[3] = { (btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2] };
+			mb->setJointPosMultiDof(i, jointPos);
+			mb->setJointVelMultiDof(i, jointVel);
+
+			break;
+		}
+		case btMultibodyLink::ePlanar:
+		{
+			break;
+		}
+		default:
+		{
+		}
+		}
+	}
+	mb->forwardKinematics(scratchQ, scratchM);
+	mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
+}
+
+template<class T>  void convertMultiBody(T* mbd, btMultiBodyWorldImporterInternalData* m_data)
+{
+	bool isFixedBase = mbd->m_baseMass == 0;
+	bool canSleep = false;
+	btVector3 baseInertia;
+	baseInertia.deSerialize(mbd->m_baseInertia);
+	btMultiBody* mb = new btMultiBody(mbd->m_numLinks, mbd->m_baseMass, baseInertia, isFixedBase, canSleep);
+	mb->setHasSelfCollision(false);
+
+	btVector3 baseWorldPos;
+	baseWorldPos.deSerialize(mbd->m_baseWorldPosition);
+
+	btQuaternion baseWorldOrn;
+	baseWorldOrn.deSerialize(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++)
+	{
+		btVector3 localInertiaDiagonal;
+		localInertiaDiagonal.deSerialize(mbd->m_links[i].m_linkInertia);
+		btQuaternion parentRotToThis;
+		parentRotToThis.deSerialize(mbd->m_links[i].m_zeroRotParentToThis);
+		btVector3 parentComToThisPivotOffset;
+		parentComToThisPivotOffset.deSerialize(mbd->m_links[i].m_parentComToThisComOffset);
+		btVector3 thisPivotToThisComOffset;
+		thisPivotToThisComOffset.deSerialize(mbd->m_links[i].m_thisPivotToThisComOffset);
+
+		switch (mbd->m_links[i].m_jointType)
+		{
+		case btMultibodyLink::eFixed:
+		{
+
+
+			mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
+				parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset);
+			//search for the collider
+			//mbd->m_links[i].m_linkCollider
+			break;
+		}
+		case btMultibodyLink::ePrismatic:
+		{
+			btVector3 jointAxis;
+			jointAxis.deSerialize(mbd->m_links[i].m_jointAxisBottom[0]);
+			bool disableParentCollision = true;//todo
+			mb->setupPrismatic(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
+				parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
+			mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
+			mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
+			break;
+		}
+		case btMultibodyLink::eRevolute:
+		{
+			btVector3 jointAxis;
+			jointAxis.deSerialize(mbd->m_links[i].m_jointAxisTop[0]);
+			bool disableParentCollision = true;//todo
+			mb->setupRevolute(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
+				parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
+			mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
+			mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
+			break;
+		}
+		case btMultibodyLink::eSpherical:
+		{
+			btAssert(0);
+			bool disableParentCollision = true;//todo
+			mb->setupSpherical(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
+				parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
+			btScalar jointPos[3] = { (btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2] };
+			btScalar jointVel[3] = { (btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2] };
+			mb->setJointPosMultiDof(i, jointPos);
+			mb->setJointVelMultiDof(i, jointVel);
+
+			break;
+		}
+		case btMultibodyLink::ePlanar:
+		{
+			btAssert(0);
+			break;
+		}
+		default:
+		{
+			btAssert(0);
+		}
+		}
+	}
+}
 
 bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletFile2)
 {
@@ -56,138 +280,77 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 		{
 
 			//for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
-			for (int i = bulletFile2->m_multiBodies.size()-1; i >=0; 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;
-				bool canSleep = false;
-				btVector3 baseInertia;
-				baseInertia.deSerializeDouble(mbd->m_baseInertia);
-
 				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
-				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++)
-				{
-					switch (mbd->m_links[i].m_jointType)
-					{
-					case btMultibodyLink::eFixed:
-					{
-						break;
-					}
-					case btMultibodyLink::ePrismatic:
-					{
-						mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
-						mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
-						break;
-					}
-					case btMultibodyLink::eRevolute:
-					{
-						mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
-						mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
-						break;
-					}
-					case btMultibodyLink::eSpherical:
-					{
-						btScalar jointPos[3] = { mbd->m_links[i].m_jointPos[0], mbd->m_links[i].m_jointPos[1], mbd->m_links[i].m_jointPos[2] };
-						btScalar jointVel[3] = { mbd->m_links[i].m_jointVel[0], mbd->m_links[i].m_jointVel[1], mbd->m_links[i].m_jointVel[2] };
-						mb->setJointPosMultiDof(i, jointPos);
-						mb->setJointVelMultiDof(i, jointVel);
-
-						break;
-					}
-					case btMultibodyLink::ePlanar:
-					{
-						break;
-					}
-					default:
-					{
-					}
-					}
-				}
-				mb->forwardKinematics(scratchQ, scratchM);
-				mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
+				syncMultiBody(mbd, mb, m_data, 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;
+					void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
+					if (ptr)
+					{
+						manifoldData->m_body0 = ptr;
+					}
+				}
+
+				{
+					void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
+					if (ptr)
+					{
+						manifoldData->m_body1 = ptr;
+					}
 				}
 			}
 
-			btDispatcherInfo& dispatchInfo = m_data->m_mbDynamicsWorld->getDispatchInfo();
 
+			if (bulletFile2->m_contactManifolds.size())
 			{
-				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
-								{
-									existingManifold->setNumContacts(0);
-									//printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1);
-								}
+				syncContactManifolds((btPersistentManifoldDoubleData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data);
+			}
+		}
+		else
+		{
+			//single precision version
+			//for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
+			for (int i = bulletFile2->m_multiBodies.size() - 1; i >= 0; i--)
+			{
+				btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i];
+				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
+				syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
+			}
 
-								manifoldArray.clear();
-							}
-						}
+			//todo: check why body1 pointer is not properly deserialized
+			for (int i = 0; i < bulletFile2->m_contactManifolds.size(); i++)
+			{
+				btPersistentManifoldFloatData* manifoldData = (btPersistentManifoldFloatData*)bulletFile2->m_contactManifolds[i];
+				{
+					void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
+					if (ptr)
+					{
+						manifoldData->m_body0 = ptr;
+					}
+				}
+				{
+					void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
+					if (ptr)
+					{
+						manifoldData->m_body1 = ptr;
 					}
 				}
 			}
+
+
+			if (bulletFile2->m_contactManifolds.size())
+			{
+				syncContactManifolds((btPersistentManifoldFloatData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data);
+			}
+
 		}
 	}
 	else
@@ -202,91 +365,12 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 			if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
 			{
 				btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
-				bool isFixedBase = mbd->m_baseMass == 0;
-				bool canSleep = false;
-				btVector3 baseInertia;
-				baseInertia.deSerializeDouble(mbd->m_baseInertia);
-				btMultiBody* mb = new btMultiBody(mbd->m_numLinks, mbd->m_baseMass, baseInertia, isFixedBase, canSleep);
-				mb->setHasSelfCollision(false);
-				
-				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++)
-				{
-					btVector3 localInertiaDiagonal;
-					localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia);
-					btQuaternion parentRotToThis;
-					parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis);
-					btVector3 parentComToThisPivotOffset;
-					parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset);
-					btVector3 thisPivotToThisComOffset;
-					thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset);
-
-					switch (mbd->m_links[i].m_jointType)
-					{
-					case btMultibodyLink::eFixed:
-					{
-
-
-						mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
-							parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset);
-						//search for the collider
-						//mbd->m_links[i].m_linkCollider
-						break;
-					}
-					case btMultibodyLink::ePrismatic:
-					{
-						btVector3 jointAxis;
-						jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisBottom[0]);
-						bool disableParentCollision = true;//todo
-						mb->setupPrismatic(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
-							parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
-						mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
-						mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
-						break;
-					}
-					case btMultibodyLink::eRevolute:
-					{
-						btVector3 jointAxis;
-						jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisTop[0]);
-						bool disableParentCollision = true;//todo
-						mb->setupRevolute(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
-							parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
-						mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
-						mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
-						break;
-					}
-					case btMultibodyLink::eSpherical:
-					{
-						btAssert(0);
-						bool disableParentCollision = true;//todo
-						mb->setupSpherical(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
-							parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
-						btScalar jointPos[3] = { mbd->m_links[i].m_jointPos[0], mbd->m_links[i].m_jointPos[1], mbd->m_links[i].m_jointPos[2] };
-						btScalar jointVel[3] = { mbd->m_links[i].m_jointVel[0], mbd->m_links[i].m_jointVel[1], mbd->m_links[i].m_jointVel[2] };
-						mb->setJointPosMultiDof(i, jointPos);
-						mb->setJointVelMultiDof(i, jointVel);
-
-						break;
-					}
-					case btMultibodyLink::ePlanar:
-					{
-						btAssert(0);
-						break;
-					}
-					default:
-					{
-						btAssert(0);
-					}
-					}
-				}
+				convertMultiBody(mbd, m_data);
+			}
+			else
+			{
+				btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i];
+				convertMultiBody(mbd, m_data);
 			}
 		}
 

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

@@ -361,7 +361,7 @@ const char*	btPersistentManifold::serialize(const class btPersistentManifold* ma
 	return btPersistentManifoldDataName;
 }
 
-void btPersistentManifold::deSerializeDouble(const struct btPersistentManifoldDoubleData* manifoldDataPtr)
+void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr)
 {
 	m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
 	m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
@@ -405,4 +405,50 @@ void btPersistentManifold::deSerializeDouble(const struct btPersistentManifoldDo
 		pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
 	}
 
+}
+
+void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* 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.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);
+		pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]);
+		pt.m_normalWorldOnB.deSerialize(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.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
+		pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
+		pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
+		pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
+		pt.m_lateralFrictionDir2.deSerialize(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

@@ -261,7 +261,8 @@ public:
 
 	int	calculateSerializeBufferSize()	const;
 	const char*	serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const;
-	void deSerializeDouble(const struct btPersistentManifoldDoubleData* manifoldDataPtr);
+	void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr);
+	void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr);
 
 
 };
@@ -321,8 +322,8 @@ struct btPersistentManifoldFloatData
 	btVector3FloatData m_pointCachePositionWorldOnA[4];
 	btVector3FloatData m_pointCachePositionWorldOnB[4];
 	btVector3FloatData m_pointCacheNormalWorldOnB[4];
-	btVector3FloatData	m_pointCacheLateralFrictionDir1;
-	btVector3FloatData	m_pointCacheLateralFrictionDir2;
+	btVector3FloatData	m_pointCacheLateralFrictionDir1[4];
+	btVector3FloatData	m_pointCacheLateralFrictionDir2[4];
 	float m_pointCacheDistance[4];
 	float m_pointCacheAppliedImpulse[4];
 	float m_pointCacheCombinedFriction[4];

src/LinearMath/btQuaternion.h

@@ -602,7 +602,9 @@ public:
 
 	SIMD_FORCE_INLINE	void	serialize(struct	btQuaternionData& dataOut) const;
 
-	SIMD_FORCE_INLINE	void	deSerialize(const struct	btQuaternionData& dataIn);
+	SIMD_FORCE_INLINE	void	deSerialize(const struct	btQuaternionFloatData& dataIn);
+
+	SIMD_FORCE_INLINE	void	deSerialize(const struct	btQuaternionDoubleData& dataIn);
 
 	SIMD_FORCE_INLINE	void	serializeFloat(struct	btQuaternionFloatData& dataOut) const;
 
@@ -1003,10 +1005,16 @@ SIMD_FORCE_INLINE	void	btQuaternion::serialize(struct	btQuaternionData& dataOut)
 		dataOut.m_floats[i] = m_floats[i];
 }
 
-SIMD_FORCE_INLINE void	btQuaternion::deSerialize(const struct	btQuaternionData& dataIn)
+SIMD_FORCE_INLINE void	btQuaternion::deSerialize(const struct	btQuaternionFloatData& dataIn)
+{
+	for (int i = 0; i<4; i++)
+		m_floats[i] = (btScalar)dataIn.m_floats[i];
+}
+
+SIMD_FORCE_INLINE void	btQuaternion::deSerialize(const struct	btQuaternionDoubleData& dataIn)
 {
 	for (int i=0;i<4;i++)
-		m_floats[i] = dataIn.m_floats[i];
+		m_floats[i] = (btScalar)dataIn.m_floats[i];
 }
 
 

src/LinearMath/btVector3.h

@@ -705,7 +705,9 @@ public:
 
 	SIMD_FORCE_INLINE	void	serialize(struct	btVector3Data& dataOut) const;
 
-	SIMD_FORCE_INLINE	void	deSerialize(const struct	btVector3Data& dataIn);
+	SIMD_FORCE_INLINE	void	deSerialize(const struct	btVector3DoubleData& dataIn);
+
+	SIMD_FORCE_INLINE	void	deSerialize(const struct	btVector3FloatData& dataIn);
 
 	SIMD_FORCE_INLINE	void	serializeFloat(struct	btVector3FloatData& dataOut) const;
 
@@ -1354,10 +1356,18 @@ SIMD_FORCE_INLINE	void	btVector3::serialize(struct	btVector3Data& dataOut) const
 		dataOut.m_floats[i] = m_floats[i];
 }
 
-SIMD_FORCE_INLINE void	btVector3::deSerialize(const struct	btVector3Data& dataIn)
+
+SIMD_FORCE_INLINE void	btVector3::deSerialize(const struct	btVector3FloatData& dataIn)
+{
+	for (int i = 0; i<4; i++)
+		m_floats[i] = (btScalar)dataIn.m_floats[i];
+}
+
+
+SIMD_FORCE_INLINE void	btVector3::deSerialize(const struct	btVector3DoubleData& dataIn)
 {
 	for (int i=0;i<4;i++)
-		m_floats[i] = dataIn.m_floats[i];
+		m_floats[i] = (btScalar)dataIn.m_floats[i];
 }
 
 #endif //BT_VECTOR3_H