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

add additional demos/examples for MultiBody and Constraints

Browse Source
This commit is contained in:
erwin coumans
2015-04-22 13:02:27 -07:00
parent 13396d32bf
commit f2aef6b73e
9 changed files with 1368 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

150
examples/Constraints/ConstraintPhysicsSetup.cpp Normal file
View File

@@ -0,0 +1,150 @@
#include "ConstraintPhysicsSetup.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
struct ConstraintPhysicsSetup : public CommonRigidBodyBase
{
ConstraintPhysicsSetup(struct GUIHelperInterface* helper);
virtual ~ConstraintPhysicsSetup();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
};
ConstraintPhysicsSetup::ConstraintPhysicsSetup(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
{
}
ConstraintPhysicsSetup::~ConstraintPhysicsSetup()
{
}
static btScalar val;
static btScalar targetVel=0;
static btScalar maxImpulse=10000;
static btHingeAccumulatedAngleConstraint* spDoorHinge=0;
static btScalar actualHingeVelocity=0.f;
static btVector3 btAxisA(0,1,0);
void ConstraintPhysicsSetup::stepSimulation(float deltaTime)
{
val=spDoorHinge->getAccumulatedHingeAngle()*SIMD_DEGS_PER_RAD;
if (m_dynamicsWorld)
{
spDoorHinge->enableAngularMotor(true,targetVel,maxImpulse);
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
btHingeConstraint* hinge = spDoorHinge;
if (hinge)
{
const btRigidBody& bodyA = hinge->getRigidBodyA();
const btRigidBody& bodyB = hinge->getRigidBodyB();
btTransform trA = bodyA.getWorldTransform();
btVector3 angVelA = bodyA.getAngularVelocity();
btVector3 angVelB = bodyB.getAngularVelocity();
{
btVector3 ax1 = trA.getBasis()*hinge->getFrameOffsetA().getBasis().getColumn(2);
btScalar vel = angVelA.dot(ax1);
vel -= angVelB.dot(ax1);
printf("hinge velocity (q) = %f\n", vel);
actualHingeVelocity=vel;
}
btVector3 ortho0,ortho1;
btPlaneSpace1(btAxisA,ortho0,ortho1);
{
btScalar vel2 = angVelA.dot(ortho0);
vel2 -= angVelB.dot(ortho0);
printf("hinge orthogonal1 velocity (q) = %f\n", vel2);
}
{
btScalar vel0 = angVelA.dot(ortho1);
vel0 -= angVelB.dot(ortho1);
printf("hinge orthogonal0 velocity (q) = %f\n", vel0);
}
}
}
}
void ConstraintPhysicsSetup::initPhysics()
{
m_guiHelper->setUpAxis(1);
createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
int mode = btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawConstraintLimits;
m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode);
{
SliderParams slider("target vel",&targetVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("max impulse",&maxImpulse);
slider.m_minVal=0;
slider.m_maxVal=1000;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("actual vel",&actualHingeVelocity);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
val=1.f;
{
SliderParams slider("angle",&val);
slider.m_minVal=-720;
slider.m_maxVal=720;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{ // create a door using hinge constraint attached to the world
btCollisionShape* pDoorShape = new btBoxShape(btVector3(2.0f, 5.0f, 0.2f));
m_collisionShapes.push_back(pDoorShape);
btTransform doorTrans;
doorTrans.setIdentity();
doorTrans.setOrigin(btVector3(-5.0f, -2.0f, 0.0f));
btRigidBody* pDoorBody = createRigidBody( 1.0, doorTrans, pDoorShape);
pDoorBody->setActivationState(DISABLE_DEACTIVATION);
const btVector3 btPivotA(10.f + 2.1f, -2.0f, 0.0f ); // right next to the door slightly outside
spDoorHinge = new btHingeAccumulatedAngleConstraint( *pDoorBody, btPivotA, btAxisA );
m_dynamicsWorld->addConstraint(spDoorHinge);
spDoorHinge->setDbgDrawSize(btScalar(5.f));
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
class ExampleInterface* ConstraintCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option)
{
return new ConstraintPhysicsSetup(helper);
}

6
examples/Constraints/ConstraintPhysicsSetup.h Normal file
View File

@@ -0,0 +1,6 @@
#ifndef CONSTAINT_PHYSICS_SETUP_H
#define CONSTAINT_PHYSICS_SETUP_H
class ExampleInterface* ConstraintCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option);
#endif //CONSTAINT_PHYSICS_SETUP_H

479
examples/Constraints/Dof6Spring2Setup.cpp Normal file
View File

@@ -0,0 +1,479 @@
#include "Dof6Spring2Setup.h"
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#ifndef M_PI_2
#define M_PI_2 1.57079632679489661923
#endif
#ifndef M_PI_4
#define M_PI_4 0.785398163397448309616
#endif
extern float g_additionalBodyMass;
//comment this out to compare with original spring constraint
#define USE_6DOF2
#ifdef USE_6DOF2
#define CONSTRAINT_TYPE btGeneric6DofSpring2Constraint
#define EXTRAPARAMS
#else
#define CONSTRAINT_TYPE btGeneric6DofSpringConstraint
#define EXTRAPARAMS ,true
#endif
#include "../CommonInterfaces/CommonRigidBodyBase.h"
struct Dof6Spring2Setup : public CommonRigidBodyBase
{
struct Dof6Spring2SetupInternalData* m_data;
Dof6Spring2Setup(struct GUIHelperInterface* helper);
virtual ~Dof6Spring2Setup();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
void animate();
};
struct Dof6Spring2SetupInternalData
{
btRigidBody* m_TranslateSpringBody;
btRigidBody* m_TranslateSpringBody2;
btRigidBody* m_RotateSpringBody;
btRigidBody* m_RotateSpringBody2;
btRigidBody* m_BouncingTranslateBody;
btRigidBody* m_MotorBody;
btRigidBody* m_ServoMotorBody;
btRigidBody* m_ChainLeftBody;
btRigidBody* m_ChainRightBody;
CONSTRAINT_TYPE* m_ServoMotorConstraint;
CONSTRAINT_TYPE* m_ChainLeftConstraint;
CONSTRAINT_TYPE* m_ChainRightConstraint;
float mDt;
unsigned int frameID;
Dof6Spring2SetupInternalData()
: mDt(1./60.),frameID(0)
{
}
};
Dof6Spring2Setup::Dof6Spring2Setup(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
{
m_data = new Dof6Spring2SetupInternalData;
}
Dof6Spring2Setup::~Dof6Spring2Setup()
{
exitPhysics();
delete m_data;
}
void Dof6Spring2Setup::initPhysics()
{
// Setup the basic world
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
btVector3 worldAabbMin(-10000,-10000,-10000);
btVector3 worldAabbMax(10000,10000,10000);
m_broadphase = new btAxisSweep3 (worldAabbMin, worldAabbMax);
/////// uncomment the corresponding line to test a solver.
//m_solver = new btSequentialImpulseConstraintSolver;
m_solver = new btNNCGConstraintSolver;
//m_solver = new btMLCPSolver(new btSolveProjectedGaussSeidel());
//m_solver = new btMLCPSolver(new btDantzigSolver());
//m_solver = new btMLCPSolver(new btLemkeSolver());
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
m_dynamicsWorld->getDispatchInfo().m_useContinuous = true;
m_dynamicsWorld->setGravity(btVector3(0,0,0));
// Setup a big ground box
{
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(5.),btScalar(200.)));
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-10,0));
#define CREATE_GROUND_COLLISION_OBJECT 1
#ifdef CREATE_GROUND_COLLISION_OBJECT
btCollisionObject* fixedGround = new btCollisionObject();
fixedGround->setCollisionShape(groundShape);
fixedGround->setWorldTransform(groundTransform);
m_dynamicsWorld->addCollisionObject(fixedGround);
#else
localCreateRigidBody(btScalar(0.),groundTransform,groundShape);
#endif //CREATE_GROUND_COLLISION_OBJECT
}
m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
btCollisionShape* shape;
btVector3 localInertia(0,0,0);
btDefaultMotionState* motionState;
btTransform bodyTransform;
btScalar mass;
btTransform localA;
btTransform localB;
CONSTRAINT_TYPE* constraint;
//static body centered in the origo
mass = 0.0;
shape= new btBoxShape(btVector3(0.5,0.5,0.5));
localInertia = btVector3(0,0,0);
bodyTransform.setIdentity();
motionState = new btDefaultMotionState(bodyTransform);
btRigidBody* staticBody = new btRigidBody(mass,motionState,shape,localInertia);
/////////// box with undamped translate spring attached to static body
/////////// the box should oscillate left-to-right forever
{
mass = 1.0;
shape= new btBoxShape(btVector3(0.5,0.5,0.5));
shape->calculateLocalInertia(mass,localInertia);
bodyTransform.setIdentity();
bodyTransform.setOrigin(btVector3(-2,0,-5));
motionState = new btDefaultMotionState(bodyTransform);
m_data->m_TranslateSpringBody = new btRigidBody(mass,motionState,shape,localInertia);
m_data->m_TranslateSpringBody->setActivationState(DISABLE_DEACTIVATION);
m_dynamicsWorld->addRigidBody(m_data->m_TranslateSpringBody);
localA.setIdentity();localA.getOrigin() = btVector3(0,0,-5);
localB.setIdentity();
constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_TranslateSpringBody, localA, localB EXTRAPARAMS);
constraint->setLimit(0, 1,-1);
constraint->setLimit(1, 0, 0);
constraint->setLimit(2, 0, 0);
constraint->setLimit(3, 0, 0);
constraint->setLimit(4, 0, 0);
constraint->setLimit(5, 0, 0);
constraint->enableSpring(0, true);
constraint->setStiffness(0, 100);
#ifdef USE_6DOF2
constraint->setDamping(0, 0);
#else
constraint->setDamping(0, 1);
#endif
constraint->setEquilibriumPoint(0, 0);
m_dynamicsWorld->addConstraint(constraint, true);
}
/////////// box with rotate spring, attached to static body
/////////// box should swing (rotate) left-to-right forever
{
mass = 1.0;
shape= new btBoxShape(btVector3(0.5,0.5,0.5));
shape->calculateLocalInertia(mass,localInertia);
bodyTransform.setIdentity();
bodyTransform.getBasis().setEulerZYX(0,0,M_PI_2);
motionState = new btDefaultMotionState(bodyTransform);
m_data->m_RotateSpringBody = new btRigidBody(mass,motionState,shape,localInertia);
m_data->m_RotateSpringBody->setActivationState(DISABLE_DEACTIVATION);
m_dynamicsWorld->addRigidBody(m_data->m_RotateSpringBody);
localA.setIdentity();localA.getOrigin() = btVector3(0,0,0);
localB.setIdentity();localB.setOrigin(btVector3(0,0.5,0));
constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_RotateSpringBody, localA, localB EXTRAPARAMS);
constraint->setLimit(0, 0, 0);
constraint->setLimit(1, 0, 0);
constraint->setLimit(2, 0, 0);
constraint->setLimit(3, 0, 0);
constraint->setLimit(4, 0, 0);
constraint->setLimit(5, 1, -1);
constraint->enableSpring(5, true);
constraint->setStiffness(5, 100);
#ifdef USE_6DOF2
constraint->setDamping(5, 0);
#else
constraint->setDamping(5, 1);
#endif
constraint->setEquilibriumPoint(0, 0);
m_dynamicsWorld->addConstraint(constraint, true);
}
/////////// box with bouncing constraint, translation is bounced at the positive x limit, but not at the negative limit
/////////// bouncing can not be set independently at low and high limits, so two constraints will be created: one that defines the low (non bouncing) limit, and one that defines the high (bouncing) limit
/////////// the box should move to the left (as an impulse will be applied to it periodically) until it reaches its limit, then bounce back
{
mass = 1.0;
shape= new btBoxShape(btVector3(0.5,0.5,0.5));
shape->calculateLocalInertia(mass,localInertia);
bodyTransform.setIdentity();
bodyTransform.setOrigin(btVector3(0,0,-3));
motionState = new btDefaultMotionState(bodyTransform);
m_data->m_BouncingTranslateBody = new btRigidBody(mass,motionState,shape,localInertia);
m_data->m_BouncingTranslateBody->setActivationState(DISABLE_DEACTIVATION);
m_data->m_BouncingTranslateBody->setDeactivationTime(btScalar(20000000));
m_dynamicsWorld->addRigidBody(m_data->m_BouncingTranslateBody);
localA.setIdentity();localA.getOrigin() = btVector3(0,0,0);
localB.setIdentity();
constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_BouncingTranslateBody, localA, localB EXTRAPARAMS);
constraint->setLimit(0, -2, SIMD_INFINITY);
constraint->setLimit(1, 0, 0);
constraint->setLimit(2, -3, -3);
constraint->setLimit(3, 0, 0);
constraint->setLimit(4, 0, 0);
constraint->setLimit(5, 0, 0);
#ifdef USE_6DOF2
constraint->setBounce(0,0);
#else //bounce is named restitution in 6dofspring, but not implemented for translational limit motor, so the following line has no effect
constraint->getTranslationalLimitMotor()->m_restitution = 0.0;
#endif
constraint->setParam(BT_CONSTRAINT_STOP_ERP,0.995,0);
constraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,0);
m_dynamicsWorld->addConstraint(constraint, true);
constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_BouncingTranslateBody, localA, localB EXTRAPARAMS);
constraint->setLimit(0, -SIMD_INFINITY, 2);
constraint->setLimit(1, 0, 0);
constraint->setLimit(2, -3, -3);
constraint->setLimit(3, 0, 0);
constraint->setLimit(4, 0, 0);
constraint->setLimit(5, 0, 0);
#ifdef USE_6DOF2
constraint->setBounce(0,1);
#else //bounce is named restitution in 6dofspring, but not implemented for translational limit motor, so the following line has no effect
constraint->getTranslationalLimitMotor()->m_restitution = 1.0;
#endif
constraint->setParam(BT_CONSTRAINT_STOP_ERP,0.995,0);
constraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,0);
m_dynamicsWorld->addConstraint(constraint, true);
}
/////////// box with rotational motor, attached to static body
/////////// the box should rotate around the y axis
{
mass = 1.0;
shape= new btBoxShape(btVector3(0.5,0.5,0.5));
shape->calculateLocalInertia(mass,localInertia);
bodyTransform.setIdentity();
bodyTransform.setOrigin(btVector3(4,0,0));
motionState = new btDefaultMotionState(bodyTransform);
m_data->m_MotorBody = new btRigidBody(mass,motionState,shape,localInertia);
m_data->m_MotorBody->setActivationState(DISABLE_DEACTIVATION);
m_dynamicsWorld->addRigidBody(m_data->m_MotorBody);
localA.setIdentity();localA.getOrigin() = btVector3(4,0,0);
localB.setIdentity();
constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_MotorBody, localA, localB EXTRAPARAMS);
constraint->setLimit(0, 0, 0);
constraint->setLimit(1, 0, 0);
constraint->setLimit(2, 0, 0);
constraint->setLimit(3, 0, 0);
constraint->setLimit(4, 0, 0);
constraint->setLimit(5, 1,-1);
#ifdef USE_6DOF2
constraint->enableMotor(5,true);
constraint->setTargetVelocity(5,3.f);
constraint->setMaxMotorForce(5,10.f);
#else
constraint->getRotationalLimitMotor(2)->m_enableMotor = true;
constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f;
constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10;
#endif
m_dynamicsWorld->addConstraint(constraint, true);
}
/////////// box with rotational servo motor, attached to static body
/////////// the box should rotate around the y axis until it reaches its target
/////////// the target will be negated periodically
{
mass = 1.0;
shape= new btBoxShape(btVector3(0.5,0.5,0.5));
shape->calculateLocalInertia(mass,localInertia);
bodyTransform.setIdentity();
bodyTransform.setOrigin(btVector3(7,0,0));
motionState = new btDefaultMotionState(bodyTransform);
m_data->m_ServoMotorBody = new btRigidBody(mass,motionState,shape,localInertia);
m_data->m_ServoMotorBody->setActivationState(DISABLE_DEACTIVATION);
m_dynamicsWorld->addRigidBody(m_data->m_ServoMotorBody);
localA.setIdentity();localA.getOrigin() = btVector3(7,0,0);
localB.setIdentity();
constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_ServoMotorBody, localA, localB EXTRAPARAMS);
constraint->setLimit(0, 0, 0);
constraint->setLimit(1, 0, 0);
constraint->setLimit(2, 0, 0);
constraint->setLimit(3, 0, 0);
constraint->setLimit(4, 0, 0);
constraint->setLimit(5, 1,-1);
#ifdef USE_6DOF2
constraint->enableMotor(5,true);
constraint->setTargetVelocity(5,3.f);
constraint->setMaxMotorForce(5,10.f);
constraint->setServo(5,true);
constraint->setServoTarget(5, M_PI_2);
#else
constraint->getRotationalLimitMotor(2)->m_enableMotor = true;
constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f;
constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10;
//servo motor is not implemented in 6dofspring constraint
#endif
m_dynamicsWorld->addConstraint(constraint, true);
m_data->m_ServoMotorConstraint = constraint;
}
////////// chain of boxes linked together with fully limited rotational and translational constraints
////////// the chain will be pulled to the left and to the right periodically. They should strictly stick together.
{
btScalar limitConstraintStrength = 0.6;
int bodycount = 10;
btRigidBody* prevBody = 0;
for(int i = 0; i < bodycount; ++i)
{
mass = 1.0;
shape= new btBoxShape(btVector3(0.5,0.5,0.5));
shape->calculateLocalInertia(mass,localInertia);
bodyTransform.setIdentity();
bodyTransform.setOrigin(btVector3(- i,0,3));
motionState = new btDefaultMotionState(bodyTransform);
btRigidBody* body = new btRigidBody(mass,motionState,shape,localInertia);
body->setActivationState(DISABLE_DEACTIVATION);
m_dynamicsWorld->addRigidBody(body);
if(prevBody != 0)
{
localB.setIdentity();
localB.setOrigin(btVector3(0.5,0,0));
btTransform localA;
localA.setIdentity();
localA.setOrigin(btVector3(-0.5,0,0));
CONSTRAINT_TYPE* constraint = new CONSTRAINT_TYPE(*prevBody, *body, localA, localB EXTRAPARAMS);
constraint->setLimit(0, -0.01, 0.01);
constraint->setLimit(1, 0, 0);
constraint->setLimit(2, 0, 0);
constraint->setLimit(3, 0, 0);
constraint->setLimit(4, 0, 0);
constraint->setLimit(5, 0, 0);
for(int a = 0; a < 6; ++a)
{
constraint->setParam(BT_CONSTRAINT_STOP_ERP,0.9,a);
constraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,a);
}
m_dynamicsWorld->addConstraint(constraint, true);
if(i < bodycount - 1)
{
localA.setIdentity();localA.getOrigin() = btVector3(0,0,3);
localB.setIdentity();
CONSTRAINT_TYPE* constraintZY = new CONSTRAINT_TYPE(*staticBody, *body, localA, localB EXTRAPARAMS);
constraintZY->setLimit(0, 1, -1);
m_dynamicsWorld->addConstraint(constraintZY, true);
}
}
else
{
localA.setIdentity();localA.getOrigin() = btVector3(bodycount,0,3);
localB.setIdentity();
localB.setOrigin(btVector3(0,0,0));
m_data->m_ChainLeftBody = body;
m_data->m_ChainLeftConstraint = new CONSTRAINT_TYPE(*staticBody, *body, localA, localB EXTRAPARAMS);
m_data->m_ChainLeftConstraint->setLimit(3,0,0);
m_data->m_ChainLeftConstraint->setLimit(4,0,0);
m_data->m_ChainLeftConstraint->setLimit(5,0,0);
for(int a = 0; a < 6; ++a)
{
m_data->m_ChainLeftConstraint->setParam(BT_CONSTRAINT_STOP_ERP,limitConstraintStrength,a);
m_data->m_ChainLeftConstraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,a);
}
m_dynamicsWorld->addConstraint(m_data->m_ChainLeftConstraint, true);
}
prevBody = body;
}
m_data->m_ChainRightBody = prevBody;
localA.setIdentity();localA.getOrigin() = btVector3(-bodycount,0,3);
localB.setIdentity();
localB.setOrigin(btVector3(0,0,0));
m_data->m_ChainRightConstraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_ChainRightBody, localA, localB EXTRAPARAMS);
m_data->m_ChainRightConstraint->setLimit(3,0,0);
m_data->m_ChainRightConstraint->setLimit(4,0,0);
m_data->m_ChainRightConstraint->setLimit(5,0,0);
for(int a = 0; a < 6; ++a)
{
m_data->m_ChainRightConstraint->setParam(BT_CONSTRAINT_STOP_ERP,limitConstraintStrength,a);
m_data->m_ChainRightConstraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,a);
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void Dof6Spring2Setup::animate()
{
/////// servo motor: flip its target periodically
#ifdef USE_6DOF2
static float servoNextFrame = -1;
btScalar pos = m_data->m_ServoMotorConstraint->getRotationalLimitMotor(2)->m_currentPosition;
btScalar target = m_data->m_ServoMotorConstraint->getRotationalLimitMotor(2)->m_servoTarget;
if(servoNextFrame < 0)
{
m_data->m_ServoMotorConstraint->getRotationalLimitMotor(2)->m_servoTarget *= -1;
servoNextFrame = 3.0;
}
servoNextFrame -= m_data->mDt;
#endif
/////// constraint chain: pull the chain left and right periodically
static float chainNextFrame = -1;
static bool left = true;
if(chainNextFrame < 0)
{
if(!left)
{
m_data->m_ChainRightBody->setActivationState(ACTIVE_TAG);
m_dynamicsWorld->removeConstraint(m_data->m_ChainRightConstraint);
m_dynamicsWorld->addConstraint(m_data->m_ChainLeftConstraint, true);
}
else
{
m_data->m_ChainLeftBody->setActivationState(ACTIVE_TAG);
m_dynamicsWorld->removeConstraint(m_data->m_ChainLeftConstraint);
m_dynamicsWorld->addConstraint(m_data->m_ChainRightConstraint, true);
}
chainNextFrame = 3.0;
left = !left;
}
chainNextFrame -= m_data->mDt;
/////// bouncing constraint: push the box periodically
m_data->m_BouncingTranslateBody->setActivationState(ACTIVE_TAG);
static float bounceNextFrame = -1;
if(bounceNextFrame < 0)
{
m_data->m_BouncingTranslateBody->applyCentralImpulse(btVector3(10,0,0));
bounceNextFrame = 3.0;
}
bounceNextFrame -= m_data->mDt;
m_data->frameID++;
}
void Dof6Spring2Setup::stepSimulation(float deltaTime)
{
animate();
m_dynamicsWorld->stepSimulation(deltaTime);
}
class ExampleInterface* Dof6Spring2CreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option)
{
return new Dof6Spring2Setup(helper);
}

