PyBullet: Use fileIOPlugin in processRestoreStateCommand

PyBullet: Report debug information in case of failure in restoreState.

Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp

@@ -274,6 +274,7 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
 		//equal number of objects, # links etc
 		if ((bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies()))
 		{
+			printf("btMultiBodyWorldImporter::convertAllObjects error: expected %d multibodies, got %d.\n", m_data->m_mbDynamicsWorld->getNumMultibodies(), bulletFile2->m_multiBodies.size());
 			result = false;
 			return result;
 		}
@@ -287,8 +288,16 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
 			{
 				btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
 				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
+				if (mbd->m_numLinks != mb->getNumLinks())
+				{
+					printf("btMultiBodyWorldImporter::convertAllObjects error: mismatch in number of links in a body (expected %d, found %d).\n", mbd->m_numLinks, mb->getNumLinks() );
+					result = false;
+					return result;
+				} else
+				{
 					syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
 				}
+			}
 
 			for (int i = bulletFile2->m_rigidBodies.size() - 1; i >= 0; i--)
 			{
@@ -319,11 +328,13 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
 					}
 					else
 					{
+						printf("btMultiBodyWorldImporter::convertAllObjects error: cannot find btRigidBody with bodyUniqueId %d\n", uid);
 						result = false;
 					}
 				}
 				else
 				{
+					printf("Error in btMultiBodyWorldImporter::convertAllObjects: didn't find bodyUniqueId: %d\n", uid);
 					result = false;
 				}
 			}
@@ -362,8 +373,56 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
 			{
 				btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i];
 				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
+				if (mbd->m_numLinks != mb->getNumLinks())
+				{
+					printf("btMultiBodyWorldImporter::convertAllObjects error: mismatch in number of links in a body (expected %d, found %d).\n", mbd->m_numLinks, mb->getNumLinks() );
+					result = false;
+					return result;
+				} else
+				{
 					syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
 				}
+			}
+
+			for (int i = bulletFile2->m_rigidBodies.size() - 1; i >= 0; i--)
+			{
+				btRigidBodyFloatData* rbd = (btRigidBodyFloatData*)bulletFile2->m_rigidBodies[i];
+				int foundRb = -1;
+				int uid = rbd->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)
+					{
+						foundRb = i;
+						break;
+					}
+				}
+				if (foundRb >= 0)
+				{
+					btRigidBody* rb = btRigidBody::upcast(m_data->m_mbDynamicsWorld->getCollisionObjectArray()[foundRb]);
+					if (rb)
+					{
+						btTransform tr;
+						tr.deSerializeFloat(rbd->m_collisionObjectData.m_worldTransform);
+						rb->setWorldTransform(tr);
+						btVector3 linVel, angVel;
+						linVel.deSerializeFloat(rbd->m_linearVelocity);
+						angVel.deSerializeFloat(rbd->m_angularVelocity);
+						rb->setLinearVelocity(linVel);
+						rb->setAngularVelocity(angVel);
+					}
+					else
+					{
+						printf("btMultiBodyWorldImporter::convertAllObjects error: cannot find btRigidBody with bodyUniqueId %d\n", uid);
+						result = false;
+					}
+				}
+				else
+				{
+					printf("Error in btMultiBodyWorldImporter::convertAllObjects: didn't find bodyUniqueId: %d\n", uid);
+					result = false;
+				}
+			}
 
 			//todo: check why body1 pointer is not properly deserialized
 			for (int i = 0; i < bulletFile2->m_contactManifolds.size(); i++)

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -10152,33 +10152,50 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
 	}
 	else
 	{
-		const char* prefix[] = {"", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
-		int numPrefixes = sizeof(prefix) / sizeof(const char*);
-		char relativeFileName[1024];
-		FILE* f = 0;
 		bool found = false;
+		char fileName[1024];
+		fileName[0] = 0;
 
-		for (int i = 0; !f && i < numPrefixes; i++)
+		CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
+		b3AlignedObjectArray<char> buffer;
+		buffer.reserve(1024);
+		if (fileIO)
 		{
-			sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
-			f = fopen(relativeFileName, "rb");
-			if (f)
-			{
-				found = true;
-				break;
-			}
-		}
-		if (f)
-		{
-			fclose(f);
-		}
 			
+			int fileId = -1;
+			found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName,  fileName, 1024);
 			if (found)
 			{
-			ok = importer->loadFile(relativeFileName);
+				fileId = fileIO->fileOpen(fileName,"rb");
+			}
+			if (fileId>=0)
+			{
+				int size = fileIO->getFileSize(fileId);
+				if (size>0)
+				{
+					buffer.resize(size);
+					int actual = fileIO->fileRead(fileId,&buffer[0],size);
+					if (actual != size)
+					{
+						b3Warning("image filesize mismatch!\n");
+						buffer.resize(0);
+					} else
+					{
+						found=true;
+					}
+				}
+				fileIO->fileClose(fileId);
 			}
 		}
 
+		if (found && buffer.size())
+		{
+			ok = importer->loadFileFromMemory(&buffer[0], buffer.size());
+		} else
+		{
+			b3Error("Error in restoreState: cannot load file %s\n", clientCmd.m_fileArguments.m_fileName);
+		}
+	}
 	if (ok)
 	{
 		serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED;