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

Merge pull request #420 from erwincoumans/master

Browse Source 
some work towards streaming Bullet data over shared memory for client…
This commit is contained in:
erwincoumans
2015-07-09 18:19:59 -07:00
parent c293c3df8f f6f76901fd
commit ae3e392b69
27 changed files with 1611 additions and 593 deletions
Download Patch File Download Diff File Expand all files Collapse all files

457
Extras/Serialize/BulletFileLoader/autogenerated/bullet.h
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__

2
Extras/Serialize/HeaderGenerator/apiGen.cpp
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];

6
Extras/Serialize/makesdna/makesdna.cpp
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
""
};

2
VERSION
View File

@@ -1 +1 @@
2.83
2.84

1
build3/premake4.lua
View File

@@ -152,6 +152,7 @@ if not _OPTIONS["ios"] then
include "../examples/OpenGLWindow"
include "../examples/SharedMemory"
include "../examples/ThirdPartyLibs/Gwen"
include "../Extras"
include "../examples/HelloWorld"
include "../examples/BasicDemo"

BIN
data/multibody.bullet Normal file
View File

Binary file not shown.

BIN
data/r2d2_multibody.bullet Normal file
View File

Binary file not shown.

BIN
data/slope.bullet Normal file
View File

Binary file not shown.

49
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
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)

11
examples/MultiBody/TestJointTorqueSetup.cpp
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)

