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

Merge pull request #1491 from erwincoumans/master

Browse Source 
some progress in saving and restoring the state
This commit is contained in:
erwincoumans
2017-12-31 13:39:27 -08:00
committed by GitHub
parent e3a85493e8 f104765c47
commit 515aa9ee9b
26 changed files with 2215 additions and 1222 deletions
Download Patch File Download Diff File Expand all files Collapse all files

121
Extras/Serialize/BulletFileLoader/autogenerated/bullet.h
View File

@@ -63,6 +63,8 @@ typedef struct bInvalidHandle {
class btCapsuleShapeData;
class btTriangleInfoData;
class btTriangleInfoMapData;
class btPersistentManifoldDoubleData;
class btPersistentManifoldFloatData;
class btGImpactMeshShapeData;
class btConvexHullShapeData;
class btCollisionObjectDoubleData;
@@ -514,6 +516,96 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btPersistentManifoldDoubleData
{
public:
btVector3DoubleData m_pointCacheLocalPointA[4];
btVector3DoubleData m_pointCacheLocalPointB[4];
btVector3DoubleData m_pointCachePositionWorldOnA[4];
btVector3DoubleData m_pointCachePositionWorldOnB[4];
btVector3DoubleData m_pointCacheNormalWorldOnB[4];
btVector3DoubleData m_pointCacheLateralFrictionDir1[4];
btVector3DoubleData m_pointCacheLateralFrictionDir2[4];
double m_pointCacheDistance[4];
double m_pointCacheAppliedImpulse[4];
double m_pointCacheCombinedFriction[4];
double m_pointCacheCombinedRollingFriction[4];
double m_pointCacheCombinedSpinningFriction[4];
double m_pointCacheCombinedRestitution[4];
int m_pointCachePartId0[4];
int m_pointCachePartId1[4];
int m_pointCacheIndex0[4];
int m_pointCacheIndex1[4];
int m_pointCacheContactPointFlags[4];
double m_pointCacheAppliedImpulseLateral1[4];
double m_pointCacheAppliedImpulseLateral2[4];
double m_pointCacheContactMotion1[4];
double m_pointCacheContactMotion2[4];
double m_pointCacheContactCFM[4];
double m_pointCacheCombinedContactStiffness1[4];
double m_pointCacheContactERP[4];
double m_pointCacheCombinedContactDamping1[4];
double m_pointCacheFrictionCFM[4];
int m_pointCacheLifeTime[4];
int m_numCachedPoints;
int m_companionIdA;
int m_companionIdB;
int m_index1a;
int m_objectType;
double m_contactBreakingThreshold;
double m_contactProcessingThreshold;
int m_padding;
void *m_body0;
void *m_body1;
};
// -------------------------------------------------- //
class btPersistentManifoldFloatData
{
public:
btVector3FloatData m_pointCacheLocalPointA[4];
btVector3FloatData m_pointCacheLocalPointB[4];
btVector3FloatData m_pointCachePositionWorldOnA[4];
btVector3FloatData m_pointCachePositionWorldOnB[4];
btVector3FloatData m_pointCacheNormalWorldOnB[4];
btVector3FloatData m_pointCacheLateralFrictionDir1;
btVector3FloatData m_pointCacheLateralFrictionDir2;
float m_pointCacheDistance[4];
float m_pointCacheAppliedImpulse[4];
float m_pointCacheCombinedFriction[4];
float m_pointCacheCombinedRollingFriction[4];
float m_pointCacheCombinedSpinningFriction[4];
float m_pointCacheCombinedRestitution[4];
int m_pointCachePartId0[4];
int m_pointCachePartId1[4];
int m_pointCacheIndex0[4];
int m_pointCacheIndex1[4];
int m_pointCacheContactPointFlags[4];
float m_pointCacheAppliedImpulseLateral1[4];
float m_pointCacheAppliedImpulseLateral2[4];
float m_pointCacheContactMotion1[4];
float m_pointCacheContactMotion2[4];
float m_pointCacheContactCFM[4];
float m_pointCacheCombinedContactStiffness1[4];
float m_pointCacheContactERP[4];
float m_pointCacheCombinedContactDamping1[4];
float m_pointCacheFrictionCFM[4];
int m_pointCacheLifeTime[4];
int m_numCachedPoints;
int m_companionIdA;
int m_companionIdB;
int m_index1a;
int m_objectType;
float m_contactBreakingThreshold;
float m_contactProcessingThreshold;
int m_padding;
void *m_body0;
void *m_body1;
};
// -------------------------------------------------- //
class btGImpactMeshShapeData
{
@@ -568,7 +660,9 @@ typedef struct bInvalidHandle {
int m_activationState1;
int m_internalType;
int m_checkCollideWith;
char m_padding[4];
int m_collisionFilterGroup;
int m_collisionFilterMask;
int m_uniqueId;
};
@@ -602,7 +696,9 @@ typedef struct bInvalidHandle {
int m_activationState1;
int m_internalType;
int m_checkCollideWith;
char m_padding[4];
int m_collisionFilterGroup;
int m_collisionFilterMask;
int m_uniqueId;
};
@@ -1416,15 +1512,17 @@ typedef struct bInvalidHandle {
class btMultiBodyDoubleData
{
public:
btTransformDoubleData m_baseWorldTransform;
btVector3DoubleData m_baseWorldPosition;
btQuaternionDoubleData m_baseWorldOrientation;
btVector3DoubleData m_baseLinearVelocity;
btVector3DoubleData m_baseAngularVelocity;
btVector3DoubleData m_baseInertia;
double m_baseMass;
int m_numLinks;
char m_padding[4];
char *m_baseName;
btMultiBodyLinkDoubleData *m_links;
btCollisionObjectDoubleData *m_baseCollider;
char *m_paddingPtr;
int m_numLinks;
char m_padding[4];
};
@@ -1432,13 +1530,16 @@ typedef struct bInvalidHandle {
class btMultiBodyFloatData
{
public:
char *m_baseName;
btMultiBodyLinkFloatData *m_links;
btCollisionObjectFloatData *m_baseCollider;
btTransformFloatData m_baseWorldTransform;
btVector3FloatData m_baseWorldPosition;
btQuaternionFloatData m_baseWorldOrientation;
btVector3FloatData m_baseLinearVelocity;
btVector3FloatData m_baseAngularVelocity;
btVector3FloatData m_baseInertia;
float m_baseMass;
int m_numLinks;
char *m_baseName;
btMultiBodyLinkFloatData *m_links;
btCollisionObjectFloatData *m_baseCollider;
};

4
Extras/Serialize/BulletFileLoader/btBulletFile.cpp
View File

@@ -176,6 +176,10 @@ void btBulletFile::parseData()
// listID->push_back((bStructHandle*)id);
}
if (dataChunk.code == BT_CONTACTMANIFOLD_CODE)
{
m_contactManifolds.push_back((bStructHandle*)id);
}
if (dataChunk.code == BT_MULTIBODY_CODE)
{
m_multiBodies.push_back((bStructHandle*)id);

2
Extras/Serialize/BulletFileLoader/btBulletFile.h
View File

@@ -59,6 +59,8 @@ namespace bParse {
btAlignedObjectArray<bStructHandle*> m_dynamicsWorldInfo;
btAlignedObjectArray<bStructHandle*> m_contactManifolds;
btAlignedObjectArray<char*> m_dataBlocks;
btBulletFile();

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

@@ -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)
{
@@ -52,28 +276,81 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
result = true;
//convert all multibodies
for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
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];
bool isFixedBase = mbd->m_baseMass == 0;
bool canSleep = false;
btVector3 baseInertia;
baseInertia.deSerializeDouble(mbd->m_baseInertia);
btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
mb->setBaseWorldTransform(tr);
for (int i = 0; i < mbd->m_numLinks; i++)
{
}
btVector3 linvel = mb->getBaseVel();
btVector3 angvel = mb->getBaseOmega();
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_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((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);
}
//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
@@ -88,85 +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);
btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
mb->setBaseWorldTransform(tr);
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);
}
}

2
Extras/Serialize/makesdna/makesdna.cpp
View File

@@ -132,6 +132,7 @@ typedef unsigned __int64 uint64_t;
#include "BulletCollision/CollisionShapes/btConeShape.h"
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btTriangleInfoMap.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "BulletCollision/Gimpact/btGImpactShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
@@ -181,6 +182,7 @@ char *includefiles[] = {
"../../../src/BulletCollision/CollisionShapes/btConeShape.h",
"../../../src/BulletCollision/CollisionShapes/btCapsuleShape.h",
"../../../src/BulletCollision/CollisionShapes/btTriangleInfoMap.h",
"../../../src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h",
"../../../src/BulletCollision/Gimpact/btGImpactShape.h",
"../../../src/BulletCollision/CollisionShapes/btConvexHullShape.h",
"../../../src/BulletCollision/CollisionDispatch/btCollisionObject.h",

29
data/planeMesh.urdf Normal file
View File

@@ -0,0 +1,29 @@
<?xml version="0.0" ?>
<robot name="plane">
<link name="planeLink">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="plane.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="plane.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
</robot>

102
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2172,9 +2172,12 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_pairCache = new btHashedOverlappingPairCache();
m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback);
//int maxProxies = 32768;
//m_data->m_broadphase = new btSimpleBroadphase(maxProxies, m_data->m_pairCache);
m_data->m_broadphase = new btDbvtBroadphase(m_data->m_pairCache);
m_data->m_solver = new btMultiBodyConstraintSolver;
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
@@ -2183,7 +2186,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#endif
//Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
//Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(32768);
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
@@ -3773,29 +3776,78 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
char pathPrefix[1024];
pathPrefix[0] = 0;
if (visualShape.m_geometry.m_type == URDF_GEOM_MESH)
{
std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName;
const std::string& error_message_prefix="";
std::string out_found_filename;
int out_type;
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type);
visualShape.m_geometry.m_meshFileType = out_type;
visualShape.m_geometry.m_meshFileName=fileName;
const b3CreateUserShapeData& visShape = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex];
visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]);
}
visualShape.m_name = "bla";
switch (visualShape.m_geometry.m_type)
{
case URDF_GEOM_CYLINDER:
{
visualShape.m_geometry.m_capsuleHeight = visShape.m_capsuleHeight;
visualShape.m_geometry.m_capsuleRadius = visShape.m_capsuleRadius;
break;
}
case URDF_GEOM_BOX:
{
visualShape.m_geometry.m_boxSize.setValue(2.*visShape.m_boxHalfExtents[0],
2.*visShape.m_boxHalfExtents[1],
2.*visShape.m_boxHalfExtents[2]);
break;
}
case URDF_GEOM_SPHERE:
{
visualShape.m_geometry.m_sphereRadius = visShape.m_sphereRadius;
break;
}
case URDF_GEOM_CAPSULE:
{
visualShape.m_geometry.m_hasFromTo = visShape.m_hasFromTo;
if (visualShape.m_geometry.m_hasFromTo)
{
visualShape.m_geometry.m_capsuleFrom.setValue(visShape.m_capsuleFrom[0],
visShape.m_capsuleFrom[1],
visShape.m_capsuleFrom[2]);
visualShape.m_geometry.m_capsuleTo.setValue(visShape.m_capsuleTo[0],
visShape.m_capsuleTo[1],
visShape.m_capsuleTo[2]);
}
else
{
visualShape.m_geometry.m_capsuleHeight = visShape.m_capsuleHeight;
visualShape.m_geometry.m_capsuleRadius = visShape.m_capsuleRadius;
}
break;
}
case URDF_GEOM_MESH:
{
std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName;
const std::string& error_message_prefix = "";
std::string out_found_filename;
int out_type;
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
visualShape.m_geometry.m_meshFileType = out_type;
visualShape.m_geometry.m_meshFileName = fileName;
visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]);
break;
}
default:
{
}
};
visualShape.m_name = "in_memory";
visualShape.m_materialName="";
visualShape.m_sourceFileLocation="blaat_line_10";
visualShape.m_sourceFileLocation="in_memory_unknown_line";
visualShape.m_linkLocalFrame.setIdentity();
visualShape.m_geometry.m_hasLocalMaterial = false;
@@ -3845,7 +3897,6 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
}
u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
if (vertices.size() && indices.size())
@@ -8305,6 +8356,9 @@ bool PhysicsServerCommandProcessor::processSaveBulletCommand(const struct Shared
if (f)
{
btDefaultSerializer* ser = new btDefaultSerializer();
int currentFlags = ser->getSerializationFlags();
ser->setSerializationFlags(currentFlags | BT_SERIALIZE_CONTACT_MANIFOLDS);
m_data->m_dynamicsWorld->serialize(ser);
fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f);
fclose(f);

130
examples/pybullet/examples/saveRestoreState.py
View File

@@ -1,30 +1,118 @@
import pybullet as p
import math, time
import difflib,sys
numSteps = 500
numSteps2 = 30
p.connect(p.GUI, options="--width=1024 --height=768")
numObjects = 50
verbose = 0
p.loadURDF("plane.urdf")
for i in range (10):
cube = p.loadURDF("cube_small.urdf",0,i*0.01,i*0.5)
p.setGravity(0,0,-10)
def setupWorld():
p.resetSimulation()
p.loadURDF("planeMesh.urdf")
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
for i in range (p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
for i in range (numObjects):
cube = p.loadURDF("cube_small.urdf",0,i*0.02,(i+1)*0.2)
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0,0,-10)
#for i in range (500):
# p.stepSimulation()
saveState = 1
if saveState:
for i in range (500):
p.stepSimulation()
p.saveBullet("state.bullet")
else:
p.restoreState(fileName="state.bullet")
for i in range (10):
print("pos[",i,"]=",p.getBasePositionAndOrientation(i))
def dumpStateToFile(file):
for i in range (p.getNumBodies()):
pos,orn = p.getBasePositionAndOrientation(i)
linVel,angVel = p.getBaseVelocity(i)
txtPos = "pos="+str(pos)+"\n"
txtOrn = "orn="+str(orn)+"\n"
txtLinVel = "linVel"+str(linVel)+"\n"
txtAngVel = "angVel"+str(angVel)+"\n"
file.write(txtPos)
file.write(txtOrn)
file.write(txtLinVel)
file.write(txtAngVel)
def compareFiles(file1,file2):
diff = difflib.unified_diff(
file1.readlines(),
file2.readlines(),
fromfile='saveFile.txt',
tofile='restoreFile.txt',
)
for line in diff:
sys.stdout.write(line)
setupWorld()
for i in range (numSteps):
p.stepSimulation()
p.saveBullet("state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points=")
for q in p.getContactPoints():
print(q)
for i in range (numSteps2):
p.stepSimulation()
file = open("saveFile.txt","w")
dumpStateToFile(file)
file.close()
#################################
setupWorld()
p.restoreState(fileName="state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range (numSteps2):
p.stepSimulation()
file = open("restoreFile.txt","w")
dumpStateToFile(file)
file.close()
p.restoreState(fileName="state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range (numSteps2):
p.stepSimulation()
file = open("restoreFile2.txt","w")
dumpStateToFile(file)
file.close()
file1 = open("saveFile.txt","r")
file2 = open("restoreFile.txt","r")
compareFiles(file1,file2)
file1.close()
file2.close()
file1 = open("saveFile.txt","r")
file2 = open("restoreFile2.txt","r")
compareFiles(file1,file2)
file1.close()
file2.close()
while (p.getConnectionInfo()["isConnected"]):
time.sleep(1)
time.sleep(1)

4
examples/pybullet/pybullet.c
View File

@@ -7876,8 +7876,8 @@ static PyMethodDef SpamMethods[] = {
{ "restoreState", (PyCFunction)pybullet_restoreState, METH_VARARGS | METH_KEYWORDS,
"Restore the full state of an existing world." },
// { "saveState", (PyCFunction)pybullet_saveState, METH_VARARGS | METH_KEYWORDS,
// "Save the full state of the world to memory." },
{ "saveState", (PyCFunction)pybullet_saveState, METH_VARARGS | METH_KEYWORDS,
"Save the full state of the world to memory." },
{"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS,
"Load multibodies from an MJCF file."},

17
src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
View File

@@ -16,6 +16,7 @@ subject to the following restrictions:
#include "btCollisionObject.h"
#include "LinearMath/btSerializer.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
btCollisionObject::btCollisionObject()
: m_interpolationLinearVelocity(0.f, 0.f, 0.f),
@@ -114,10 +115,18 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius;
dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
dataOut->m_checkCollideWith = m_checkCollideWith;
// Fill padding with zeros to appease msan.
memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
if (m_broadphaseHandle)
{
dataOut->m_collisionFilterGroup = m_broadphaseHandle->m_collisionFilterGroup;
dataOut->m_collisionFilterMask = m_broadphaseHandle->m_collisionFilterMask;
dataOut->m_uniqueId = m_broadphaseHandle->m_uniqueId;
}
else
{
dataOut->m_collisionFilterGroup = 0;
dataOut->m_collisionFilterMask = 0;
dataOut->m_uniqueId = -1;
}
return btCollisionObjectDataName;
}

13
src/BulletCollision/CollisionDispatch/btCollisionObject.h
View File

@@ -621,7 +621,6 @@ struct btCollisionObjectDoubleData
double m_hitFraction;
double m_ccdSweptSphereRadius;
double m_ccdMotionThreshold;
int m_hasAnisotropicFriction;
int m_collisionFlags;
int m_islandTag1;
@@ -629,8 +628,9 @@ struct btCollisionObjectDoubleData
int m_activationState1;
int m_internalType;
int m_checkCollideWith;
char m_padding[4];
int m_collisionFilterGroup;
int m_collisionFilterMask;
int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
@@ -650,13 +650,12 @@ struct btCollisionObjectFloatData
float m_deactivationTime;
float m_friction;
float m_rollingFriction;
float m_contactDamping;
float m_contactDamping;
float m_contactStiffness;
float m_restitution;
float m_hitFraction;
float m_ccdSweptSphereRadius;
float m_ccdMotionThreshold;
int m_hasAnisotropicFriction;
int m_collisionFlags;
int m_islandTag1;
@@ -664,7 +663,9 @@ struct btCollisionObjectFloatData
int m_activationState1;
int m_internalType;
int m_checkCollideWith;
char m_padding[4];
int m_collisionFilterGroup;
int m_collisionFilterMask;
int m_uniqueId;
};

24
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
View File

@@ -1654,12 +1654,36 @@ void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
}
void btCollisionWorld::serializeContactManifolds(btSerializer* serializer)
{
if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS)
{
int numManifolds = getDispatcher()->getNumManifolds();
for (int i = 0; i < numManifolds; i++)
{
const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i];
//don't serialize empty manifolds, they just take space
//(may have to do it anyway if it destroys determinism)
//if (manifold->getNumContacts() == 0)
// continue;
btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1);
const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk, structType, BT_CONTACTMANIFOLD_CODE, (void*)manifold);
}
}
}
void btCollisionWorld::serialize(btSerializer* serializer)
{
serializer->startSerialization();
serializeCollisionObjects(serializer);
serializeContactManifolds(serializer);
serializer->finishSerialization();
}

3
src/BulletCollision/CollisionDispatch/btCollisionWorld.h
View File

@@ -107,6 +107,9 @@ protected:
void serializeCollisionObjects(btSerializer* serializer);
void serializeContactManifolds(btSerializer* serializer);
public:
//this constructor doesn't own the dispatcher and paircache/broadphase

28
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
View File

@@ -199,6 +199,22 @@ class btPersistentManifoldSortPredicate
}
};
class btPersistentManifoldSortPredicateDeterministic
{
public:
SIMD_FORCE_INLINE bool operator() (const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
{
return (
(getIslandId(lhs) < getIslandId(rhs))
|| ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId)
||((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) &&
(lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId))
);
}
};
void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
{
@@ -318,7 +334,9 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
for (i=0;i<maxNumManifolds ;i++)
{
btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
if (manifold->getNumContacts() == 0)
continue;
const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
@@ -379,7 +397,13 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
//tried a radix sort, but quicksort/heapsort seems still faster
//@todo rewrite island management
m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
//m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
//btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid,
//but also based on object0 unique id and object1 unique id
m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic());
//m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
//now process all active islands (sets of manifolds for now)

146
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
View File

@@ -16,6 +16,7 @@ subject to the following restrictions:
#include "btPersistentManifold.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btSerializer.h"
btScalar gContactBreakingThreshold = btScalar(0.02);
@@ -33,6 +34,8 @@ btPersistentManifold::btPersistentManifold()
m_body0(0),
m_body1(0),
m_cachedPoints (0),
m_companionIdA(0),
m_companionIdB(0),
m_index1a(0)
{
}
@@ -303,6 +306,149 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT
}
int btPersistentManifold::calculateSerializeBufferSize() const
{
return sizeof(btPersistentManifoldData);
}
const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const
{
btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer;
memset(dataOut, 0, sizeof(btPersistentManifoldData));
dataOut->m_body0 = serializer->getUniquePointer((void*)manifold->getBody0());
dataOut->m_body1 = serializer->getUniquePointer((void*)manifold->getBody1());
dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold();
dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold();
dataOut->m_numCachedPoints = manifold->getNumContacts();
dataOut->m_companionIdA = manifold->m_companionIdA;
dataOut->m_companionIdB = manifold->m_companionIdB;
dataOut->m_index1a = manifold->m_index1a;
dataOut->m_objectType = manifold->m_objectType;
for (int i = 0; i < this->getNumContacts(); i++)
{
const btManifoldPoint& pt = manifold->getContactPoint(i);
dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]);
pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]);
dataOut->m_pointCacheDistance[i] = pt.m_distance1;
dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1;
dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1;
dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime;
dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM;
dataOut->m_pointCacheContactERP[i] = pt.m_contactERP;
dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM;
dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags;
dataOut->m_pointCacheIndex0[i] = pt.m_index0;
dataOut->m_pointCacheIndex1[i] = pt.m_index1;
dataOut->m_pointCachePartId0[i] = pt.m_partId0;
dataOut->m_pointCachePartId1[i] = pt.m_partId1;
pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]);
pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]);
dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction;
pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]);
pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]);
dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction;
dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction;
dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution;
dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1;
dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2;
}
return btPersistentManifoldDataName;
}
void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* 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.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]);
pt.m_localPointB.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointB[i]);
pt.m_normalWorldOnB.deSerializeDouble(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.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
pt.m_positionWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
pt.m_lateralFrictionDir1.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
pt.m_lateralFrictionDir2.deSerializeDouble(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];
}
}
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];
}
}

114
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
View File

@@ -95,7 +95,10 @@ public:
: btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
m_body0(body0),m_body1(body1),m_cachedPoints(0),
m_contactBreakingThreshold(contactBreakingThreshold),
m_contactProcessingThreshold(contactProcessingThreshold)
m_contactProcessingThreshold(contactProcessingThreshold),
m_companionIdA(0),
m_companionIdB(0),
m_index1a(0)
{
}
@@ -256,10 +259,115 @@ public:
m_cachedPoints = 0;
}
int calculateSerializeBufferSize() const;
const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const;
void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr);
void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr);
}
;
};
struct btPersistentManifoldDoubleData
{
btVector3DoubleData m_pointCacheLocalPointA[4];
btVector3DoubleData m_pointCacheLocalPointB[4];
btVector3DoubleData m_pointCachePositionWorldOnA[4];
btVector3DoubleData m_pointCachePositionWorldOnB[4];
btVector3DoubleData m_pointCacheNormalWorldOnB[4];
btVector3DoubleData m_pointCacheLateralFrictionDir1[4];
btVector3DoubleData m_pointCacheLateralFrictionDir2[4];
double m_pointCacheDistance[4];
double m_pointCacheAppliedImpulse[4];
double m_pointCacheCombinedFriction[4];
double m_pointCacheCombinedRollingFriction[4];
double m_pointCacheCombinedSpinningFriction[4];
double m_pointCacheCombinedRestitution[4];
int m_pointCachePartId0[4];
int m_pointCachePartId1[4];
int m_pointCacheIndex0[4];
int m_pointCacheIndex1[4];
int m_pointCacheContactPointFlags[4];
double m_pointCacheAppliedImpulseLateral1[4];
double m_pointCacheAppliedImpulseLateral2[4];
double m_pointCacheContactMotion1[4];
double m_pointCacheContactMotion2[4];
double m_pointCacheContactCFM[4];
double m_pointCacheCombinedContactStiffness1[4];
double m_pointCacheContactERP[4];
double m_pointCacheCombinedContactDamping1[4];
double m_pointCacheFrictionCFM[4];
int m_pointCacheLifeTime[4];
int m_numCachedPoints;
int m_companionIdA;
int m_companionIdB;
int m_index1a;
int m_objectType;
double m_contactBreakingThreshold;
double m_contactProcessingThreshold;
int m_padding;
void *m_body0;
void *m_body1;
};
struct btPersistentManifoldFloatData
{
btVector3FloatData m_pointCacheLocalPointA[4];
btVector3FloatData m_pointCacheLocalPointB[4];
btVector3FloatData m_pointCachePositionWorldOnA[4];
btVector3FloatData m_pointCachePositionWorldOnB[4];
btVector3FloatData m_pointCacheNormalWorldOnB[4];
btVector3FloatData m_pointCacheLateralFrictionDir1[4];
btVector3FloatData m_pointCacheLateralFrictionDir2[4];
float m_pointCacheDistance[4];
float m_pointCacheAppliedImpulse[4];
float m_pointCacheCombinedFriction[4];
float m_pointCacheCombinedRollingFriction[4];
float m_pointCacheCombinedSpinningFriction[4];
float m_pointCacheCombinedRestitution[4];
int m_pointCachePartId0[4];
int m_pointCachePartId1[4];
int m_pointCacheIndex0[4];
int m_pointCacheIndex1[4];
int m_pointCacheContactPointFlags[4];
float m_pointCacheAppliedImpulseLateral1[4];
float m_pointCacheAppliedImpulseLateral2[4];
float m_pointCacheContactMotion1[4];
float m_pointCacheContactMotion2[4];
float m_pointCacheContactCFM[4];
float m_pointCacheCombinedContactStiffness1[4];
float m_pointCacheContactERP[4];
float m_pointCacheCombinedContactDamping1[4];
float m_pointCacheFrictionCFM[4];
int m_pointCacheLifeTime[4];
int m_numCachedPoints;
int m_companionIdA;
int m_companionIdB;
int m_index1a;
int m_objectType;
float m_contactBreakingThreshold;
float m_contactProcessingThreshold;
int m_padding;
void *m_body0;
void *m_body1;
};
#ifdef BT_USE_DOUBLE_PRECISION
#define btPersistentManifoldData btPersistentManifoldDoubleData
#define btPersistentManifoldDataName "btPersistentManifoldDoubleData"
#else
#define btPersistentManifoldData btPersistentManifoldFloatData
#define btPersistentManifoldDataName "btPersistentManifoldFloatData"
#endif //BT_USE_DOUBLE_PRECISION

