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

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

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files

100
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,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];
}
}

113
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,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