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:
@@ -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;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -59,6 +59,8 @@ namespace bParse {
|
||||
|
||||
btAlignedObjectArray<bStructHandle*> m_dynamicsWorldInfo;
|
||||
|
||||
btAlignedObjectArray<bStructHandle*> m_contactManifolds;
|
||||
|
||||
btAlignedObjectArray<char*> m_dataBlocks;
|
||||
btBulletFile();
|
||||
|
||||
|
||||
Reference in New Issue
Block a user