mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 05:40:05 +00:00
571 lines
19 KiB
C++
571 lines
19 KiB
C++
#include "btMultiBodyWorldImporter.h"
|
|
|
|
#include "LinearMath/btSerializer.h"
|
|
#include "../BulletFileLoader/btBulletFile.h"
|
|
#include "btBulletWorldImporter.h"
|
|
#include "btBulletDynamicsCommon.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBody.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
|
|
|
struct btMultiBodyWorldImporterInternalData
|
|
{
|
|
btMultiBodyDynamicsWorld* m_mbDynamicsWorld;
|
|
btHashMap<btHashPtr, btMultiBody*> m_mbMap;
|
|
};
|
|
|
|
btMultiBodyWorldImporter::btMultiBodyWorldImporter(btMultiBodyDynamicsWorld* world)
|
|
: btBulletWorldImporter(world)
|
|
{
|
|
m_data = new btMultiBodyWorldImporterInternalData;
|
|
m_data->m_mbDynamicsWorld = world;
|
|
}
|
|
|
|
btMultiBodyWorldImporter::~btMultiBodyWorldImporter()
|
|
{
|
|
delete m_data;
|
|
}
|
|
|
|
void btMultiBodyWorldImporter::deleteAllData()
|
|
{
|
|
btBulletWorldImporter::deleteAllData();
|
|
}
|
|
|
|
static btCollisionObjectDoubleData* getBody0FromContactManifold(btPersistentManifoldDoubleData* manifold)
|
|
{
|
|
return (btCollisionObjectDoubleData*)manifold->m_body0;
|
|
}
|
|
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;
|
|
}
|
|
|
|
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)
|
|
{
|
|
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 < 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.deSerialize(mbd->m_baseInertia);
|
|
|
|
btVector3 baseWorldPos;
|
|
baseWorldPos.deSerialize(mbd->m_baseWorldPosition);
|
|
mb->setBasePos(baseWorldPos);
|
|
btQuaternion baseWorldRot;
|
|
baseWorldRot.deSerialize(mbd->m_baseWorldOrientation);
|
|
mb->setWorldToBaseRot(baseWorldRot.inverse());
|
|
btVector3 baseLinVal;
|
|
baseLinVal.deSerialize(mbd->m_baseLinearVelocity);
|
|
btVector3 baseAngVel;
|
|
baseAngVel.deSerialize(mbd->m_baseAngularVelocity);
|
|
mb->setBaseVel(baseLinVal);
|
|
mb->setBaseOmega(baseAngVel);
|
|
|
|
for (int i = 0; i < mbd->m_numLinks; i++)
|
|
{
|
|
mb->getLink(i).m_absFrameTotVelocity.m_topVec.deSerialize(mbd->m_links[i].m_absFrameTotVelocityTop);
|
|
mb->getLink(i).m_absFrameTotVelocity.m_bottomVec.deSerialize(mbd->m_links[i].m_absFrameTotVelocityBottom);
|
|
mb->getLink(i).m_absFrameLocVelocity.m_topVec.deSerialize(mbd->m_links[i].m_absFrameLocVelocityTop);
|
|
mb->getLink(i).m_absFrameLocVelocity.m_bottomVec.deSerialize(mbd->m_links[i].m_absFrameLocVelocityBottom);
|
|
|
|
switch (mbd->m_links[i].m_jointType)
|
|
{
|
|
case btMultibodyLink::eFixed:
|
|
{
|
|
break;
|
|
}
|
|
case btMultibodyLink::ePrismatic:
|
|
{
|
|
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
|
|
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
|
|
break;
|
|
}
|
|
case btMultibodyLink::eRevolute:
|
|
{
|
|
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
|
|
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
|
|
break;
|
|
}
|
|
case btMultibodyLink::eSpherical:
|
|
{
|
|
btScalar jointPos[4] = {(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)mbd->m_links[i].m_jointPos[3]};
|
|
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);
|
|
|
|
break;
|
|
}
|
|
case btMultibodyLink::ePlanar:
|
|
{
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
}
|
|
mb->forwardKinematics(scratchQ, scratchM);
|
|
mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
|
|
}
|
|
|
|
template <class T>
|
|
void convertMultiBody(T* mbd, btMultiBodyWorldImporterInternalData* m_data)
|
|
{
|
|
bool isFixedBase = mbd->m_baseMass == 0;
|
|
bool canSleep = false;
|
|
btVector3 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.deSerialize(mbd->m_baseWorldPosition);
|
|
|
|
btQuaternion baseWorldOrn;
|
|
baseWorldOrn.deSerialize(mbd->m_baseWorldOrientation);
|
|
mb->setBasePos(baseWorldPos);
|
|
mb->setWorldToBaseRot(baseWorldOrn.inverse());
|
|
|
|
m_data->m_mbMap.insert(mbd, mb);
|
|
for (int i = 0; i < mbd->m_numLinks; i++)
|
|
{
|
|
btVector3 localInertiaDiagonal;
|
|
localInertiaDiagonal.deSerialize(mbd->m_links[i].m_linkInertia);
|
|
btQuaternion parentRotToThis;
|
|
parentRotToThis.deSerialize(mbd->m_links[i].m_zeroRotParentToThis);
|
|
btVector3 parentComToThisPivotOffset;
|
|
parentComToThisPivotOffset.deSerialize(mbd->m_links[i].m_parentComToThisPivotOffset);
|
|
btVector3 thisPivotToThisComOffset;
|
|
thisPivotToThisComOffset.deSerialize(mbd->m_links[i].m_thisPivotToThisComOffset);
|
|
|
|
switch (mbd->m_links[i].m_jointType)
|
|
{
|
|
case btMultibodyLink::eFixed:
|
|
{
|
|
mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
|
|
parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset);
|
|
//search for the collider
|
|
//mbd->m_links[i].m_linkCollider
|
|
break;
|
|
}
|
|
case btMultibodyLink::ePrismatic:
|
|
{
|
|
btVector3 jointAxis;
|
|
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);
|
|
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
|
|
mb->finalizeMultiDof();
|
|
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
|
|
break;
|
|
}
|
|
case btMultibodyLink::eRevolute:
|
|
{
|
|
btVector3 jointAxis;
|
|
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);
|
|
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
|
|
mb->finalizeMultiDof();
|
|
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
|
|
break;
|
|
}
|
|
case btMultibodyLink::eSpherical:
|
|
{
|
|
btAssert(0);
|
|
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[4] = {(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)mbd->m_links[i].m_jointPos[3]};
|
|
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->finalizeMultiDof();
|
|
mb->setJointVelMultiDof(i, jointVel);
|
|
|
|
break;
|
|
}
|
|
case btMultibodyLink::ePlanar:
|
|
{
|
|
btAssert(0);
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
btAssert(0);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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()))
|
|
{
|
|
printf("btMultiBodyWorldImporter::convertAllObjects error: expected %d multibodies, got %d.\n", m_data->m_mbDynamicsWorld->getNumMultibodies(), bulletFile2->m_multiBodies.size());
|
|
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);
|
|
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--)
|
|
{
|
|
btRigidBodyDoubleData* rbd = (btRigidBodyDoubleData*)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.deSerializeDouble(rbd->m_collisionObjectData.m_worldTransform);
|
|
rb->setWorldTransform(tr);
|
|
btVector3 linVel, angVel;
|
|
linVel.deSerializeDouble(rbd->m_linearVelocity);
|
|
angVel.deSerializeDouble(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++)
|
|
{
|
|
btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[i];
|
|
{
|
|
void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
|
|
if (ptr)
|
|
{
|
|
manifoldData->m_body0 = (btCollisionObjectDoubleData*)ptr;
|
|
}
|
|
}
|
|
|
|
{
|
|
void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
|
|
if (ptr)
|
|
{
|
|
manifoldData->m_body1 = (btCollisionObjectDoubleData*)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);
|
|
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++)
|
|
{
|
|
btPersistentManifoldFloatData* manifoldData = (btPersistentManifoldFloatData*)bulletFile2->m_contactManifolds[i];
|
|
{
|
|
void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
|
|
if (ptr)
|
|
{
|
|
manifoldData->m_body0 = (btCollisionObjectFloatData*)ptr;
|
|
}
|
|
}
|
|
{
|
|
void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
|
|
if (ptr)
|
|
{
|
|
manifoldData->m_body1 = (btCollisionObjectFloatData*)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);
|
|
}
|
|
}
|
|
|
|
//forward kinematics, so that the link world transforms are valid, for collision detection
|
|
for (int i = 0; i < m_data->m_mbMap.size(); i++)
|
|
{
|
|
btMultiBody** ptr = m_data->m_mbMap.getAtIndex(i);
|
|
if (ptr)
|
|
{
|
|
btMultiBody* mb = *ptr;
|
|
mb->finalizeMultiDof();
|
|
btVector3 linvel = mb->getBaseVel();
|
|
btVector3 angvel = mb->getBaseOmega();
|
|
mb->forwardKinematics(scratchQ, scratchM);
|
|
}
|
|
}
|
|
|
|
//convert all multibody link colliders
|
|
for (int i = 0; i < bulletFile2->m_multiBodyLinkColliders.size(); i++)
|
|
{
|
|
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
|
|
{
|
|
btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*)bulletFile2->m_multiBodyLinkColliders[i];
|
|
|
|
btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody];
|
|
if (ptr)
|
|
{
|
|
btMultiBody* multiBody = *ptr;
|
|
|
|
btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape);
|
|
if (shapePtr && *shapePtr)
|
|
{
|
|
btTransform startTransform;
|
|
mblcd->m_colObjData.m_worldTransform.m_origin.m_floats[3] = 0.f;
|
|
startTransform.deSerializeDouble(mblcd->m_colObjData.m_worldTransform);
|
|
|
|
btCollisionShape* shape = (btCollisionShape*)*shapePtr;
|
|
if (shape)
|
|
{
|
|
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(multiBody, mblcd->m_link);
|
|
col->setCollisionShape(shape);
|
|
//btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name);
|
|
col->setFriction(btScalar(mblcd->m_colObjData.m_friction));
|
|
col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution));
|
|
//m_bodyMap.insert(colObjData,body);
|
|
if (mblcd->m_link == -1)
|
|
{
|
|
col->setWorldTransform(multiBody->getBaseWorldTransform());
|
|
multiBody->setBaseCollider(col);
|
|
}
|
|
else
|
|
{
|
|
col->setWorldTransform(multiBody->getLink(mblcd->m_link).m_cachedWorldTransform);
|
|
multiBody->getLink(mblcd->m_link).m_collider = col;
|
|
}
|
|
int mbLinkIndex = mblcd->m_link;
|
|
|
|
bool isDynamic = (mbLinkIndex < 0 && multiBody->hasFixedBase()) ? false : true;
|
|
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
|
|
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
|
|
|
|
#if 0
|
|
int colGroup = 0, colMask = 0;
|
|
int collisionFlags = mblcd->m_colObjData.m_collisionFlags;
|
|
if (collisionFlags & URDF_HAS_COLLISION_GROUP)
|
|
{
|
|
collisionFilterGroup = colGroup;
|
|
}
|
|
if (collisionFlags & URDF_HAS_COLLISION_MASK)
|
|
{
|
|
collisionFilterMask = colMask;
|
|
}
|
|
#endif
|
|
m_data->m_mbDynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
printf("error: no shape found\n");
|
|
}
|
|
#if 0
|
|
//base and fixed? -> static, otherwise flag as dynamic
|
|
|
|
world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < m_data->m_mbMap.size(); i++)
|
|
{
|
|
btMultiBody** ptr = m_data->m_mbMap.getAtIndex(i);
|
|
if (ptr)
|
|
{
|
|
btMultiBody* mb = *ptr;
|
|
mb->finalizeMultiDof();
|
|
|
|
m_data->m_mbDynamicsWorld->addMultiBody(mb);
|
|
}
|
|
}
|
|
}
|
|
return result;
|
|
} |