First draft of btMultiBody serialization, including optional names for base, link and joints (see ImportURDFDemo/ImportURDFSetup.cpp how this is done)

Bump up version number to 2.84 because of new serialization data.
This commit is contained in:
erwincoumans
2015-07-09 17:36:00 -07:00
parent 285ac286fa
commit f6f76901fd
20 changed files with 1435 additions and 517 deletions

View File

@@ -31,6 +31,8 @@ typedef struct bInvalidHandle {
class ListBase;
class btVector3FloatData;
class btVector3DoubleData;
class btQuaternionFloatData;
class btQuaternionDoubleData;
class btMatrix3x3FloatData;
class btMatrix3x3DoubleData;
class btTransformFloatData;
@@ -57,6 +59,7 @@ typedef struct bInvalidHandle {
class btCompoundShapeChildData;
class btCompoundShapeData;
class btCylinderShapeData;
class btConeShapeData;
class btCapsuleShapeData;
class btTriangleInfoData;
class btTriangleInfoMapData;
@@ -64,18 +67,34 @@ typedef struct bInvalidHandle {
class btConvexHullShapeData;
class btCollisionObjectDoubleData;
class btCollisionObjectFloatData;
class btDynamicsWorldDoubleData;
class btDynamicsWorldFloatData;
class btRigidBodyFloatData;
class btRigidBodyDoubleData;
class btConstraintInfo1;
class btTypedConstraintFloatData;
class btTypedConstraintData;
class btTypedConstraintDoubleData;
class btPoint2PointConstraintFloatData;
class btPoint2PointConstraintDoubleData2;
class btPoint2PointConstraintDoubleData;
class btHingeConstraintDoubleData;
class btHingeConstraintFloatData;
class btHingeConstraintDoubleData2;
class btConeTwistConstraintDoubleData;
class btConeTwistConstraintData;
class btGeneric6DofConstraintData;
class btGeneric6DofConstraintDoubleData2;
class btGeneric6DofSpringConstraintData;
class btGeneric6DofSpringConstraintDoubleData2;
class btGeneric6DofSpring2ConstraintData;
class btGeneric6DofSpring2ConstraintDoubleData2;
class btSliderConstraintData;
class btSliderConstraintDoubleData;
class btGearConstraintFloatData;
class btGearConstraintDoubleData;
class btContactSolverInfoDoubleData;
class btContactSolverInfoFloatData;
class SoftBodyMaterialData;
class SoftBodyNodeData;
class SoftBodyLinkData;
@@ -87,6 +106,10 @@ typedef struct bInvalidHandle {
class SoftBodyClusterData;
class btSoftBodyJointData;
class btSoftBodyFloatData;
class btMultiBodyLinkDoubleData;
class btMultiBodyLinkFloatData;
class btMultiBodyDoubleData;
class btMultiBodyFloatData;
// -------------------------------------------------- //
class PointerArray
{
@@ -132,6 +155,22 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btQuaternionFloatData
{
public:
float m_floats[4];
};
// -------------------------------------------------- //
class btQuaternionDoubleData
{
public:
double m_floats[4];
};
// -------------------------------------------------- //
class btMatrix3x3FloatData
{
@@ -421,6 +460,16 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btConeShapeData
{
public:
btConvexInternalShapeData m_convexInternalShapeData;
int m_upIndex;
char m_padding[4];
};
// -------------------------------------------------- //
class btCapsuleShapeData
{
@@ -503,6 +552,7 @@ typedef struct bInvalidHandle {
double m_contactProcessingThreshold;
double m_deactivationTime;
double m_friction;
double m_rollingFriction;
double m_restitution;
double m_hitFraction;
double m_ccdSweptSphereRadius;
@@ -534,6 +584,7 @@ typedef struct bInvalidHandle {
float m_contactProcessingThreshold;
float m_deactivationTime;
float m_friction;
float m_rollingFriction;
float m_restitution;
float m_hitFraction;
float m_ccdSweptSphereRadius;
@@ -545,6 +596,25 @@ typedef struct bInvalidHandle {
int m_activationState1;
int m_internalType;
int m_checkCollideWith;
char m_padding[4];
};
// -------------------------------------------------- //
class btDynamicsWorldDoubleData
{
public:
btContactSolverInfoDoubleData m_solverInfo;
btVector3DoubleData m_gravity;
};
// -------------------------------------------------- //
class btDynamicsWorldFloatData
{
public:
btContactSolverInfoFloatData m_solverInfo;
btVector3FloatData m_gravity;
};
@@ -614,6 +684,26 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btTypedConstraintFloatData
{
public:
btRigidBodyFloatData *m_rbA;
btRigidBodyFloatData *m_rbB;
char *m_name;
int m_objectType;
int m_userConstraintType;
int m_userConstraintId;
int m_needsFeedback;
float m_appliedImpulse;
float m_dbgDrawSize;
int m_disableCollisionsBetweenLinkedBodies;
int m_overrideNumSolverIterations;
float m_breakingImpulseThreshold;
int m_isEnabled;
};
// -------------------------------------------------- //
class btTypedConstraintData
{
@@ -634,6 +724,27 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btTypedConstraintDoubleData
{
public:
btRigidBodyDoubleData *m_rbA;
btRigidBodyDoubleData *m_rbB;
char *m_name;
int m_objectType;
int m_userConstraintType;
int m_userConstraintId;
int m_needsFeedback;
double m_appliedImpulse;
double m_dbgDrawSize;
int m_disableCollisionsBetweenLinkedBodies;
int m_overrideNumSolverIterations;
double m_breakingImpulseThreshold;
int m_isEnabled;
char padding[4];
};
// -------------------------------------------------- //
class btPoint2PointConstraintFloatData
{
@@ -644,6 +755,16 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btPoint2PointConstraintDoubleData2
{
public:
btTypedConstraintDoubleData m_typeConstraintData;
btVector3DoubleData m_pivotInA;
btVector3DoubleData m_pivotInB;
};
// -------------------------------------------------- //
class btPoint2PointConstraintDoubleData
{
@@ -694,6 +815,44 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btHingeConstraintDoubleData2
{
public:
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame;
btTransformDoubleData m_rbBFrame;
int m_useReferenceFrameA;
int m_angularOnly;
int m_enableAngularMotor;
double m_motorTargetVelocity;
double m_maxMotorImpulse;
double m_lowerLimit;
double m_upperLimit;
double m_limitSoftness;
double m_biasFactor;
double m_relaxationFactor;
char m_padding1[4];
};
// -------------------------------------------------- //
class btConeTwistConstraintDoubleData
{
public:
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame;
btTransformDoubleData m_rbBFrame;
double m_swingSpan1;
double m_swingSpan2;
double m_twistSpan;
double m_limitSoftness;
double m_biasFactor;
double m_relaxationFactor;
double m_damping;
};
// -------------------------------------------------- //
class btConeTwistConstraintData
{
@@ -728,6 +887,22 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btGeneric6DofConstraintDoubleData2
{
public:
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame;
btTransformDoubleData m_rbBFrame;
btVector3DoubleData m_linearUpperLimit;
btVector3DoubleData m_linearLowerLimit;
btVector3DoubleData m_angularUpperLimit;
btVector3DoubleData m_angularLowerLimit;
int m_useLinearReferenceFrameA;
int m_useOffsetForConstraintFrame;
};
// -------------------------------------------------- //
class btGeneric6DofSpringConstraintData
{
@@ -740,6 +915,114 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btGeneric6DofSpringConstraintDoubleData2
{
public:
btGeneric6DofConstraintDoubleData2 m_6dofData;
int m_springEnabled[6];
double m_equilibriumPoint[6];
double m_springStiffness[6];
double m_springDamping[6];
};
// -------------------------------------------------- //
class btGeneric6DofSpring2ConstraintData
{
public:
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame;
btTransformFloatData m_rbBFrame;
btVector3FloatData m_linearUpperLimit;
btVector3FloatData m_linearLowerLimit;
btVector3FloatData m_linearBounce;
btVector3FloatData m_linearStopERP;
btVector3FloatData m_linearStopCFM;
btVector3FloatData m_linearMotorERP;
btVector3FloatData m_linearMotorCFM;
btVector3FloatData m_linearTargetVelocity;
btVector3FloatData m_linearMaxMotorForce;
btVector3FloatData m_linearServoTarget;
btVector3FloatData m_linearSpringStiffness;
btVector3FloatData m_linearSpringDamping;
btVector3FloatData m_linearEquilibriumPoint;
char m_linearEnableMotor[4];
char m_linearServoMotor[4];
char m_linearEnableSpring[4];
char m_linearSpringStiffnessLimited[4];
char m_linearSpringDampingLimited[4];
char m_padding1[4];
btVector3FloatData m_angularUpperLimit;
btVector3FloatData m_angularLowerLimit;
btVector3FloatData m_angularBounce;
btVector3FloatData m_angularStopERP;
btVector3FloatData m_angularStopCFM;
btVector3FloatData m_angularMotorERP;
btVector3FloatData m_angularMotorCFM;
btVector3FloatData m_angularTargetVelocity;
btVector3FloatData m_angularMaxMotorForce;
btVector3FloatData m_angularServoTarget;
btVector3FloatData m_angularSpringStiffness;
btVector3FloatData m_angularSpringDamping;
btVector3FloatData m_angularEquilibriumPoint;
char m_angularEnableMotor[4];
char m_angularServoMotor[4];
char m_angularEnableSpring[4];
char m_angularSpringStiffnessLimited[4];
char m_angularSpringDampingLimited[4];
int m_rotateOrder;
};
// -------------------------------------------------- //
class btGeneric6DofSpring2ConstraintDoubleData2
{
public:
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame;
btTransformDoubleData m_rbBFrame;
btVector3DoubleData m_linearUpperLimit;
btVector3DoubleData m_linearLowerLimit;
btVector3DoubleData m_linearBounce;
btVector3DoubleData m_linearStopERP;
btVector3DoubleData m_linearStopCFM;
btVector3DoubleData m_linearMotorERP;
btVector3DoubleData m_linearMotorCFM;
btVector3DoubleData m_linearTargetVelocity;
btVector3DoubleData m_linearMaxMotorForce;
btVector3DoubleData m_linearServoTarget;
btVector3DoubleData m_linearSpringStiffness;
btVector3DoubleData m_linearSpringDamping;
btVector3DoubleData m_linearEquilibriumPoint;
char m_linearEnableMotor[4];
char m_linearServoMotor[4];
char m_linearEnableSpring[4];
char m_linearSpringStiffnessLimited[4];
char m_linearSpringDampingLimited[4];
char m_padding1[4];
btVector3DoubleData m_angularUpperLimit;
btVector3DoubleData m_angularLowerLimit;
btVector3DoubleData m_angularBounce;
btVector3DoubleData m_angularStopERP;
btVector3DoubleData m_angularStopCFM;
btVector3DoubleData m_angularMotorERP;
btVector3DoubleData m_angularMotorCFM;
btVector3DoubleData m_angularTargetVelocity;
btVector3DoubleData m_angularMaxMotorForce;
btVector3DoubleData m_angularServoTarget;
btVector3DoubleData m_angularSpringStiffness;
btVector3DoubleData m_angularSpringDamping;
btVector3DoubleData m_angularEquilibriumPoint;
char m_angularEnableMotor[4];
char m_angularServoMotor[4];
char m_angularEnableSpring[4];
char m_angularSpringStiffnessLimited[4];
char m_angularSpringDampingLimited[4];
int m_rotateOrder;
};
// -------------------------------------------------- //
class btSliderConstraintData
{
@@ -756,6 +1039,103 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btSliderConstraintDoubleData
{
public:
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame;
btTransformDoubleData m_rbBFrame;
double m_linearUpperLimit;
double m_linearLowerLimit;
double m_angularUpperLimit;
double m_angularLowerLimit;
int m_useLinearReferenceFrameA;
int m_useOffsetForConstraintFrame;
};
// -------------------------------------------------- //
class btGearConstraintFloatData
{
public:
btTypedConstraintFloatData m_typeConstraintData;
btVector3FloatData m_axisInA;
btVector3FloatData m_axisInB;
float m_ratio;
char m_padding[4];
};
// -------------------------------------------------- //
class btGearConstraintDoubleData
{
public:
btTypedConstraintDoubleData m_typeConstraintData;
btVector3DoubleData m_axisInA;
btVector3DoubleData m_axisInB;
double m_ratio;
};
// -------------------------------------------------- //
class btContactSolverInfoDoubleData
{
public:
double m_tau;
double m_damping;
double m_friction;
double m_timeStep;
double m_restitution;
double m_maxErrorReduction;
double m_sor;
double m_erp;
double m_erp2;
double m_globalCfm;
double m_splitImpulsePenetrationThreshold;
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_maxGyroscopicForce;
double m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
// -------------------------------------------------- //
class btContactSolverInfoFloatData
{
public:
float m_tau;
float m_damping;
float m_friction;
float m_timeStep;
float m_restitution;
float m_maxErrorReduction;
float m_sor;
float m_erp;
float m_erp2;
float m_globalCfm;
float m_splitImpulsePenetrationThreshold;
float m_splitImpulseTurnErp;
float m_linearSlop;
float m_warmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
// -------------------------------------------------- //
class SoftBodyMaterialData
{
@@ -964,5 +1344,82 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btMultiBodyLinkDoubleData
{
public:
btQuaternionDoubleData m_zeroRotParentToThis;
btVector3DoubleData m_parentComToThisComOffset;
btVector3DoubleData m_thisPivotToThisComOffset;
btVector3DoubleData m_jointAxisTop[6];
btVector3DoubleData m_jointAxisBottom[6];
char *m_linkName;
char *m_jointName;
btCollisionObjectDoubleData *m_linkCollider;
btVector3DoubleData m_linkInertia;
double m_linkMass;
int m_parentIndex;
int m_jointType;
int m_dofCount;
int m_posVarCount;
double m_jointPos[7];
double m_jointVel[6];
double m_jointTorque[6];
};
// -------------------------------------------------- //
class btMultiBodyLinkFloatData
{
public:
btQuaternionFloatData m_zeroRotParentToThis;
btVector3FloatData m_parentComToThisComOffset;
btVector3FloatData m_thisPivotToThisComOffset;
btVector3FloatData m_jointAxisTop[6];
btVector3FloatData m_jointAxisBottom[6];
char *m_linkName;
char *m_jointName;
btCollisionObjectFloatData *m_linkCollider;
btVector3FloatData m_linkInertia;
int m_dofCount;
float m_linkMass;
int m_parentIndex;
int m_jointType;
float m_jointPos[7];
float m_jointVel[6];
float m_jointTorque[6];
int m_posVarCount;
};
// -------------------------------------------------- //
class btMultiBodyDoubleData
{
public:
char *m_baseName;
btMultiBodyLinkDoubleData *m_links;
btCollisionObjectDoubleData *m_baseCollider;
btTransformDoubleData m_baseWorldTransform;
btVector3DoubleData m_baseInertia;
int m_numLinks;
double m_baseMass;
char m_padding[4];
};
// -------------------------------------------------- //
class btMultiBodyFloatData
{
public:
char *m_baseName;
btMultiBodyLinkFloatData *m_links;
btCollisionObjectFloatData *m_baseCollider;
btTransformFloatData m_baseWorldTransform;
btVector3FloatData m_baseInertia;
float m_baseMass;
int m_numLinks;
};
}
#endif//__BULLET_H__

View File

@@ -220,7 +220,7 @@ int main(int argc,char** argv)
fprintf(dump, "%s\n", data);
char* filename = "../../../Demos/SerializeDemo/testFile.bullet";
char* filename = "../../../../data/slope.bullet";
if (argc==2)
filename = argv[1];

View File

@@ -120,6 +120,7 @@ typedef unsigned long uintptr_t;
// include files for automatic dependancies
#include "DNA_rigidbody.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btTransform.h"
#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h"
@@ -152,6 +153,7 @@ typedef unsigned long uintptr_t;
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletSoftBody/btSoftBodyData.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#ifdef HAVE_CONFIG_H
#include <config.h>
@@ -167,6 +169,7 @@ char *includefiles[] = {
// of makesdna.c (this file) as well
"../makesdna/DNA_rigidbody.h",
"../../../src/LinearMath/btVector3.h",
"../../../src/LinearMath/btQuaternion.h",
"../../../src/LinearMath/btMatrix3x3.h",
"../../../src/LinearMath/btTransform.h",
"../../../src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h",
@@ -197,7 +200,8 @@ char *includefiles[] = {
"../../../src/BulletDynamics/ConstraintSolver/btSliderConstraint.h",
"../../../src/BulletDynamics/ConstraintSolver/btGearConstraint.h",
"../../../src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h",
"../../../src/BulletSoftBody/btSoftBodyData.h",
"../../../src/BulletSoftBody/btSoftBodyData.h",
"../../../src/BulletDynamics/Featherstone/btMultiBody.h",
// empty string to indicate end of includefiles
""
};

View File

@@ -1 +1 @@
2.83
2.84

BIN
data/multibody.bullet Normal file

Binary file not shown.

BIN
data/r2d2_multibody.bullet Normal file

Binary file not shown.

Binary file not shown.

View File

@@ -9,6 +9,8 @@
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../../Utils/b3ResourcePath.h"
#ifdef ENABLE_ROS_URDF
#include "ROSURDFImporter.h"
#endif
@@ -35,6 +37,7 @@ class ImportUrdfSetup : public CommonMultiBodyBase
struct ImportUrdfInternalData* m_data;
bool m_useMultiBody;
btAlignedObjectArray<std::string* > m_nameMemory;
public:
ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
@@ -143,6 +146,11 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
ImportUrdfSetup::~ImportUrdfSetup()
{
for (int i=0;i<m_nameMemory.size();i++)
{
delete m_nameMemory[i];
}
m_nameMemory.clear();
delete m_data;
}
@@ -194,7 +202,7 @@ void ImportUrdfSetup::initPhysics()
m_dynamicsWorld->setGravity(gravity);
//now print the tree using the new interface
URDFImporterInterface* bla=0;
@@ -215,6 +223,9 @@ void ImportUrdfSetup::initPhysics()
#endif//USE_ROS_URDF
URDFImporterInterface& u2b = *bla;
bool loadOk = u2b.loadURDF(m_fileName);
//test to serialize a multibody to disk or shared memory, with base, link and joint names
btSerializer* s = new btDefaultSerializer;
if (loadOk)
{
@@ -240,25 +251,40 @@ void ImportUrdfSetup::initPhysics()
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
mb = creation.getBulletMultiBody();
if (m_useMultiBody)
if (m_useMultiBody && mb )
{
std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
m_nameMemory.push_back(name);
s->registerNameForPointer(name->c_str(),name->c_str());
mb->setBaseName(name->c_str());
//create motors for each btMultiBody joint
int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++)
{
int mbLinkIndex = i;
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
s->registerNameForPointer(jointName->c_str(),jointName->c_str());
s->registerNameForPointer(linkName->c_str(),linkName->c_str());
m_nameMemory.push_back(jointName);
m_nameMemory.push_back(linkName);
mb->getLink(i).m_linkName = linkName->c_str();
mb->getLink(i).m_jointName = jointName->c_str();
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
)
{
if (m_data->m_numMotors<MAX_NUM_MOTORS)
{
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
std::string jointName = u2b.getJointName(urdfLinkIndex);
char motorName[1024];
sprintf(motorName,"%s q'", jointName.c_str());
sprintf(motorName,"%s q'", jointName->c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
@@ -349,6 +375,17 @@ void ImportUrdfSetup::initPhysics()
///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
}
m_dynamicsWorld->serialize(s);
b3ResourcePath p;
char resourcePath[1024];
if (p.findResourcePath("r2d2_multibody.bullet",resourcePath,1024))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
fclose(f);
}
}
void ImportUrdfSetup::stepSimulation(float deltaTime)

View File

@@ -5,6 +5,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../Utils/b3ResourcePath.h"
static btScalar radius(0.2);
@@ -366,6 +367,16 @@ void TestJointTorqueSetup::initPhysics()
}
}
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
b3ResourcePath p;
char resourcePath[1024];
if (p.findResourcePath("multibody.bullet",resourcePath,1024))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
fclose(f);
}
}
void TestJointTorqueSetup::stepSimulation(float deltaTime)

View File

@@ -35,7 +35,7 @@ subject to the following restrictions:
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The RollingFrictionDemo shows the use of rolling friction.
///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same.
@@ -231,12 +231,17 @@ void RollingFrictionDemo::initPhysics()
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
FILE* f = fopen("slope.bullet","wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
fclose(f);
b3ResourcePath p;
char resourcePath[1024];
if (p.findResourcePath("slope.bullet",resourcePath,1024))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
fclose(f);
}
}
void RollingFrictionDemo::exitPhysics()
{

View File

@@ -1533,7 +1533,7 @@ void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
for (i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
if ((colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK))
{
colObj->serializeSingleObject(serializer);
}

View File

@@ -26,7 +26,7 @@
#include "btMultiBodyLinkCollider.h"
#include "btMultiBodyJointFeedback.h"
#include "LinearMath/btTransformUtil.h"
#include "LinearMath/btSerializer.h"
#include "Bullet3Common/b3Logging.h"
// #define INCLUDE_GYRO_TERM
@@ -95,6 +95,7 @@ btMultiBody::btMultiBody(int n_links,
bool multiDof)
:
m_baseCollider(0),
m_baseName(0),
m_basePos(0,0,0),
m_baseQuat(0, 0, 0, 1),
m_baseMass(mass),
@@ -400,6 +401,17 @@ btScalar * btMultiBody::getJointVelMultiDof(int i)
return &m_realBuf[6 + m_links[i].m_dofOffset];
}
const btScalar * btMultiBody::getJointPosMultiDof(int i) const
{
return &m_links[i].m_jointPos[0];
}
const btScalar * btMultiBody::getJointVelMultiDof(int i) const
{
return &m_realBuf[6 + m_links[i].m_dofOffset];
}
void btMultiBody::setJointPos(int i, btScalar q)
{
m_links[i].m_jointPos[0] = q;
@@ -2412,3 +2424,85 @@ void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to
}
int btMultiBody::calculateSerializeBufferSize() const
{
int sz = sizeof(btMultiBodyData);
return sz;
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
{
btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
mbd->m_baseMass = this->getBaseMass();
getBaseInertia().serialize(mbd->m_baseInertia);
{
char* name = (char*) serializer->findNameForPointer(m_baseName);
mbd->m_baseName = (char*)serializer->getUniquePointer(name);
if (mbd->m_baseName)
{
serializer->serializeName(name);
}
}
mbd->m_numLinks = this->getNumLinks();
mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
if (mbd->m_links)
{
int sz = sizeof(btMultiBodyLinkData);
int numElem = mbd->m_numLinks;
btChunk* chunk = serializer->allocate(sz,numElem);
btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr;
for (int i=0;i<numElem;i++,memPtr++)
{
memPtr->m_jointType = getLink(i).m_jointType;
memPtr->m_dofCount = getLink(i).m_dofCount;
getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
memPtr->m_linkMass = getLink(i).m_mass;
memPtr->m_parentIndex = getLink(i).m_parent;
getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
btAssert(memPtr->m_dofCount<=3);
for (int dof = 0;dof<getLink(i).m_dofCount;dof++)
{
getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
}
int numPosVar = getLink(i).m_posVarCount;
for (int posvar = 0; posvar < numPosVar;posvar++)
{
memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
}
{
char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName);
memPtr->m_linkName = (char*)serializer->getUniquePointer(name);
if (memPtr->m_linkName)
{
serializer->serializeName(name);
}
}
{
char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName);
memPtr->m_jointName = (char*)serializer->getUniquePointer(name);
if (memPtr->m_jointName)
{
serializer->serializeName(name);
}
}
memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider);
}
serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]);
}
return btMultiBodyDataName;
}

View File

@@ -30,6 +30,17 @@
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btAlignedObjectArray.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btMultiBodyData btMultiBodyDoubleData
#define btMultiBodyDataName "btMultiBodyDoubleData"
#define btMultiBodyLinkData btMultiBodyLinkDoubleData
#define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData"
#else
#define btMultiBodyData btMultiBodyFloatData
#define btMultiBodyDataName "btMultiBodyFloatData"
#define btMultiBodyLinkData btMultiBodyLinkFloatData
#define btMultiBodyLinkDataName "btMultiBodyLinkFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#include "btMultiBodyLink.h"
class btMultiBodyLinkCollider;
@@ -220,6 +231,9 @@ public:
btScalar * getJointVelMultiDof(int i);
btScalar * getJointPosMultiDof(int i);
const btScalar * getJointVelMultiDof(int i) const ;
const btScalar * getJointPosMultiDof(int i) const ;
void setJointPos(int i, btScalar q);
void setJointVel(int i, btScalar qdot);
void setJointPosMultiDof(int i, btScalar *q);
@@ -586,6 +600,20 @@ void addJointTorque(int i, btScalar Q);
}
void forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
const char* getBaseName() const
{
return m_baseName;
}
///memory of setBaseName needs to be manager by user
void setBaseName(const char* name)
{
m_baseName = name;
}
private:
btMultiBody(const btMultiBody &); // not implemented
@@ -608,11 +636,11 @@ private:
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
private:
btMultiBodyLinkCollider* m_baseCollider;//can be NULL
const char* m_baseName;//memory needs to be manager by user!
btVector3 m_basePos; // position of COM of base (world frame)
btQuaternion m_baseQuat; // rotates world points into base frame
@@ -679,4 +707,96 @@ private:
bool m_internalNeedsJointFeedback;
};
struct btMultiBodyLinkDoubleData
{
btQuaternionDoubleData m_zeroRotParentToThis;
btVector3DoubleData m_parentComToThisComOffset;
btVector3DoubleData m_thisPivotToThisComOffset;
btVector3DoubleData m_jointAxisTop[6];
btVector3DoubleData m_jointAxisBottom[6];
char *m_linkName;
char *m_jointName;
btCollisionObjectDoubleData *m_linkCollider;
btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal)
double m_linkMass;
int m_parentIndex;
int m_jointType;
int m_dofCount;
int m_posVarCount;
double m_jointPos[7];
double m_jointVel[6];
double m_jointTorque[6];
};
struct btMultiBodyLinkFloatData
{
btQuaternionFloatData m_zeroRotParentToThis;
btVector3FloatData m_parentComToThisComOffset;
btVector3FloatData m_thisPivotToThisComOffset;
btVector3FloatData m_jointAxisTop[6];
btVector3FloatData m_jointAxisBottom[6];
char *m_linkName;
char *m_jointName;
btCollisionObjectFloatData *m_linkCollider;
btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal)
int m_dofCount;
float m_linkMass;
int m_parentIndex;
int m_jointType;
float m_jointPos[7];
float m_jointVel[6];
float m_jointTorque[6];
int m_posVarCount;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btMultiBodyDoubleData
{
char *m_baseName;
btMultiBodyLinkDoubleData *m_links;
btCollisionObjectDoubleData *m_baseCollider;
btTransformDoubleData m_baseWorldTransform;
btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal)
int m_numLinks;
double m_baseMass;
char m_padding[4];
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btMultiBodyFloatData
{
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;
};
#endif

View File

@@ -21,7 +21,7 @@ subject to the following restrictions:
#include "LinearMath/btQuickprof.h"
#include "btMultiBodyConstraint.h"
#include "LinearMath/btIDebugDraw.h"
#include "LinearMath/btSerializer.h"
void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
@@ -1011,3 +1011,37 @@ void btMultiBodyDynamicsWorld::clearForces()
}
void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
{
serializer->startSerialization();
serializeDynamicsWorldInfo( serializer);
serializeMultiBodies(serializer);
serializeRigidBodies(serializer);
serializeCollisionObjects(serializer);
serializer->finishSerialization();
}
void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
{
int i;
//serialize all collision objects
for (i=0;i<m_multiBodies.size();i++)
{
btMultiBody* mb = m_multiBodies[i];
{
int len = mb->calculateSerializeBufferSize();
btChunk* chunk = serializer->allocate(len,1);
const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
}
}
}

View File

@@ -40,6 +40,8 @@ protected:
virtual void updateActivationState(btScalar timeStep);
virtual void solveConstraints(btContactSolverInfo& solverInfo);
virtual void serializeMultiBodies(btSerializer* serializer);
public:
btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
@@ -91,5 +93,7 @@ public:
virtual void clearMultiBodyForces();
virtual void applyGravity();
virtual void serialize(btSerializer* serializer);
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

View File

@@ -127,6 +127,9 @@ btVector3 m_appliedConstraintForce; // In WORLD frame
btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
// ctor: set some sensible defaults
btMultibodyLink()
: m_mass(1),
@@ -138,8 +141,9 @@ btVector3 m_appliedConstraintForce; // In WORLD frame
m_dofCount(0),
m_posVarCount(0),
m_jointType(btMultibodyLink::eInvalid),
m_jointFeedback(0)
m_jointFeedback(0),
m_linkName(0),
m_jointName(0)
{
m_inertiaLocal.setValue(1, 1, 1);

View File

@@ -22,6 +22,13 @@ subject to the following restrictions:
#include "btQuadWord.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btQuaternionData btQuaternionDoubleData
#define btQuaternionDataName "btQuaternionDoubleData"
#else
#define btQuaternionData btQuaternionFloatData
#define btQuaternionDataName "btQuaternionFloatData"
#endif //BT_USE_DOUBLE_PRECISION
@@ -560,7 +567,18 @@ public:
SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
SIMD_FORCE_INLINE void serialize(struct btQuaternionData& dataOut) const;
SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionData& dataIn);
SIMD_FORCE_INLINE void serializeFloat(struct btQuaternionFloatData& dataOut) const;
SIMD_FORCE_INLINE void deSerializeFloat(const struct btQuaternionFloatData& dataIn);
SIMD_FORCE_INLINE void serializeDouble(struct btQuaternionDoubleData& dataOut) const;
SIMD_FORCE_INLINE void deSerializeDouble(const struct btQuaternionDoubleData& dataIn);
};
@@ -903,6 +921,62 @@ shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
return shortestArcQuat(v0,v1);
}
struct btQuaternionFloatData
{
float m_floats[4];
};
struct btQuaternionDoubleData
{
double m_floats[4];
};
SIMD_FORCE_INLINE void btQuaternion::serializeFloat(struct btQuaternionFloatData& dataOut) const
{
///could also do a memcpy, check if it is worth it
for (int i=0;i<4;i++)
dataOut.m_floats[i] = float(m_floats[i]);
}
SIMD_FORCE_INLINE void btQuaternion::deSerializeFloat(const struct btQuaternionFloatData& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = btScalar(dataIn.m_floats[i]);
}
SIMD_FORCE_INLINE void btQuaternion::serializeDouble(struct btQuaternionDoubleData& dataOut) const
{
///could also do a memcpy, check if it is worth it
for (int i=0;i<4;i++)
dataOut.m_floats[i] = double(m_floats[i]);
}
SIMD_FORCE_INLINE void btQuaternion::deSerializeDouble(const struct btQuaternionDoubleData& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = btScalar(dataIn.m_floats[i]);
}
SIMD_FORCE_INLINE void btQuaternion::serialize(struct btQuaternionData& dataOut) const
{
///could also do a memcpy, check if it is worth it
for (int i=0;i<4;i++)
dataOut.m_floats[i] = m_floats[i];
}
SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionData& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = dataIn.m_floats[i];
}
#endif //BT_SIMD__QUATERNION_H_

View File

@@ -28,7 +28,7 @@ subject to the following restrictions:
#include <float.h>
/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
#define BT_BULLET_VERSION 283
#define BT_BULLET_VERSION 284
inline int btGetVersion()
{

File diff suppressed because it is too large Load Diff

View File

@@ -113,6 +113,8 @@ public:
# define BT_MAKE_ID(a,b,c,d) ( (int)(d)<<24 | (int)(c)<<16 | (b)<<8 | (a) )
#endif
#define BT_MULTIBODY_CODE BT_MAKE_ID('M','B','D','Y')
#define BT_SOFTBODY_CODE BT_MAKE_ID('S','B','D','Y')
#define BT_COLLISIONOBJECT_CODE BT_MAKE_ID('C','O','B','J')
#define BT_RIGIDBODY_CODE BT_MAKE_ID('R','B','D','Y')
@@ -465,7 +467,7 @@ public:
buffer[9] = '2';
buffer[10] = '8';
buffer[11] = '3';
buffer[11] = '4';
}