2
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -1533,6 +1533,8 @@ void btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
serializeRigidBodies(serializer);
serializeContactManifolds(serializer);
serializer->finishSerialization();
}

6
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -1994,7 +1994,11 @@ int btMultiBody::calculateSerializeBufferSize() const
const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
{
btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
getBasePos().serialize(mbd->m_baseWorldPosition);
getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
getBaseVel().serialize(mbd->m_baseLinearVelocity);
getBaseOmega().serialize(mbd->m_baseAngularVelocity);
mbd->m_baseMass = this->getBaseMass();
getBaseInertia().serialize(mbd->m_baseInertia);
{

33
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -702,13 +702,13 @@ private:
int m_companionId;
btScalar m_linearDamping;
btScalar m_angularDamping;
bool m_useGyroTerm;
bool m_useGyroTerm;
btScalar m_maxAppliedImpulse;
btScalar m_maxCoordinateVelocity;
bool m_hasSelfCollision;
bool __posUpdated;
int m_dofCount, m_posVarCnt;
bool __posUpdated;
int m_dofCount, m_posVarCnt;
bool m_useRK4, m_useGlobalVelocities;
///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
@@ -784,29 +784,38 @@ struct btMultiBodyLinkFloatData
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btMultiBodyDoubleData
{
btTransformDoubleData m_baseWorldTransform;
btVector3DoubleData m_baseWorldPosition;
btQuaternionDoubleData m_baseWorldOrientation;
btVector3DoubleData m_baseLinearVelocity;
btVector3DoubleData m_baseAngularVelocity;
btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal)
double m_baseMass;
int m_numLinks;
char m_padding[4];
char *m_baseName;
btMultiBodyLinkDoubleData *m_links;
btCollisionObjectDoubleData *m_baseCollider;
char *m_paddingPtr;
int m_numLinks;
char m_padding[4];
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btMultiBodyFloatData
{
btVector3FloatData m_baseWorldPosition;
btQuaternionFloatData m_baseWorldOrientation;
btVector3FloatData m_baseLinearVelocity;
btVector3FloatData m_baseAngularVelocity;
btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal)
float m_baseMass;
int m_numLinks;
char *m_baseName;
btMultiBodyLinkFloatData *m_links;
btCollisionObjectFloatData *m_baseCollider;
btTransformFloatData m_baseWorldTransform;
btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal)
float m_baseMass;
int m_numLinks;
};

