mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 13:20:07 +00:00
added some template to restore (syncMultiBody, syncContactManifolds) for single float and double precision, in 'pybullet.restoreState'
This commit is contained in:
Erwin Coumans
parent
20e00d11d8
commit
f104765c47
@ -33,6 +33,230 @@ void btMultiBodyWorldImporter::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++)
|
||||
{
|
||||
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[3] = { (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 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_parentComToThisComOffset);
|
||||
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->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->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[3] = { (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 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:
|
||||
{
|
||||
btAssert(0);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile2)
|
||||
{
|
||||
@ -56,138 +280,77 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
|
||||
{
|
||||
|
||||
//for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
|
||||
for (int i = bulletFile2->m_multiBodies.size()-1; i >=0; i--)
|
||||
for (int i = bulletFile2->m_multiBodies.size() - 1; i >= 0; i--)
|
||||
{
|
||||
btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
|
||||
bool isFixedBase = mbd->m_baseMass == 0;
|
||||
bool canSleep = false;
|
||||
btVector3 baseInertia;
|
||||
baseInertia.deSerializeDouble(mbd->m_baseInertia);
|
||||
|
||||
btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
|
||||
btVector3 baseWorldPos;
|
||||
baseWorldPos.deSerializeDouble(mbd->m_baseWorldPosition);
|
||||
mb->setBasePos(baseWorldPos);
|
||||
btQuaternion baseWorldRot;
|
||||
baseWorldRot.deSerializeDouble(mbd->m_baseWorldOrientation);
|
||||
mb->setWorldToBaseRot(baseWorldRot.inverse());
|
||||
btVector3 baseLinVal;
|
||||
baseLinVal.deSerializeDouble(mbd->m_baseLinearVelocity);
|
||||
btVector3 baseAngVel;
|
||||
baseAngVel.deSerializeDouble(mbd->m_baseAngularVelocity);
|
||||
mb->setBaseVel(baseLinVal);
|
||||
mb->setBaseOmega(baseAngVel);
|
||||
|
||||
for (int i = 0; i < mbd->m_numLinks; i++)
|
||||
{
|
||||
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[3] = { mbd->m_links[i].m_jointPos[0], mbd->m_links[i].m_jointPos[1], mbd->m_links[i].m_jointPos[2] };
|
||||
btScalar jointVel[3] = { mbd->m_links[i].m_jointVel[0], mbd->m_links[i].m_jointVel[1], 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);
|
||||
syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
|
||||
}
|
||||
|
||||
//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_body1);
|
||||
if (ptr)
|
||||
{
|
||||
manifoldData->m_body1 = ptr;
|
||||
btCollisionObjectDoubleData* cdd = (btCollisionObjectDoubleData*)ptr;
|
||||
void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
|
||||
if (ptr)
|
||||
{
|
||||
manifoldData->m_body0 = ptr;
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
|
||||
if (ptr)
|
||||
{
|
||||
manifoldData->m_body1 = ptr;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
btDispatcherInfo& dispatchInfo = m_data->m_mbDynamicsWorld->getDispatchInfo();
|
||||
|
||||
if (bulletFile2->m_contactManifolds.size())
|
||||
{
|
||||
m_data->m_mbDynamicsWorld->updateAabbs();
|
||||
m_data->m_mbDynamicsWorld->computeOverlappingPairs();
|
||||
btDispatcher* dispatcher = m_data->m_mbDynamicsWorld->getDispatcher();
|
||||
|
||||
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 < bulletFile2->m_contactManifolds.size(); q++)
|
||||
{
|
||||
btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[q];
|
||||
btCollisionObjectDoubleData* cdd0 = (btCollisionObjectDoubleData*)manifoldData->m_body0;
|
||||
btCollisionObjectDoubleData* cdd1 = (btCollisionObjectDoubleData*)manifoldData->m_body1;
|
||||
if (uid0 == cdd0->m_uniqueId && uid1 == cdd1->m_uniqueId)
|
||||
{
|
||||
matchingManifoldIndex = q;
|
||||
}
|
||||
}
|
||||
if (matchingManifoldIndex >= 0)
|
||||
{
|
||||
btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[matchingManifoldIndex];
|
||||
existingManifold->deSerializeDouble(manifoldData);
|
||||
}
|
||||
else
|
||||
{
|
||||
existingManifold->setNumContacts(0);
|
||||
//printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1);
|
||||
}
|
||||
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);
|
||||
syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
|
||||
}
|
||||
|
||||
manifoldArray.clear();
|
||||
}
|
||||
}
|
||||
//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 = ptr;
|
||||
}
|
||||
}
|
||||
{
|
||||
void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
|
||||
if (ptr)
|
||||
{
|
||||
manifoldData->m_body1 = ptr;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (bulletFile2->m_contactManifolds.size())
|
||||
{
|
||||
syncContactManifolds((btPersistentManifoldFloatData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
else
|
||||
@ -202,91 +365,12 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
|
||||
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
|
||||
{
|
||||
btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
|
||||
bool isFixedBase = mbd->m_baseMass == 0;
|
||||
bool canSleep = false;
|
||||
btVector3 baseInertia;
|
||||
baseInertia.deSerializeDouble(mbd->m_baseInertia);
|
||||
btMultiBody* mb = new btMultiBody(mbd->m_numLinks, mbd->m_baseMass, baseInertia, isFixedBase, canSleep);
|
||||
mb->setHasSelfCollision(false);
|
||||
|
||||
btVector3 baseWorldPos;
|
||||
baseWorldPos.deSerializeDouble(mbd->m_baseWorldPosition);
|
||||
|
||||
btQuaternion baseWorldOrn;
|
||||
baseWorldOrn.deSerializeDouble(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.deSerializeDouble(mbd->m_links[i].m_linkInertia);
|
||||
btQuaternion parentRotToThis;
|
||||
parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis);
|
||||
btVector3 parentComToThisPivotOffset;
|
||||
parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset);
|
||||
btVector3 thisPivotToThisComOffset;
|
||||
thisPivotToThisComOffset.deSerializeDouble(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.deSerializeDouble(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->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::eRevolute:
|
||||
{
|
||||
btVector3 jointAxis;
|
||||
jointAxis.deSerializeDouble(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->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[3] = { mbd->m_links[i].m_jointPos[0], mbd->m_links[i].m_jointPos[1], mbd->m_links[i].m_jointPos[2] };
|
||||
btScalar jointVel[3] = { mbd->m_links[i].m_jointVel[0], mbd->m_links[i].m_jointVel[1], mbd->m_links[i].m_jointVel[2] };
|
||||
mb->setJointPosMultiDof(i, jointPos);
|
||||
mb->setJointVelMultiDof(i, jointVel);
|
||||
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::ePlanar:
|
||||
{
|
||||
btAssert(0);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
convertMultiBody(mbd, m_data);
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i];
|
||||
convertMultiBody(mbd, m_data);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -361,7 +361,7 @@ const char* btPersistentManifold::serialize(const class btPersistentManifold* ma
|
||||
return btPersistentManifoldDataName;
|
||||
}
|
||||
|
||||
void btPersistentManifold::deSerializeDouble(const struct btPersistentManifoldDoubleData* manifoldDataPtr)
|
||||
void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr)
|
||||
{
|
||||
m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
|
||||
m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
|
||||
@ -405,4 +405,50 @@ void btPersistentManifold::deSerializeDouble(const struct btPersistentManifoldDo
|
||||
pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr)
|
||||
{
|
||||
m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
|
||||
m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
|
||||
m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
|
||||
m_companionIdA = manifoldDataPtr->m_companionIdA;
|
||||
m_companionIdB = manifoldDataPtr->m_companionIdB;
|
||||
//m_index1a = manifoldDataPtr->m_index1a;
|
||||
m_objectType = manifoldDataPtr->m_objectType;
|
||||
|
||||
for (int i = 0; i < this->getNumContacts(); i++)
|
||||
{
|
||||
btManifoldPoint& pt = m_pointCache[i];
|
||||
|
||||
pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
|
||||
pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
|
||||
pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
|
||||
pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);
|
||||
pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]);
|
||||
pt.m_normalWorldOnB.deSerialize(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]);
|
||||
pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
|
||||
pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i];
|
||||
pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i];
|
||||
pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
|
||||
pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
|
||||
pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
|
||||
pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
|
||||
pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
|
||||
pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
|
||||
pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
|
||||
pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
|
||||
pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
|
||||
pt.m_positionWorldOnA.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
|
||||
pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
|
||||
pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
|
||||
pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
|
||||
pt.m_lateralFrictionDir2.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]);
|
||||
pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i];
|
||||
pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i];
|
||||
pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i];
|
||||
pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
|
||||
pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
|
||||
}
|
||||
|
||||
}
|
||||
@ -261,7 +261,8 @@ public:
|
||||
|
||||
int calculateSerializeBufferSize() const;
|
||||
const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const;
|
||||
void deSerializeDouble(const struct btPersistentManifoldDoubleData* manifoldDataPtr);
|
||||
void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr);
|
||||
void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr);
|
||||
|
||||
|
||||
};
|
||||
@ -321,8 +322,8 @@ struct btPersistentManifoldFloatData
|
||||
btVector3FloatData m_pointCachePositionWorldOnA[4];
|
||||
btVector3FloatData m_pointCachePositionWorldOnB[4];
|
||||
btVector3FloatData m_pointCacheNormalWorldOnB[4];
|
||||
btVector3FloatData m_pointCacheLateralFrictionDir1;
|
||||
btVector3FloatData m_pointCacheLateralFrictionDir2;
|
||||
btVector3FloatData m_pointCacheLateralFrictionDir1[4];
|
||||
btVector3FloatData m_pointCacheLateralFrictionDir2[4];
|
||||
float m_pointCacheDistance[4];
|
||||
float m_pointCacheAppliedImpulse[4];
|
||||
float m_pointCacheCombinedFriction[4];
|
||||
|
||||
@ -602,7 +602,9 @@ public:
|
||||
|
||||
SIMD_FORCE_INLINE void serialize(struct btQuaternionData& dataOut) const;
|
||||
|
||||
SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionData& dataIn);
|
||||
SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionFloatData& dataIn);
|
||||
|
||||
SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionDoubleData& dataIn);
|
||||
|
||||
SIMD_FORCE_INLINE void serializeFloat(struct btQuaternionFloatData& dataOut) const;
|
||||
|
||||
@ -1003,10 +1005,16 @@ SIMD_FORCE_INLINE void btQuaternion::serialize(struct btQuaternionData& dataOut)
|
||||
dataOut.m_floats[i] = m_floats[i];
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionData& dataIn)
|
||||
SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionFloatData& dataIn)
|
||||
{
|
||||
for (int i = 0; i<4; i++)
|
||||
m_floats[i] = (btScalar)dataIn.m_floats[i];
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionDoubleData& dataIn)
|
||||
{
|
||||
for (int i=0;i<4;i++)
|
||||
m_floats[i] = dataIn.m_floats[i];
|
||||
m_floats[i] = (btScalar)dataIn.m_floats[i];
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -705,7 +705,9 @@ public:
|
||||
|
||||
SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const;
|
||||
|
||||
SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn);
|
||||
SIMD_FORCE_INLINE void deSerialize(const struct btVector3DoubleData& dataIn);
|
||||
|
||||
SIMD_FORCE_INLINE void deSerialize(const struct btVector3FloatData& dataIn);
|
||||
|
||||
SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData& dataOut) const;
|
||||
|
||||
@ -1354,10 +1356,18 @@ SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const
|
||||
dataOut.m_floats[i] = m_floats[i];
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn)
|
||||
|
||||
SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3FloatData& dataIn)
|
||||
{
|
||||
for (int i = 0; i<4; i++)
|
||||
m_floats[i] = (btScalar)dataIn.m_floats[i];
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3DoubleData& dataIn)
|
||||
{
|
||||
for (int i=0;i<4;i++)
|
||||
m_floats[i] = dataIn.m_floats[i];
|
||||
m_floats[i] = (btScalar)dataIn.m_floats[i];
|
||||
}
|
||||
|
||||
#endif //BT_VECTOR3_H
|
||||
|
||||
Loading…
Reference in New Issue
Block a user