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