Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

PyBullet: Use fileIOPlugin in processRestoreStateCommand

Browse Source 
PyBullet: Report debug information in case of failure in restoreState.
This commit is contained in:
erwincoumans
2018-10-29 12:08:34 -07:00
parent 35790c36c2
commit c80e2816ad
2 changed files with 95 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp
View File

@@ -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

51
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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)
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;