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

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

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

@@ -52,10 +52,11 @@ 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;
@@ -63,9 +64,20 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
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);
btVector3 baseWorldPos;
baseWorldPos.deSerializeDouble(mbd->m_baseWorldPosition);
mb->setBasePos(baseWorldPos);
btQuaternion baseWorldRot;
baseWorldRot.deSerializeDouble(mbd->m_baseWorldOrientation);
mb->setWorldToBaseRot(baseWorldRot.inverse());
btVector3 baseLinVal;
baseLinVal.deSerializeDouble(mbd->m_baseLinearVelocity);
btVector3 baseAngVel;
baseAngVel.deSerializeDouble(mbd->m_baseAngularVelocity);
mb->setBaseVel(baseLinVal);
mb->setBaseOmega(baseAngVel);
for (int i = 0; i < mbd->m_numLinks; i++)
{
}
@@ -74,6 +86,74 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
mb->forwardKinematics(scratchQ, scratchM);
mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
}
//todo: check why body1 pointer is not properly deserialized
for (int i = 0; i < bulletFile2->m_contactManifolds.size(); i++)
{
btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[i];
void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
if (ptr)
{
manifoldData->m_body1 = ptr;
btCollisionObjectDoubleData* cdd = (btCollisionObjectDoubleData*)ptr;
}
}
btDispatcherInfo& dispatchInfo = m_data->m_mbDynamicsWorld->getDispatchInfo();
{
m_data->m_mbDynamicsWorld->updateAabbs();
m_data->m_mbDynamicsWorld->computeOverlappingPairs();
btDispatcher* dispatcher = m_data->m_mbDynamicsWorld->getDispatcher();
if (dispatcher)
{
btOverlappingPairCache* pairCache = m_data->m_mbDynamicsWorld->getBroadphase()->getOverlappingPairCache();
if (dispatcher)
{
dispatcher->dispatchAllCollisionPairs(pairCache, dispatchInfo, dispatcher);
}
int numExistingManifolds = m_data->m_mbDynamicsWorld->getDispatcher()->getNumManifolds();
btManifoldArray manifoldArray;
for (int i = 0; i < pairCache->getNumOverlappingPairs(); i++)
{
btBroadphasePair& pair = pairCache->getOverlappingPairArray()[i];
if (pair.m_algorithm)
{
pair.m_algorithm->getAllContactManifolds(manifoldArray);
//for each existing manifold, search a matching manifoldData and reconstruct
for (int m = 0; m < manifoldArray.size(); m++)
{
btPersistentManifold* existingManifold = manifoldArray[m];
int uid0 = existingManifold->getBody0()->getBroadphaseHandle()->m_uniqueId;
int uid1 = existingManifold->getBody1()->getBroadphaseHandle()->m_uniqueId;
int matchingManifoldIndex = -1;
for (int q= 0; q < bulletFile2->m_contactManifolds.size(); q++)
{
btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[q];
btCollisionObjectDoubleData* cdd0 = (btCollisionObjectDoubleData*)manifoldData->m_body0;
btCollisionObjectDoubleData* cdd1 = (btCollisionObjectDoubleData*)manifoldData->m_body1;
if (uid0 == cdd0->m_uniqueId && uid1 == cdd1->m_uniqueId)
{
matchingManifoldIndex = q;
}
}
if (matchingManifoldIndex >= 0)
{
btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[matchingManifoldIndex];
existingManifold->deSerializeDouble(manifoldData);
}
else
{
printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1);
}
manifoldArray.clear();
}
}
}
}
}
}
}
else
@@ -94,8 +174,14 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
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);
btVector3 baseWorldPos;
baseWorldPos.deSerializeDouble(mbd->m_baseWorldPosition);
btQuaternion baseWorldOrn;
baseWorldOrn.deSerializeDouble(mbd->m_baseWorldOrientation);
mb->setBasePos(baseWorldPos);
mb->setWorldToBaseRot(baseWorldOrn.inverse());
m_data->m_mbMap.insert(mbd, mb);
for (int i = 0; i < mbd->m_numLinks; i++)

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>

8
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2173,8 +2173,11 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
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();
@@ -8305,6 +8308,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);

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

@@ -1,30 +1,95 @@
import pybullet as p
import math, time
import difflib,sys
numSteps = 500
numSteps2 = 3
p.connect(p.GUI, options="--width=1024 --height=768")
numObjects = 10
verbose = 0
p.loadURDF("plane.urdf")
for i in range (10):
cube = p.loadURDF("cube_small.urdf",0,i*0.01,i*0.5)
def setupWorld():
p.resetSimulation()
p.loadURDF("planeMesh.urdf")
for i in range (numObjects):
cube = p.loadURDF("cube_small.urdf",0,0,(i+1)*0.5)
p.stepSimulation()
p.setGravity(0,0,-10)
#for i in range (500):
# p.stepSimulation()
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)
saveState = 1
if saveState:
for i in range (500):
setupWorld()
for i in range (numSteps):
p.stepSimulation()
p.saveBullet("state.bullet")
else:
p.restoreState(fileName="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()
file1 = open("saveFile.txt","r")
file2 = open("restoreFile.txt","r")
#file3 = open("saveFile.txt","r")#saveFileReorder.txt","r")
compareFiles(file1,file2)
#compareFiles(file1,file3)
for i in range (10):
print("pos[",i,"]=",p.getBasePositionAndOrientation(i))
while (p.getConnectionInfo()["isConnected"]):
time.sleep(1)
file1.close()
file2.close()

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

11
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
@@ -656,7 +656,6 @@ struct btCollisionObjectFloatData
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,6 +1654,28 @@ 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)
{
@@ -1661,6 +1683,8 @@ void btCollisionWorld::serialize(btSerializer* serializer)
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

26
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,6 +334,8 @@ 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)

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

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

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

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

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