2
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -972,6 +972,8 @@ void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
serializeCollisionObjects(serializer);
serializeContactManifolds(serializer);
serializer->finishSerialization();
}

2
src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp
View File

@@ -361,6 +361,8 @@ void btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
serializeCollisionObjects(serializer);
serializeContactManifolds(serializer);
serializer->finishSerialization();
}

14
src/LinearMath/btQuaternion.h
View File

@@ -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];
}

1112
src/LinearMath/btSerializer.cpp
View File

File diff suppressed because it is too large Load Diff

5
src/LinearMath/btSerializer.h
View File

@@ -62,7 +62,8 @@ enum btSerializationFlags
{
BT_SERIALIZE_NO_BVH = 1,
BT_SERIALIZE_NO_TRIANGLEINFOMAP = 2,
BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4
BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4,
BT_SERIALIZE_CONTACT_MANIFOLDS = 8,
};
class btSerializer
@@ -128,9 +129,9 @@ public:
#define BT_SBMATERIAL_CODE BT_MAKE_ID('S','B','M','T')
#define BT_SBNODE_CODE BT_MAKE_ID('S','B','N','D')
#define BT_DYNAMICSWORLD_CODE BT_MAKE_ID('D','W','L','D')
#define BT_CONTACTMANIFOLD_CODE BT_MAKE_ID('C','O','N','T')
#define BT_DNA_CODE BT_MAKE_ID('D','N','A','1')
struct btPointerUid
{
union

1112
src/LinearMath/btSerializer64.cpp
View File

File diff suppressed because it is too large Load Diff

16
src/LinearMath/btVector3.h
View File

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