56
examples/RollingFrictionDemo/RollingFrictionDemo.cpp
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.
@@ -75,7 +75,7 @@ public:
void RollingFrictionDemo::initPhysics()
{
m_guiHelper->setUpAxis(1);
m_guiHelper->setUpAxis(2);
///collision configuration contains default setup for memory, collision setup
@@ -93,23 +93,22 @@ void RollingFrictionDemo::initPhysics()
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
// m_dynamicsWorld->getSolverInfo().m_singleAxisRollingFrictionThreshold = 0.f;//faster but lower quality
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///create a few basic rigid bodies
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(20.),btScalar(50.),btScalar(10.)));
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.),btScalar(5.),btScalar(25.)));
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
groundTransform.setRotation(btQuaternion(btVector3(0,0,1),SIMD_PI*0.03));
groundTransform.setOrigin(btVector3(0,0,-28));
groundTransform.setRotation(btQuaternion(btVector3(0,1,0),SIMD_PI*0.03));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
@@ -133,13 +132,13 @@ void RollingFrictionDemo::initPhysics()
{
///create a few basic rigid bodies
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(100.),btScalar(50.),btScalar(100.)));
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(100.),btScalar(100.),btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-54,0));
groundTransform.setOrigin(btVector3(0,0,-54));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
@@ -165,16 +164,16 @@ void RollingFrictionDemo::initPhysics()
// Re-using the same collision is better for memory usage and performance
#define NUM_SHAPES 10
btCollisionShape* colShapes[NUM_SHAPES] = {
new btSphereShape(btScalar(1.)),
new btCapsuleShape(0.5,1),
new btCapsuleShapeX(0.5,1),
new btCapsuleShapeZ(0.5,1),
new btConeShape(0.5,1),
new btConeShapeX(0.5,1),
new btConeShapeZ(0.5,1),
new btCylinderShape(btVector3(0.5,1,0.5)),
new btCylinderShapeX(btVector3(1,0.5,0.5)),
new btCylinderShapeZ(btVector3(0.5,0.5,1)),
new btSphereShape(btScalar(0.5)),
new btCapsuleShape(0.25,0.5),
new btCapsuleShapeX(0.25,0.5),
new btCapsuleShapeZ(0.25,0.5),
new btConeShape(0.25,0.5),
new btConeShapeX(0.25,0.5),
new btConeShapeZ(0.25,0.5),
new btCylinderShape(btVector3(0.25,0.5,0.25)),
new btCylinderShapeX(btVector3(0.5,0.25,0.25)),
new btCylinderShapeZ(btVector3(0.25,0.25,0.5)),
};
for (int i=0;i<NUM_SHAPES;i++)
m_collisionShapes.push_back(colShapes[i]);
@@ -200,9 +199,9 @@ void RollingFrictionDemo::initPhysics()
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
startTransform.setOrigin(SCALING*btVector3(
btScalar(2.0*i + start_x),
btScalar(20+2.0*k + start_y),
btScalar(2.0*j + start_z)));
btScalar(2.0*i + start_x)+25,
btScalar(2.0*j + start_z),
btScalar(20+2.0*k + start_y)));
shapeIndex++;
@@ -230,10 +229,19 @@ void RollingFrictionDemo::initPhysics()
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
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()
{

146
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -6,7 +6,7 @@
#include "Win32SharedMemory.h"
#include "SharedMemoryCommon.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../Utils/b3ResourcePath.h"
class PhysicsClient : public SharedMemoryCommon
{
@@ -24,6 +24,8 @@ protected:
void createClientCommand();
void createButton(const char* name, int id, bool isTrigger );
public:
PhysicsClient(GUIHelperInterface* helper);
@@ -57,23 +59,13 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
switch (buttonId)
{
case CMD_LOAD_URDF:
{
cl->submitCommand(CMD_LOAD_URDF);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
case CMD_REQUEST_ACTUAL_STATE:
{
cl->submitCommand(CMD_REQUEST_ACTUAL_STATE);
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
cl->submitCommand(CMD_STEP_FORWARD_SIMULATION);
break;
}
case CMD_SHUTDOWN:
case CMD_SEND_BULLET_DATA_STREAM:
{
cl->submitCommand(CMD_SHUTDOWN);
cl->submitCommand(buttonId);
break;
}
@@ -116,50 +108,26 @@ PhysicsClient::~PhysicsClient()
delete m_sharedMemory;
}
void PhysicsClient::createButton(const char* name, int buttonId, bool isTrigger )
{
ButtonParams button(name,buttonId, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
void PhysicsClient::initPhysics()
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
{
bool isTrigger = false;
ButtonParams button("Load URDF",CMD_LOAD_URDF, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
bool isTrigger = false;
{
bool isTrigger = false;
ButtonParams button("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
{
bool isTrigger = false;
ButtonParams button("Shut Down",CMD_SHUTDOWN, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
} else
{
m_userCommandRequests.push_back(CMD_LOAD_URDF);
@@ -167,7 +135,7 @@ void PhysicsClient::initPhysics()
//m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
//m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
@@ -225,6 +193,17 @@ void PhysicsClient::processServerCommands()
m_serverLoadUrdfOK = false;
break;
}
case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED:
{
break;
}
case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED:
{
break;
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
b3Printf("Received actual state\n");
@@ -312,6 +291,67 @@ void PhysicsClient::createClientCommand()
{
b3Warning("Server already loaded URDF, no client command submitted\n");
}
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
if (m_serverLoadUrdfOK)
{
b3Printf("Requesting create box collision shape\n");
m_testBlock1->m_clientCommands[0].m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
m_testBlock1->m_numClientCommands++;
} else
{
b3Warning("No URDF loaded\n");
}
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
b3Printf("Sending a Bullet Data Stream\n");
///The idea is to pass a stream of chunks from client to server
///over shared memory. The server will process it
///Initially we will just copy an entire .bullet file into shared
///memory but we can also send individual chunks one at a time
///so it becomes a streaming solution
///In addition, we can make a separate API to create those chunks
///if needed, instead of using a 3d modeler or the Bullet SDK btSerializer
char relativeFileName[1024];
const char* fileName = "slope.bullet";
bool fileFound = b3ResourcePath::findResourcePath(fileName,relativeFileName,1024);
if (fileFound)
{
FILE *fp = fopen(relativeFileName, "rb");
if (fp)
{
fseek(fp, 0L, SEEK_END);
int mFileLen = ftell(fp);
fseek(fp, 0L, SEEK_SET);
if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
{
fread(m_testBlock1->m_bulletStreamDataClientToServer, mFileLen, 1, fp);
fclose(fp);
m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_BULLET_DATA_STREAM;
m_testBlock1->m_clientCommands[0].m_dataStreamArguments.m_streamChunkLength = mFileLen;
m_testBlock1->m_numClientCommands++;
b3Printf("Send bullet data stream command\n");
} else
{
b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
} else
{
b3Warning("Cannot open file %s\n", relativeFileName);
}
} else
{
b3Warning("Cannot find file %s\n", fileName);
}
break;
}
case CMD_REQUEST_ACTUAL_STATE:

41
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -7,6 +7,7 @@
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
#include "SharedMemoryCommon.h"
@@ -17,6 +18,8 @@ class PhysicsServer : public SharedMemoryCommon
SharedMemoryExampleData* m_testBlock1;
btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks;
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
bool m_wantsShutdown;
public:
@@ -196,6 +199,29 @@ void PhysicsServer::stepSimulation(float deltaTime)
//consume the command
switch (clientCmd.m_type)
{
case CMD_SEND_BULLET_DATA_STREAM:
{
b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength);
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_dynamicsWorld);
this->m_worldImporters.push_back(worldImporter);
bool completedOk = worldImporter->loadFileFromMemory(m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
if (completedOk)
{
serverCmd.m_type =CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED;
} else
{
serverCmd.m_type =CMD_BULLET_DATA_STREAM_RECEIVED_FAILED;
}
m_testBlock1->m_numServerCommands++;
break;
}
case CMD_LOAD_URDF:
{
b3Printf("Processed CMD_LOAD_URDF:%s",clientCmd.m_urdfArguments.m_urdfFileName);
@@ -330,6 +356,21 @@ void PhysicsServer::stepSimulation(float deltaTime)
wantsShutdown = true;
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
btVector3 halfExtents(30,30,1);
btTransform startTrans;
startTrans.setIdentity();
startTrans.setOrigin(btVector3(0,0,-4));
btCollisionShape* shape = createBoxShape(halfExtents);
btScalar mass = 0.f;
createRigidBody(mass,startTrans,shape);
this->m_guiHelper->autogenerateGraphicsObjects(this->m_dynamicsWorld);
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED;
m_testBlock1->m_numServerCommands++;
break;
}
default:
{
b3Error("Unsupported command encountered");

10
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -8,6 +8,8 @@ enum SharedMemoryServerCommand
{
CMD_URDF_LOADING_COMPLETED,
CMD_URDF_LOADING_FAILED,
CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,
CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,
CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED,
CMD_RIGID_BODY_CREATION_COMPLETED,
CMD_SET_JOINT_FEEDBACK_COMPLETED,
@@ -20,6 +22,7 @@ enum SharedMemoryServerCommand
enum SharedMemoryClientCommand
{
CMD_LOAD_URDF,
CMD_SEND_BULLET_DATA_STREAM,
CMD_CREATE_BOX_COLLISION_SHAPE,
CMD_DELETE_BOX_COLLISION_SHAPE,
CMD_CREATE_RIGID_BODY,
@@ -45,6 +48,12 @@ struct UrdfArgs
bool m_useFixedBase;
};
struct BulletDataStreamArgs
{
int m_streamChunkLength;
};
struct SetJointFeedbackArgs
{
int m_bodyUniqueId;
@@ -96,6 +105,7 @@ struct SharedMemoryCommand
union
{
UrdfArgs m_urdfArguments;
BulletDataStreamArgs m_dataStreamArguments;
StepSimulationArgs m_stepSimulationArguments;
SendDesiredStateArgs m_sendDesiredQandUStateCommandArgument;
RequestActualStateArgs m_requestActualStateInformationCommandArgument;

3
examples/SharedMemory/SharedMemoryInterface.h
View File

@@ -4,6 +4,7 @@
#define SHARED_MEMORY_KEY 12347
#define SHARED_MEMORY_MAGIC_NUMBER 64738
#define SHARED_MEMORY_MAX_COMMANDS 64
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024)
#include "SharedMemoryCommands.h"
@@ -31,6 +32,8 @@ struct SharedMemoryExampleData
double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM];
double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
char m_bulletStreamDataClientToServer[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
};
#define SHARED_MEMORY_SIZE sizeof(SharedMemoryExampleData)

2
examples/SharedMemory/premake4.lua
View File

@@ -33,6 +33,8 @@ files {
"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/URDF2Bullet.h",
"../Utils/b3ResourcePath.cpp",
"../../Extras/Serialize/BulletWorldImporter/*",
"../../Extras/Serialize/BulletFileLoader/*",
"../Importers/ImportURDFDemo/URDFImporterInterface.h",
"../Importers/ImportURDFDemo/URDFJointTypes.h",
"../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",

6
src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h
View File

@@ -17,8 +17,8 @@
* 3. This notice may not be removed or altered from any source distribution.
*/
// Auto generated from Bullet/Extras/HeaderGenerator/bulletGenerate.py
#ifndef __BULLET_H__
#define __BULLET_H__
#ifndef __BULLET2_H__
#define __BULLET2_H__
namespace Bullet3SerializeBullet2 {
// put an empty struct in the case
@@ -1050,4 +1050,4 @@ typedef struct bInvalidHandle {
}
#endif//__BULLET_H__
#endif//__BULLET2_H__

2
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
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);
}

96
src/BulletDynamics/Featherstone/btMultiBody.cpp
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;
}

122
src/BulletDynamics/Featherstone/btMultiBody.h
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

36
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
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);
}
}
}

4
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
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

8
src/BulletDynamics/Featherstone/btMultiBodyLink.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);

76
src/LinearMath/btQuaternion.h
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_

2
src/LinearMath/btScalar.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()
{

1060
src/LinearMath/btSerializer.cpp
View File

File diff suppressed because it is too large Load Diff

6
src/LinearMath/btSerializer.h
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')
@@ -206,7 +208,7 @@ protected:
void writeDNA()
virtual void writeDNA()
{
btChunk* dnaChunk = allocate(m_dnaLength,1);
memcpy(dnaChunk->m_oldPtr,m_dna,m_dnaLength);
@@ -465,7 +467,7 @@ public:
buffer[9] = '2';
buffer[10] = '8';
buffer[11] = '3';
buffer[11] = '4';
}