From c80e2816ad3e5f4b540e1b0497fba502c2d95b10 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 29 Oct 2018 12:08:34 -0700 Subject: [PATCH] PyBullet: Use fileIOPlugin in processRestoreStateCommand PyBullet: Report debug information in case of failure in restoreState. --- .../btMultiBodyWorldImporter.cpp | 63 ++++++++++++++++++- .../PhysicsServerCommandProcessor.cpp | 51 ++++++++++----- 2 files changed, 95 insertions(+), 19 deletions(-) diff --git a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp index 884da0072..f18a5d2c5 100644 --- a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp +++ b/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,7 +288,15 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil { btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i]; btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i); - syncMultiBody(mbd, mb, m_data, scratchQ, scratchM); + 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,7 +373,55 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil { btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i]; btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i); - syncMultiBody(mbd, mb, m_data, scratchQ, scratchM); + 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 diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4085a6949..d251ca53e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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 buffer; + buffer.reserve(1024); + if (fileIO) { - sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName); - f = fopen(relativeFileName, "rb"); - if (f) + + int fileId = -1; + found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024); + if (found) { - found = true; - break; + 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 (f) - { - fclose(f); - } - if (found) + if (found && buffer.size()) { - ok = importer->loadFile(relativeFileName); + 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;