6
examples/Constraints/Dof6Spring2Setup.h Normal file
View File

@@ -0,0 +1,6 @@
#ifndef GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H
#define GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H
class ExampleInterface* Dof6Spring2CreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option);
#endif //GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H

15
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -17,6 +17,10 @@
#include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
#include "../GyroscopicDemo/GyroscopicSetup.h"
#include "../Constraints/Dof6Spring2Setup.h"
#include "../Constraints/ConstraintPhysicsSetup.h"
#include "../MultiBody/TestJointTorqueSetup.h"
#include "../MultiBody/MultiDofDemo.h"
struct ExampleEntry
{
@@ -42,8 +46,15 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Gyroscopic", GyroscopicCreateFunc),
ExampleEntry(1,"Planar 2D",Planar2DCreateFunc),
ExampleEntry(1,"Constraints",ConstraintCreateFunc),
ExampleEntry(1,"6DofSpring2",Dof6Spring2CreateFunc),
//#ifndef _DEBUG
ExampleEntry(0,"MultiBody",0),
ExampleEntry(1,"TestJointTorque",TestJointTorqueCreateFunc),
ExampleEntry(1,"MultiDofCreateFunc",MultiDofCreateFunc),
#ifndef _DEBUG
ExampleEntry(0,"Benchmarks", 0),
ExampleEntry(1,"3000 boxes", BenchmarkCreateFunc, 1),
ExampleEntry(1,"1000 stack", BenchmarkCreateFunc, 2),
@@ -52,7 +63,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Prim vs Mesh", BenchmarkCreateFunc, 5),
ExampleEntry(1,"Convex vs Mesh", BenchmarkCreateFunc, 6),
ExampleEntry(1,"Raycast", BenchmarkCreateFunc, 7),
//#endif
#endif
ExampleEntry(0,"Importers", 0),

424
examples/MultiBody/MultiDofDemo.cpp Normal file
View File

@@ -0,0 +1,424 @@
#include "MultiDofDemo.h"
#include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "../OpenGLWindow/GLInstancingRenderer.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
class MultiDofDemo : public CommonMultiBodyBase
{
public:
MultiDofDemo(GUIHelperInterface* helper);
virtual ~MultiDofDemo();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents, bool spherical = false, bool floating = false);
void addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents);
void addBoxes_testMultiDof();
};
static bool g_floatingBase = false;
static bool g_firstInit = true;
static float scaling = 0.4f;
static float friction = 1.;
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_Z 5
//maximum number of objects (and allow user to shoot additional boxes)
#define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024)
#define START_POS_X -5
//#define START_POS_Y 12
#define START_POS_Y 2
#define START_POS_Z -3
MultiDofDemo::MultiDofDemo(GUIHelperInterface* helper)
:CommonMultiBodyBase(helper)
{
m_guiHelper->setUpAxis(1);
}
MultiDofDemo::~MultiDofDemo()
{
}
void MultiDofDemo::stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1./240.f;
m_dynamicsWorld->stepSimulation(deltaTime,10,internalTimeStep);
}
void MultiDofDemo::initPhysics()
{
if(g_firstInit)
{
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10.*scaling));
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50);
g_firstInit = false;
}
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
//Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support
btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver;
m_solver = sol;
//use btMultiBodyDynamicsWorld for Featherstone btMultiBody support
btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher,m_broadphase,sol,m_collisionConfiguration);
m_dynamicsWorld = world;
// m_dynamicsWorld->setDebugDrawer(&gDebugDraw);
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
///create a few basic rigid bodies
btVector3 groundHalfExtents(50,50,50);
btCollisionShape* groundShape = new btBoxShape(groundHalfExtents);
//groundShape->initializePolyhedralFeatures();
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,00));
/////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////
bool damping = true;
bool gyro = true;
int numLinks = 5;
bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool multibodyOnly = false;
bool canSleep = true;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
g_floatingBase = ! g_floatingBase;
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if(!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}else
{ mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0, -9.81 ,0));
//////////////////////////////////////////////
if(numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
if(mbC->isMultiDof())
mbC->setJointPosMultiDof(0, &q0);
else
mbC->setJointPos(0, q0);
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
///
addColliders_testMultiDof(mbC, world, baseHalfExtents, linkHalfExtents);
/////////////////////////////////////////////////////////////////
btScalar groundHeight = -51.55;
if (!multibodyOnly)
{
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,groundHeight,0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body,1,1+2);//,1,1+2);
}
/////////////////////////////////////////////////////////////////
if(!multibodyOnly)
{
btVector3 halfExtents(.5,.5,.5);
btBoxShape* colShape = new btBoxShape(halfExtents);
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
startTransform.setOrigin(btVector3(
btScalar(0.0),
-0.95,
btScalar(0.0)));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body);//,1,1+2);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
/////////////////////////////////////////////////////////////////
}
btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld *pWorld, int numLinks, const btVector3 &basePosition, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents, bool spherical, bool floating)
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
if(baseMass)
{
btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool canSleep = false;
bool isMultiDof = true;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = 1.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI/ 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for(int i = 0; i < numLinks; ++i)
{
if(!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
else
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
pMultiBody->finalizeMultiDof();
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents)
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
{
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
pWorld->addCollisionObject(col, 2,1+2);
col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1];
local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i)));
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
pWorld->addCollisionObject(col,2,1+2);
pMultiBody->getLink(i).m_collider=col;
}
}
void MultiDofDemo::addBoxes_testMultiDof()
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btBoxShape* colShape = new btBoxShape(btVector3(1,1,1));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
float start_x = START_POS_X - ARRAY_SIZE_X/2;
float start_y = START_POS_Y;
float start_z = START_POS_Z - ARRAY_SIZE_Z/2;
for (int k=0;k<ARRAY_SIZE_Y;k++)
{
for (int i=0;i<ARRAY_SIZE_X;i++)
{
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
startTransform.setOrigin(btVector3(
btScalar(3.0*i + start_x),
btScalar(3.0*k + start_y),
btScalar(3.0*j + start_z)));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body);//,1,1+2);
}
}
}
}
class ExampleInterface* MultiDofCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option)
{
return new MultiDofDemo(helper);
}

