made some progress in saving and restoring the state during the simulation, with identical results.

Option to de/serialize btPersistentContactManifolds and fix lossy conversion during btMultiBody de/serialization of base world transform
(serialize the quaternion, not the converted 3x3 matrix)
There are still several caches not taken into account, and btMultiBody links/constraints are not deserialized yet etc.
See examples\pybullet\examples\saveRestoreState.py for an example.
This commit is contained in:
Erwin Coumans
2017-12-30 14:19:13 -08:00
parent 29cfac096b
commit 0326fa93a8
22 changed files with 1840 additions and 1105 deletions

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

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

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();
}

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

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)

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,103 @@ 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::deSerializeDouble(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];
}
}

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,114 @@ public:
m_cachedPoints = 0;
}
int calculateSerializeBufferSize() const;
const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const;
void deSerializeDouble(const struct btPersistentManifoldDoubleData* 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;
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;
};
#ifdef BT_USE_DOUBLE_PRECISION
#define btPersistentManifoldData btPersistentManifoldDoubleData
#define btPersistentManifoldDataName "btPersistentManifoldDoubleData"
#else
#define btPersistentManifoldData btPersistentManifoldFloatData
#define btPersistentManifoldDataName "btPersistentManifoldFloatData"
#endif //BT_USE_DOUBLE_PRECISION

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

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

View File

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff