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

Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp

@@ -33,48 +33,97 @@ void btMultiBodyWorldImporter::deleteAllData()
 }
 
 
-
+static btCollisionObjectDoubleData* getBody0FromContactManifold(btPersistentManifoldDoubleData* manifold)
-bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletFile2)
 {
-	bool result = false;
+	return (btCollisionObjectDoubleData*)manifold->m_body0;
-	btAlignedObjectArray<btQuaternion> scratchQ;
+}
-	btAlignedObjectArray<btVector3> scratchM;
+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;
+}
 
-	if (m_importerFlags&eRESTORE_EXISTING_OBJECTS)
+
+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)
 	{
-		//check if the snapshot is valid for the existing world
+		btOverlappingPairCache* pairCache = m_data->m_mbDynamicsWorld->getBroadphase()->getOverlappingPairCache();
-		//equal number of objects, # links etc
+		if (dispatcher)
-		if (bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies())
 		{
-			result = false;
+			dispatcher->dispatchAllCollisionPairs(pairCache, dispatchInfo, dispatcher);
-			return result;
 		}
-		result = true;
+		int numExistingManifolds = m_data->m_mbDynamicsWorld->getDispatcher()->getNumManifolds();
-
+		btManifoldArray manifoldArray;
-		//convert all multibodies
+		for (int i = 0; i < pairCache->getNumOverlappingPairs(); i++)
-		if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
 		{
-
+			btBroadphasePair& pair = pairCache->getOverlappingPairArray()[i];
-			//for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
+			if (pair.m_algorithm)
-			for (int i = bulletFile2->m_multiBodies.size()-1; i >=0; i--)
 			{
-				btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
+				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.deSerializeDouble(mbd->m_baseInertia);
+	baseInertia.deSerialize(mbd->m_baseInertia);
 
-				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
 	btVector3 baseWorldPos;
-				baseWorldPos.deSerializeDouble(mbd->m_baseWorldPosition);
+	baseWorldPos.deSerialize(mbd->m_baseWorldPosition);
 	mb->setBasePos(baseWorldPos);
 	btQuaternion baseWorldRot;
-				baseWorldRot.deSerializeDouble(mbd->m_baseWorldOrientation);
+	baseWorldRot.deSerialize(mbd->m_baseWorldOrientation);
 	mb->setWorldToBaseRot(baseWorldRot.inverse());
 	btVector3 baseLinVal;
-				baseLinVal.deSerializeDouble(mbd->m_baseLinearVelocity);
+	baseLinVal.deSerialize(mbd->m_baseLinearVelocity);
 	btVector3 baseAngVel;
-				baseAngVel.deSerializeDouble(mbd->m_baseAngularVelocity);
+	baseAngVel.deSerialize(mbd->m_baseAngularVelocity);
 	mb->setBaseVel(baseLinVal);
 	mb->setBaseOmega(baseAngVel);
 
@@ -100,8 +149,8 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 		}
 		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 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] = { mbd->m_links[i].m_jointVel[0], mbd->m_links[i].m_jointVel[1], mbd->m_links[i].m_jointVel[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);
 
@@ -118,102 +167,22 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 	}
 	mb->forwardKinematics(scratchQ, scratchM);
 	mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
-			}
+}
 
-			//todo: check why body1 pointer is not properly deserialized
+template<class T>  void convertMultiBody(T* mbd, btMultiBodyWorldImporterInternalData* m_data)
-			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
-								{
-									existingManifold->setNumContacts(0);
-									//printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1);
-								}
-
-								manifoldArray.clear();
-							}
-						}
-					}
-				}
-			}
-		}
-	}
-	else
-	{
-		result = btBulletWorldImporter::convertAllObjects(bulletFile2);
-
-
-		//convert all multibodies
-		for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
-		{
-
-			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);
+	baseInertia.deSerialize(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);
+	baseWorldPos.deSerialize(mbd->m_baseWorldPosition);
 
 	btQuaternion baseWorldOrn;
-				baseWorldOrn.deSerializeDouble(mbd->m_baseWorldOrientation);
+	baseWorldOrn.deSerialize(mbd->m_baseWorldOrientation);
 	mb->setBasePos(baseWorldPos);
 	mb->setWorldToBaseRot(baseWorldOrn.inverse());
 
@@ -221,13 +190,13 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 	for (int i = 0; i < mbd->m_numLinks; i++)
 	{
 		btVector3 localInertiaDiagonal;
-					localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia);
+		localInertiaDiagonal.deSerialize(mbd->m_links[i].m_linkInertia);
 		btQuaternion parentRotToThis;
-					parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis);
+		parentRotToThis.deSerialize(mbd->m_links[i].m_zeroRotParentToThis);
 		btVector3 parentComToThisPivotOffset;
-					parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset);
+		parentComToThisPivotOffset.deSerialize(mbd->m_links[i].m_parentComToThisComOffset);
 		btVector3 thisPivotToThisComOffset;
-					thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset);
+		thisPivotToThisComOffset.deSerialize(mbd->m_links[i].m_thisPivotToThisComOffset);
 
 		switch (mbd->m_links[i].m_jointType)
 		{
@@ -244,7 +213,7 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 		case btMultibodyLink::ePrismatic:
 		{
 			btVector3 jointAxis;
-						jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisBottom[0]);
+			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);
@@ -255,7 +224,7 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 		case btMultibodyLink::eRevolute:
 		{
 			btVector3 jointAxis;
-						jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisTop[0]);
+			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);
@@ -269,8 +238,8 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 			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 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] = { mbd->m_links[i].m_jointVel[0], mbd->m_links[i].m_jointVel[1], mbd->m_links[i].m_jointVel[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);
 
@@ -287,6 +256,121 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 		}
 		}
 	}
+}
+
+bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletFile2)
+{
+	bool result = false;
+	btAlignedObjectArray<btQuaternion> scratchQ;
+	btAlignedObjectArray<btVector3> scratchM;
+
+	if (m_importerFlags&eRESTORE_EXISTING_OBJECTS)
+	{
+		//check if the snapshot is valid for the existing world
+		//equal number of objects, # links etc
+		if (bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies())
+		{
+			result = false;
+			return result;
+		}
+		result = true;
+
+		//convert all multibodies
+		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];
+				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
+				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_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((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);
+			}
+
+			//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
+	{
+		result = btBulletWorldImporter::convertAllObjects(bulletFile2);
+
+
+		//convert all multibodies
+		for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
+		{
+
+			if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
+			{
+				btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
+				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;
@@ -406,3 +406,49 @@ void btPersistentManifold::deSerializeDouble(const struct btPersistentManifoldDo
 	}
 
 }
+
+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_pointCacheLateralFrictionDir1[4];
-	btVector3FloatData	m_pointCacheLateralFrictionDir2;
+	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