8
examples/MultiBody/MultiDofDemo.h Normal file
View File

@@ -0,0 +1,8 @@
#ifndef MULTI_DOF_DEMO_H
#define MULTI_DOF_DEMO_H
class ExampleInterface* MultiDofCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option);
#endif //MULTI_DOF_DEMO_H

275
examples/MultiBody/TestJointTorqueSetup.cpp Normal file
View File

@@ -0,0 +1,275 @@
//test addJointTorque
#include "TestJointTorqueSetup.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
struct TestJointTorqueSetup : public CommonMultiBodyBase
{
btMultiBody* m_multiBody;
public:
TestJointTorqueSetup(struct GUIHelperInterface* helper);
virtual ~TestJointTorqueSetup();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
};
TestJointTorqueSetup::TestJointTorqueSetup(struct GUIHelperInterface* helper)
:CommonMultiBodyBase(helper)
{
}
TestJointTorqueSetup::~TestJointTorqueSetup()
{
}
void TestJointTorqueSetup::initPhysics()
{
int upAxis = 2;
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 0;
m_guiHelper->setUpAxis(upAxis);
this->createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
//create a static ground object
if (0)
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-1.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
{
bool floating = false;
bool damping = true;
bool gyro = true;
int numLinks = 5;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
if(baseMass)
{
btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool isMultiDof = false;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = 1.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI/ 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for(int i = 0; i < numLinks; ++i)
{
if(!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
else
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
//pMultiBody->finalizeMultiDof();
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
///
world->addMultiBody(pMultiBody);
btMultiBody* mbC = pMultiBody;
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if(!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}else
{ mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
btVector3 gravity(0,0,0);
//gravity[upAxis] = -9.81;
m_dynamicsWorld->setGravity(gravity);
//////////////////////////////////////////////
if(numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
if(mbC->isMultiDof())
mbC->setJointPosMultiDof(0, &q0);
else
mbC->setJointPos(0, q0);
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
///
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
double friction = 1;
{
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
m_guiHelper->createCollisionShapeGraphicsObject(box);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
world->addCollisionObject(col, 2,1+2);
btVector3 color(0.0,0.0,0.5);
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1];
local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i)));
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
m_guiHelper->createCollisionShapeGraphicsObject(box);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
world->addCollisionObject(col,2,1+2);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
pMultiBody->getLink(i).m_collider=col;
}
}
}
void TestJointTorqueSetup::stepSimulation(float deltaTime)
{
m_multiBody->addJointTorque(0, 10.0);
m_dynamicsWorld->stepSimulation(deltaTime);
}
class ExampleInterface* TestJointTorqueCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option)
{
return new TestJointTorqueSetup(helper);
}

7
examples/MultiBody/TestJointTorqueSetup.h Normal file
View File

@@ -0,0 +1,7 @@
#ifndef TEST_JOINT_TORQUE_SETUP_H
#define TEST_JOINT_TORQUE_SETUP_H
class ExampleInterface* TestJointTorqueCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option);
#endif //TEST_JOINT_TORQUE_SETUP_H