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

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

551
examples/Constraints/ConstraintDemo.cpp
View File

@@ -13,60 +13,47 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "ConstraintDemo.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btIDebugDraw.h"
#include <stdio.h> //printf debugging
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
///AllConstraintDemo shows how to create a constraint, like Hinge or btGenericD6constraint
class AllConstraintDemo : public CommonRigidBodyBase
class AllConstraintDemo : public CommonRigidBodyBase
{
//keep track of variables to delete memory at the end
void setupEmptyDynamicsWorld();
public:
void setupEmptyDynamicsWorld();
public:
AllConstraintDemo(struct GUIHelperInterface* helper);
virtual ~AllConstraintDemo();
virtual void initPhysics();
virtual void initPhysics();
virtual void exitPhysics();
virtual void exitPhysics();
virtual void resetCamera()
{
float dist = 27;
float pitch = -30;
float yaw = 720;
float targetPos[3]={2,0,-10};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
float targetPos[3] = {2, 0, -10};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
virtual bool keyboardCallback(int key, int state);
virtual bool keyboardCallback(int key, int state);
// for cone-twist motor driving
float m_Time;
class btConeTwistConstraint* m_ctc;
};
#define ENABLE_ALL_DEMOS 1
#define CUBE_HALF_EXTENTS 1.f
@@ -74,14 +61,11 @@ class AllConstraintDemo : public CommonRigidBodyBase
#define SIMD_PI_2 ((SIMD_PI)*0.5f)
#define SIMD_PI_4 ((SIMD_PI)*0.25f)
btTransform sliderTransform;
btVector3 lowerSliderLimit = btVector3(-10,0,0);
btVector3 hiSliderLimit = btVector3(10,0,0);
btVector3 lowerSliderLimit = btVector3(-10, 0, 0);
btVector3 hiSliderLimit = btVector3(10, 0, 0);
btRigidBody* d6body0 =0;
btRigidBody* d6body0 = 0;
btHingeConstraint* spDoorHinge = NULL;
btHingeConstraint* spHingeDynAB = NULL;
@@ -89,22 +73,16 @@ btGeneric6DofConstraint* spSlider6Dof = NULL;
static bool s_bTestConeTwistMotor = false;
void AllConstraintDemo::setupEmptyDynamicsWorld()
void AllConstraintDemo::setupEmptyDynamicsWorld()
{
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
m_solver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
}
void AllConstraintDemo::initPhysics()
void AllConstraintDemo::initPhysics()
{
m_guiHelper->setUpAxis(1);
@@ -115,223 +93,210 @@ void AllConstraintDemo::initPhysics()
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
//btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(40.),btScalar(50.)));
btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),40);
btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0, 1, 0), 40);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-56,0));
groundTransform.setOrigin(btVector3(0, -56, 0));
btRigidBody* groundBody;
groundBody= createRigidBody(0, groundTransform, groundShape);
groundBody = createRigidBody(0, groundTransform, groundShape);
btCollisionShape* shape = new btBoxShape(btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
btCollisionShape* shape = new btBoxShape(btVector3(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS));
m_collisionShapes.push_back(shape);
btTransform trans;
trans.setIdentity();
trans.setOrigin(btVector3(0,20,0));
trans.setOrigin(btVector3(0, 20, 0));
float mass = 1.f;
#if ENABLE_ALL_DEMOS
///gear constraint demo
///gear constraint demo
#define THETA SIMD_PI/4.f
#define THETA SIMD_PI / 4.f
#define L_1 (2 - tan(THETA))
#define L_2 (1 / cos(THETA))
#define RATIO L_2 / L_1
btRigidBody* bodyA=0;
btRigidBody* bodyB=0;
btRigidBody* bodyA = 0;
btRigidBody* bodyB = 0;
{
btCollisionShape* cylA = new btCylinderShape(btVector3(0.2,0.25,0.2));
btCollisionShape* cylB = new btCylinderShape(btVector3(L_1,0.025,L_1));
btCollisionShape* cylA = new btCylinderShape(btVector3(0.2, 0.25, 0.2));
btCollisionShape* cylB = new btCylinderShape(btVector3(L_1, 0.025, L_1));
btCompoundShape* cyl0 = new btCompoundShape();
cyl0->addChildShape(btTransform::getIdentity(),cylA);
cyl0->addChildShape(btTransform::getIdentity(),cylB);
cyl0->addChildShape(btTransform::getIdentity(), cylA);
cyl0->addChildShape(btTransform::getIdentity(), cylB);
btScalar mass = 6.28;
btVector3 localInertia;
cyl0->calculateLocalInertia(mass,localInertia);
btRigidBody::btRigidBodyConstructionInfo ci(mass,0,cyl0,localInertia);
ci.m_startWorldTransform.setOrigin(btVector3(-8,1,-8));
cyl0->calculateLocalInertia(mass, localInertia);
btRigidBody::btRigidBodyConstructionInfo ci(mass, 0, cyl0, localInertia);
ci.m_startWorldTransform.setOrigin(btVector3(-8, 1, -8));
btRigidBody* body = new btRigidBody(ci);//1,0,cyl0,localInertia);
btRigidBody* body = new btRigidBody(ci); //1,0,cyl0,localInertia);
m_dynamicsWorld->addRigidBody(body);
body->setLinearFactor(btVector3(0,0,0));
body->setAngularFactor(btVector3(0,1,0));
body->setLinearFactor(btVector3(0, 0, 0));
body->setAngularFactor(btVector3(0, 1, 0));
bodyA = body;
}
{
btCollisionShape* cylA = new btCylinderShape(btVector3(0.2,0.26,0.2));
btCollisionShape* cylB = new btCylinderShape(btVector3(L_2,0.025,L_2));
btCollisionShape* cylA = new btCylinderShape(btVector3(0.2, 0.26, 0.2));
btCollisionShape* cylB = new btCylinderShape(btVector3(L_2, 0.025, L_2));
btCompoundShape* cyl0 = new btCompoundShape();
cyl0->addChildShape(btTransform::getIdentity(),cylA);
cyl0->addChildShape(btTransform::getIdentity(),cylB);
cyl0->addChildShape(btTransform::getIdentity(), cylA);
cyl0->addChildShape(btTransform::getIdentity(), cylB);
btScalar mass = 6.28;
btVector3 localInertia;
cyl0->calculateLocalInertia(mass,localInertia);
btRigidBody::btRigidBodyConstructionInfo ci(mass,0,cyl0,localInertia);
ci.m_startWorldTransform.setOrigin(btVector3(-10,2,-8));
cyl0->calculateLocalInertia(mass, localInertia);
btRigidBody::btRigidBodyConstructionInfo ci(mass, 0, cyl0, localInertia);
ci.m_startWorldTransform.setOrigin(btVector3(-10, 2, -8));
btQuaternion orn(btVector3(0,0,1),-THETA);
btQuaternion orn(btVector3(0, 0, 1), -THETA);
ci.m_startWorldTransform.setRotation(orn);
btRigidBody* body = new btRigidBody(ci);//1,0,cyl0,localInertia);
body->setLinearFactor(btVector3(0,0,0));
btHingeConstraint* hinge = new btHingeConstraint(*body,btVector3(0,0,0),btVector3(0,1,0),true);
btRigidBody* body = new btRigidBody(ci); //1,0,cyl0,localInertia);
body->setLinearFactor(btVector3(0, 0, 0));
btHingeConstraint* hinge = new btHingeConstraint(*body, btVector3(0, 0, 0), btVector3(0, 1, 0), true);
m_dynamicsWorld->addConstraint(hinge);
bodyB= body;
body->setAngularVelocity(btVector3(0,3,0));
bodyB = body;
body->setAngularVelocity(btVector3(0, 3, 0));
m_dynamicsWorld->addRigidBody(body);
}
btVector3 axisA(0,1,0);
btVector3 axisB(0,1,0);
btQuaternion orn(btVector3(0,0,1),-THETA);
btVector3 axisA(0, 1, 0);
btVector3 axisB(0, 1, 0);
btQuaternion orn(btVector3(0, 0, 1), -THETA);
btMatrix3x3 mat(orn);
axisB = mat.getRow(1);
btGearConstraint* gear = new btGearConstraint(*bodyA,*bodyB, axisA,axisB,RATIO);
m_dynamicsWorld->addConstraint(gear,true);
btGearConstraint* gear = new btGearConstraint(*bodyA, *bodyB, axisA, axisB, RATIO);
m_dynamicsWorld->addConstraint(gear, true);
#endif
#if ENABLE_ALL_DEMOS
//point to point constraint with a breaking threshold
{
trans.setIdentity();
trans.setOrigin(btVector3(1,30,-5));
createRigidBody( mass,trans,shape);
trans.setOrigin(btVector3(0,0,-5));
trans.setOrigin(btVector3(1, 30, -5));
createRigidBody(mass, trans, shape);
trans.setOrigin(btVector3(0, 0, -5));
btRigidBody* body0 = createRigidBody( mass,trans,shape);
trans.setOrigin(btVector3(2*CUBE_HALF_EXTENTS,20,0));
btRigidBody* body0 = createRigidBody(mass, trans, shape);
trans.setOrigin(btVector3(2 * CUBE_HALF_EXTENTS, 20, 0));
mass = 1.f;
// btRigidBody* body1 = 0;//createRigidBody( mass,trans,shape);
btVector3 pivotInA(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,0);
btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,pivotInA);
// btRigidBody* body1 = 0;//createRigidBody( mass,trans,shape);
btVector3 pivotInA(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, 0);
btTypedConstraint* p2p = new btPoint2PointConstraint(*body0, pivotInA);
m_dynamicsWorld->addConstraint(p2p);
p2p ->setBreakingImpulseThreshold(10.2);
p2p->setBreakingImpulseThreshold(10.2);
p2p->setDbgDrawSize(btScalar(5.f));
}
#endif
#if ENABLE_ALL_DEMOS
//point to point constraint (ball socket)
{
btRigidBody* body0 = createRigidBody( mass,trans,shape);
trans.setOrigin(btVector3(2*CUBE_HALF_EXTENTS,20,0));
btRigidBody* body0 = createRigidBody(mass, trans, shape);
trans.setOrigin(btVector3(2 * CUBE_HALF_EXTENTS, 20, 0));
mass = 1.f;
// btRigidBody* body1 = 0;//createRigidBody( mass,trans,shape);
// btRigidBody* body1 = createRigidBody( 0.0,trans,0);
// btRigidBody* body1 = 0;//createRigidBody( mass,trans,shape);
// btRigidBody* body1 = createRigidBody( 0.0,trans,0);
//body1->setActivationState(DISABLE_DEACTIVATION);
//body1->setDamping(0.3,0.3);
btVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS);
btVector3 axisInA(0,0,1);
btVector3 pivotInA(CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS);
btVector3 axisInA(0, 0, 1);
// btVector3 pivotInB = body1 ? body1->getCenterOfMassTransform().inverse()(body0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
// btVector3 axisInB = body1?
// (body1->getCenterOfMassTransform().getBasis().inverse()*(body1->getCenterOfMassTransform().getBasis() * axisInA)) :
// btVector3 pivotInB = body1 ? body1->getCenterOfMassTransform().inverse()(body0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
// btVector3 axisInB = body1?
// (body1->getCenterOfMassTransform().getBasis().inverse()*(body1->getCenterOfMassTransform().getBasis() * axisInA)) :
body0->getCenterOfMassTransform().getBasis() * axisInA;
#define P2P
#ifdef P2P
btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,pivotInA);
btTypedConstraint* p2p = new btPoint2PointConstraint(*body0, pivotInA);
//btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,*body1,pivotInA,pivotInB);
//btTypedConstraint* hinge = new btHingeConstraint(*body0,*body1,pivotInA,pivotInB,axisInA,axisInB);
m_dynamicsWorld->addConstraint(p2p);
p2p->setDbgDrawSize(btScalar(5.f));
#else
btHingeConstraint* hinge = new btHingeConstraint(*body0,pivotInA,axisInA);
btHingeConstraint* hinge = new btHingeConstraint(*body0, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.f;
//float maxMotorImpulse = 0.01;
float targetVelocity = 1.f;
float maxMotorImpulse = 1.0f;
hinge->enableAngularMotor(true,targetVelocity,maxMotorImpulse);
float targetVelocity = 1.f;
float maxMotorImpulse = 1.0f;
hinge->enableAngularMotor(true, targetVelocity, maxMotorImpulse);
m_dynamicsWorld->addConstraint(hinge);
hinge->setDbgDrawSize(btScalar(5.f));
#endif //P2P
#endif //P2P
}
#endif
#if ENABLE_ALL_DEMOS
{
btTransform trans;
trans.setIdentity();
btVector3 worldPos(-20,0,30);
trans.setOrigin(worldPos);
trans.setIdentity();
btVector3 worldPos(-20, 0, 30);
trans.setOrigin(worldPos);
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInB = btTransform::getIdentity();
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInB = btTransform::getIdentity();
btRigidBody* pRbA1 = createRigidBody(mass, trans, shape);
// btRigidBody* pRbA1 = createRigidBody(0.f, trans, shape);
pRbA1->setActivationState(DISABLE_DEACTIVATION);
btRigidBody* pRbA1 = createRigidBody(mass, trans, shape);
// btRigidBody* pRbA1 = createRigidBody(0.f, trans, shape);
pRbA1->setActivationState(DISABLE_DEACTIVATION);
// add dynamic rigid body B1
worldPos.setValue(-30,0,30);
trans.setOrigin(worldPos);
btRigidBody* pRbB1 = createRigidBody(mass, trans, shape);
// btRigidBody* pRbB1 = createRigidBody(0.f, trans, shape);
pRbB1->setActivationState(DISABLE_DEACTIVATION);
// add dynamic rigid body B1
worldPos.setValue(-30, 0, 30);
trans.setOrigin(worldPos);
btRigidBody* pRbB1 = createRigidBody(mass, trans, shape);
// btRigidBody* pRbB1 = createRigidBody(0.f, trans, shape);
pRbB1->setActivationState(DISABLE_DEACTIVATION);
// create slider constraint between A1 and B1 and add it to world
btSliderConstraint* spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true);
// spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, false);
spSlider1->setLowerLinLimit(-15.0F);
spSlider1->setUpperLinLimit(-5.0F);
// spSlider1->setLowerLinLimit(5.0F);
// spSlider1->setUpperLinLimit(15.0F);
// spSlider1->setLowerLinLimit(-10.0F);
// spSlider1->setUpperLinLimit(-10.0F);
// create slider constraint between A1 and B1 and add it to world
spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F);
spSlider1->setUpperAngLimit( SIMD_PI / 3.0F);
btSliderConstraint* spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true);
// spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, false);
spSlider1->setLowerLinLimit(-15.0F);
spSlider1->setUpperLinLimit(-5.0F);
// spSlider1->setLowerLinLimit(5.0F);
// spSlider1->setUpperLinLimit(15.0F);
// spSlider1->setLowerLinLimit(-10.0F);
// spSlider1->setUpperLinLimit(-10.0F);
spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F);
spSlider1->setUpperAngLimit(SIMD_PI / 3.0F);
m_dynamicsWorld->addConstraint(spSlider1, true);
spSlider1->setDbgDrawSize(btScalar(5.f));
m_dynamicsWorld->addConstraint(spSlider1, true);
spSlider1->setDbgDrawSize(btScalar(5.f));
}
#endif
#if ENABLE_ALL_DEMOS
#if ENABLE_ALL_DEMOS
//create a slider, using the generic D6 constraint
{
mass = 1.f;
btVector3 sliderWorldPos(0,10,0);
btVector3 sliderAxis(1,0,0);
btScalar angle=0.f;//SIMD_RADS_PER_DEG * 10.f;
btMatrix3x3 sliderOrientation(btQuaternion(sliderAxis ,angle));
btVector3 sliderWorldPos(0, 10, 0);
btVector3 sliderAxis(1, 0, 0);
btScalar angle = 0.f; //SIMD_RADS_PER_DEG * 10.f;
btMatrix3x3 sliderOrientation(btQuaternion(sliderAxis, angle));
trans.setIdentity();
trans.setOrigin(sliderWorldPos);
//trans.setBasis(sliderOrientation);
sliderTransform = trans;
d6body0 = createRigidBody( mass,trans,shape);
d6body0 = createRigidBody(mass, trans, shape);
d6body0->setActivationState(DISABLE_DEACTIVATION);
btRigidBody* fixedBody1 = createRigidBody(0,trans,0);
btRigidBody* fixedBody1 = createRigidBody(0, trans, 0);
m_dynamicsWorld->addRigidBody(fixedBody1);
btTransform frameInA, frameInB;
@@ -340,54 +305,52 @@ void AllConstraintDemo::initPhysics()
frameInA.setOrigin(btVector3(0., 5., 0.));
frameInB.setOrigin(btVector3(0., 5., 0.));
// bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
bool useLinearReferenceFrameA = true;//use fixed frame A for linear llimits
spSlider6Dof = new btGeneric6DofConstraint(*fixedBody1, *d6body0,frameInA,frameInB,useLinearReferenceFrameA);
// bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
spSlider6Dof = new btGeneric6DofConstraint(*fixedBody1, *d6body0, frameInA, frameInB, useLinearReferenceFrameA);
spSlider6Dof->setLinearLowerLimit(lowerSliderLimit);
spSlider6Dof->setLinearUpperLimit(hiSliderLimit);
//range should be small, otherwise singularities will 'explode' the constraint
// spSlider6Dof->setAngularLowerLimit(btVector3(-1.5,0,0));
// spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0));
// spSlider6Dof->setAngularLowerLimit(btVector3(0,0,0));
// spSlider6Dof->setAngularUpperLimit(btVector3(0,0,0));
spSlider6Dof->setAngularLowerLimit(btVector3(-SIMD_PI,0,0));
spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0));
// spSlider6Dof->setAngularLowerLimit(btVector3(-1.5,0,0));
// spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0));
// spSlider6Dof->setAngularLowerLimit(btVector3(0,0,0));
// spSlider6Dof->setAngularUpperLimit(btVector3(0,0,0));
spSlider6Dof->setAngularLowerLimit(btVector3(-SIMD_PI, 0, 0));
spSlider6Dof->setAngularUpperLimit(btVector3(1.5, 0, 0));
spSlider6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true;
spSlider6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f;
spSlider6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f;
m_dynamicsWorld->addConstraint(spSlider6Dof);
spSlider6Dof->setDbgDrawSize(btScalar(5.f));
}
#endif
#if ENABLE_ALL_DEMOS
{ // create a door using hinge constraint attached to the world
{ // 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);
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
btVector3 btAxisA( 0.0f, 1.0f, 0.0f ); // pointing upwards, aka Y-axis
const btVector3 btPivotA(10.f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
btVector3 btAxisA(0.0f, 1.0f, 0.0f); // pointing upwards, aka Y-axis
spDoorHinge = new btHingeConstraint( *pDoorBody, btPivotA, btAxisA );
spDoorHinge = new btHingeConstraint(*pDoorBody, btPivotA, btAxisA);
// spDoorHinge->setLimit( 0.0f, SIMD_PI_2 );
// spDoorHinge->setLimit( 0.0f, SIMD_PI_2 );
// test problem values
// spDoorHinge->setLimit( -SIMD_PI, SIMD_PI*0.8f);
// spDoorHinge->setLimit( -SIMD_PI, SIMD_PI*0.8f);
// spDoorHinge->setLimit( 1.f, -1.f);
// spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI);
// spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.3f, 0.0f);
// spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
spDoorHinge->setLimit( -SIMD_PI * 0.25f, SIMD_PI * 0.25f );
// spDoorHinge->setLimit( 0.0f, 0.0f );
// spDoorHinge->setLimit( 1.f, -1.f);
// spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI);
// spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.3f, 0.0f);
// spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
spDoorHinge->setLimit(-SIMD_PI * 0.25f, SIMD_PI * 0.25f);
// spDoorHinge->setLimit( 0.0f, 0.0f );
m_dynamicsWorld->addConstraint(spDoorHinge);
spDoorHinge->setDbgDrawSize(btScalar(5.f));
@@ -396,22 +359,22 @@ void AllConstraintDemo::initPhysics()
}
#endif
#if ENABLE_ALL_DEMOS
{ // create a generic 6DOF constraint
{ // create a generic 6DOF constraint
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(10.), btScalar(6.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
// btRigidBody* pBodyA = createRigidBody( mass, tr, shape);
btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape);
// btRigidBody* pBodyA = createRigidBody( 0.0, tr, 0);
tr.getBasis().setEulerZYX(0, 0, 0);
// btRigidBody* pBodyA = createRigidBody( mass, tr, shape);
btRigidBody* pBodyA = createRigidBody(0.0, tr, shape);
// btRigidBody* pBodyA = createRigidBody( 0.0, tr, 0);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(0.), btScalar(6.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
tr.getBasis().setEulerZYX(0, 0, 0);
btRigidBody* pBodyB = createRigidBody(mass, tr, shape);
// btRigidBody* pBodyB = createRigidBody(0.f, tr, shape);
// btRigidBody* pBodyB = createRigidBody(0.f, tr, shape);
pBodyB->setActivationState(DISABLE_DEACTIVATION);
btTransform frameInA, frameInB;
@@ -421,75 +384,72 @@ void AllConstraintDemo::initPhysics()
frameInB.setOrigin(btVector3(btScalar(5.), btScalar(0.), btScalar(0.)));
btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true);
// btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false);
// btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false);
pGen6DOF->setLinearLowerLimit(btVector3(-10., -2., -1.));
pGen6DOF->setLinearUpperLimit(btVector3(10., 2., 1.));
// pGen6DOF->setLinearLowerLimit(btVector3(-10., 0., 0.));
// pGen6DOF->setLinearUpperLimit(btVector3(10., 0., 0.));
// pGen6DOF->setLinearLowerLimit(btVector3(0., 0., 0.));
// pGen6DOF->setLinearUpperLimit(btVector3(0., 0., 0.));
// pGen6DOF->setLinearLowerLimit(btVector3(-10., 0., 0.));
// pGen6DOF->setLinearUpperLimit(btVector3(10., 0., 0.));
// pGen6DOF->setLinearLowerLimit(btVector3(0., 0., 0.));
// pGen6DOF->setLinearUpperLimit(btVector3(0., 0., 0.));
// pGen6DOF->getTranslationalLimitMotor()->m_enableMotor[0] = true;
// pGen6DOF->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
// pGen6DOF->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f;
// pGen6DOF->getTranslationalLimitMotor()->m_enableMotor[0] = true;
// pGen6DOF->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
// pGen6DOF->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f;
// pGen6DOF->setAngularLowerLimit(btVector3(0., SIMD_HALF_PI*0.9, 0.));
// pGen6DOF->setAngularUpperLimit(btVector3(0., -SIMD_HALF_PI*0.9, 0.));
// pGen6DOF->setAngularLowerLimit(btVector3(0., 0., -SIMD_HALF_PI));
// pGen6DOF->setAngularUpperLimit(btVector3(0., 0., SIMD_HALF_PI));
// pGen6DOF->setAngularLowerLimit(btVector3(0., SIMD_HALF_PI*0.9, 0.));
// pGen6DOF->setAngularUpperLimit(btVector3(0., -SIMD_HALF_PI*0.9, 0.));
// pGen6DOF->setAngularLowerLimit(btVector3(0., 0., -SIMD_HALF_PI));
// pGen6DOF->setAngularUpperLimit(btVector3(0., 0., SIMD_HALF_PI));
pGen6DOF->setAngularLowerLimit(btVector3(-SIMD_HALF_PI * 0.5f, -0.75, -SIMD_HALF_PI * 0.8f));
pGen6DOF->setAngularUpperLimit(btVector3(SIMD_HALF_PI * 0.5f, 0.75, SIMD_HALF_PI * 0.8f));
// pGen6DOF->setAngularLowerLimit(btVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f));
// pGen6DOF->setAngularUpperLimit(btVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f));
// pGen6DOF->setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f));
// pGen6DOF->setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI * 0.8f, -SIMD_HALF_PI * 1.98f));
// pGen6DOF->setAngularLowerLimit(btVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f));
// pGen6DOF->setAngularUpperLimit(btVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f));
// pGen6DOF->setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f));
// pGen6DOF->setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI * 0.8f, -SIMD_HALF_PI * 1.98f));
// pGen6DOF->setAngularLowerLimit(btVector3(-0.75,-0.5, -0.5));
// pGen6DOF->setAngularUpperLimit(btVector3(0.75,0.5, 0.5));
// pGen6DOF->setAngularLowerLimit(btVector3(-0.75,0., 0.));
// pGen6DOF->setAngularUpperLimit(btVector3(0.75,0., 0.));
// pGen6DOF->setAngularLowerLimit(btVector3(0., -0.7,0.));
// pGen6DOF->setAngularUpperLimit(btVector3(0., 0.7, 0.));
// pGen6DOF->setAngularLowerLimit(btVector3(-1., 0.,0.));
// pGen6DOF->setAngularUpperLimit(btVector3(1., 0., 0.));
// pGen6DOF->setAngularLowerLimit(btVector3(-0.75,-0.5, -0.5));
// pGen6DOF->setAngularUpperLimit(btVector3(0.75,0.5, 0.5));
// pGen6DOF->setAngularLowerLimit(btVector3(-0.75,0., 0.));
// pGen6DOF->setAngularUpperLimit(btVector3(0.75,0., 0.));
// pGen6DOF->setAngularLowerLimit(btVector3(0., -0.7,0.));
// pGen6DOF->setAngularUpperLimit(btVector3(0., 0.7, 0.));
// pGen6DOF->setAngularLowerLimit(btVector3(-1., 0.,0.));
// pGen6DOF->setAngularUpperLimit(btVector3(1., 0., 0.));
m_dynamicsWorld->addConstraint(pGen6DOF, true);
pGen6DOF->setDbgDrawSize(btScalar(5.f));
}
#endif
#if ENABLE_ALL_DEMOS
{ // create a ConeTwist constraint
{ // create a ConeTwist constraint
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-10.), btScalar(5.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
btRigidBody* pBodyA = createRigidBody( 1.0, tr, shape);
// btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape);
tr.getBasis().setEulerZYX(0, 0, 0);
btRigidBody* pBodyA = createRigidBody(1.0, tr, shape);
// btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-10.), btScalar(-5.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
tr.getBasis().setEulerZYX(0, 0, 0);
btRigidBody* pBodyB = createRigidBody(0.0, tr, shape);
// btRigidBody* pBodyB = createRigidBody(1.0, tr, shape);
// btRigidBody* pBodyB = createRigidBody(1.0, tr, shape);
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInA.getBasis().setEulerZYX(0, 0, SIMD_PI_2);
frameInA.setOrigin(btVector3(btScalar(0.), btScalar(-5.), btScalar(0.)));
frameInB = btTransform::getIdentity();
frameInB.getBasis().setEulerZYX(0,0, SIMD_PI_2);
frameInB.getBasis().setEulerZYX(0, 0, SIMD_PI_2);
frameInB.setOrigin(btVector3(btScalar(0.), btScalar(5.), btScalar(0.)));
m_ctc = new btConeTwistConstraint(*pBodyA, *pBodyB, frameInA, frameInB);
// m_ctc->setLimit(btScalar(SIMD_PI_4), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f);
// m_ctc->setLimit(btScalar(SIMD_PI_4*0.6f), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f, 1.0f); // soft limit == hard limit
m_ctc->setLimit(btScalar(SIMD_PI_4*0.6f), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f, 0.5f);
// m_ctc->setLimit(btScalar(SIMD_PI_4), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f);
// m_ctc->setLimit(btScalar(SIMD_PI_4*0.6f), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f, 1.0f); // soft limit == hard limit
m_ctc->setLimit(btScalar(SIMD_PI_4 * 0.6f), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f, 0.5f);
m_dynamicsWorld->addConstraint(m_ctc, true);
m_ctc->setDbgDrawSize(btScalar(5.f));
// s_bTestConeTwistMotor = true; // use only with old solver for now
@@ -497,32 +457,32 @@ void AllConstraintDemo::initPhysics()
}
#endif
#if ENABLE_ALL_DEMOS
{ // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
{ // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
btRigidBody* pBody = createRigidBody( 1.0, tr, shape);
btRigidBody* pBody = createRigidBody(1.0, tr, shape);
pBody->setActivationState(DISABLE_DEACTIVATION);
const btVector3 btPivotA( 10.0f, 0.0f, 0.0f );
btVector3 btAxisA( 0.0f, 0.0f, 1.0f );
const btVector3 btPivotA(10.0f, 0.0f, 0.0f);
btVector3 btAxisA(0.0f, 0.0f, 1.0f);
btHingeConstraint* pHinge = new btHingeConstraint( *pBody, btPivotA, btAxisA );
// pHinge->enableAngularMotor(true, -1.0, 0.165); // use for the old solver
pHinge->enableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
btHingeConstraint* pHinge = new btHingeConstraint(*pBody, btPivotA, btAxisA);
// pHinge->enableAngularMotor(true, -1.0, 0.165); // use for the old solver
pHinge->enableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
m_dynamicsWorld->addConstraint(pHinge);
pHinge->setDbgDrawSize(btScalar(5.f));
}
#endif
#if ENABLE_ALL_DEMOS
{
{
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(20.), btScalar(4.), btScalar(0.)));
btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape);
btRigidBody* pBodyA = createRigidBody(0.0, tr, shape);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
// dynamic bodyB (child) below it :
tr.setIdentity();
@@ -530,13 +490,13 @@ void AllConstraintDemo::initPhysics()
btRigidBody* pBodyB = createRigidBody(1.0, tr, shape);
pBodyB->setActivationState(DISABLE_DEACTIVATION);
// add some (arbitrary) data to build constraint frames
btVector3 parentAxis(1.f, 0.f, 0.f);
btVector3 childAxis(0.f, 0.f, 1.f);
btVector3 parentAxis(1.f, 0.f, 0.f);
btVector3 childAxis(0.f, 0.f, 1.f);
btVector3 anchor(20.f, 2.f, 0.f);
btUniversalConstraint* pUniv = new btUniversalConstraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis);
pUniv->setLowerLimit(-SIMD_HALF_PI * 0.5f, -SIMD_HALF_PI * 0.5f);
pUniv->setUpperLimit(SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f);
pUniv->setUpperLimit(SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f);
// add constraint to world
m_dynamicsWorld->addConstraint(pUniv, true);
// draw constraint frames and limits for debugging
@@ -545,18 +505,18 @@ void AllConstraintDemo::initPhysics()
#endif
#if ENABLE_ALL_DEMOS
{ // create a generic 6DOF constraint with springs
{ // create a generic 6DOF constraint with springs
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape);
tr.getBasis().setEulerZYX(0, 0, 0);
btRigidBody* pBodyA = createRigidBody(0.0, tr, shape);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
tr.getBasis().setEulerZYX(0, 0, 0);
btRigidBody* pBodyB = createRigidBody(1.0, tr, shape);
pBodyB->setActivationState(DISABLE_DEACTIVATION);
@@ -575,7 +535,7 @@ void AllConstraintDemo::initPhysics()
m_dynamicsWorld->addConstraint(pGen6DOFSpring, true);
pGen6DOFSpring->setDbgDrawSize(btScalar(5.f));
pGen6DOFSpring->enableSpring(0, true);
pGen6DOFSpring->setStiffness(0, 39.478f);
pGen6DOFSpring->setDamping(0, 0.5f);
@@ -586,14 +546,14 @@ void AllConstraintDemo::initPhysics()
}
#endif
#if ENABLE_ALL_DEMOS
{
{
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-20.), btScalar(4.), btScalar(0.)));
btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape);
btRigidBody* pBodyA = createRigidBody(0.0, tr, shape);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
// dynamic bodyB (child) below it :
tr.setIdentity();
@@ -601,27 +561,27 @@ void AllConstraintDemo::initPhysics()
btRigidBody* pBodyB = createRigidBody(1.0, tr, shape);
pBodyB->setActivationState(DISABLE_DEACTIVATION);
// add some data to build constraint frames
btVector3 parentAxis(0.f, 1.f, 0.f);
btVector3 childAxis(1.f, 0.f, 0.f);
btVector3 parentAxis(0.f, 1.f, 0.f);
btVector3 childAxis(1.f, 0.f, 0.f);
btVector3 anchor(-20.f, 0.f, 0.f);
btHinge2Constraint* pHinge2 = new btHinge2Constraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis);
pHinge2->setLowerLimit(-SIMD_HALF_PI * 0.5f);
pHinge2->setUpperLimit( SIMD_HALF_PI * 0.5f);
pHinge2->setUpperLimit(SIMD_HALF_PI * 0.5f);
// add constraint to world
m_dynamicsWorld->addConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2->setDbgDrawSize(btScalar(5.f));
}
#endif
#if ENABLE_ALL_DEMOS
{
#if ENABLE_ALL_DEMOS
{
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-20.), btScalar(-2.), btScalar(0.)));
btRigidBody* pBodyA = createRigidBody( 1.0f, tr, shape);
btRigidBody* pBodyA = createRigidBody(1.0f, tr, shape);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
// dynamic bodyB:
tr.setIdentity();
@@ -629,10 +589,10 @@ void AllConstraintDemo::initPhysics()
btRigidBody* pBodyB = createRigidBody(10.0, tr, shape);
pBodyB->setActivationState(DISABLE_DEACTIVATION);
// add some data to build constraint frames
btVector3 axisA(0.f, 1.f, 0.f);
btVector3 axisB(0.f, 1.f, 0.f);
btVector3 axisA(0.f, 1.f, 0.f);
btVector3 axisB(0.f, 1.f, 0.f);
btVector3 pivotA(-5.f, 0.f, 0.f);
btVector3 pivotB( 5.f, 0.f, 0.f);
btVector3 pivotB(5.f, 0.f, 0.f);
spHingeDynAB = new btHingeConstraint(*pBodyA, *pBodyB, pivotA, pivotB, axisA, axisB);
spHingeDynAB->setLimit(-SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f);
// add constraint to world
@@ -643,20 +603,20 @@ void AllConstraintDemo::initPhysics()
#endif
#if ENABLE_ALL_DEMOS
{ // 6DOF connected to the world, with motor
{ // 6DOF connected to the world, with motor
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(10.), btScalar(-15.), btScalar(0.)));
btRigidBody* pBody = createRigidBody( 1.0, tr, shape);
btRigidBody* pBody = createRigidBody(1.0, tr, shape);
pBody->setActivationState(DISABLE_DEACTIVATION);
btTransform frameB;
frameB.setIdentity();
btGeneric6DofConstraint* pGen6Dof = new btGeneric6DofConstraint( *pBody, frameB, false );
btGeneric6DofConstraint* pGen6Dof = new btGeneric6DofConstraint(*pBody, frameB, false);
m_dynamicsWorld->addConstraint(pGen6Dof);
pGen6Dof->setDbgDrawSize(btScalar(5.f));
pGen6Dof->setAngularLowerLimit(btVector3(0,0,0));
pGen6Dof->setAngularUpperLimit(btVector3(0,0,0));
pGen6Dof->setAngularLowerLimit(btVector3(0, 0, 0));
pGen6Dof->setAngularUpperLimit(btVector3(0, 0, 0));
pGen6Dof->setLinearLowerLimit(btVector3(-10., 0, 0));
pGen6Dof->setLinearUpperLimit(btVector3(10., 0, 0));
@@ -667,17 +627,14 @@ void AllConstraintDemo::initPhysics()
#endif
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void AllConstraintDemo::exitPhysics()
void AllConstraintDemo::exitPhysics()
{
int i;
int i;
//removed/delete constraints
for (i=m_dynamicsWorld->getNumConstraints()-1; i>=0 ;i--)
for (i = m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
{
btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
m_dynamicsWorld->removeConstraint(constraint);
@@ -686,7 +643,7 @@ void AllConstraintDemo::exitPhysics()
m_ctc = NULL;
//remove the rigidbodies from the dynamics world and delete them
for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
@@ -694,15 +651,12 @@ void AllConstraintDemo::exitPhysics()
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject( obj );
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
//delete collision shapes
for (int j=0;j<m_collisionShapes.size();j++)
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
@@ -712,25 +666,24 @@ void AllConstraintDemo::exitPhysics()
//delete dynamics world
delete m_dynamicsWorld;
m_dynamicsWorld=0;
m_dynamicsWorld = 0;
//delete solver
delete m_solver;
m_solver=0;
m_solver = 0;
//delete broadphase
delete m_broadphase;
m_broadphase=0;
m_broadphase = 0;
//delete dispatcher
delete m_dispatcher;
delete m_collisionConfiguration;
}
AllConstraintDemo::AllConstraintDemo(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
: CommonRigidBodyBase(helper)
{
}
@@ -738,8 +691,7 @@ AllConstraintDemo::~AllConstraintDemo()
{
//cleanup in the reverse order of creation/initialization
btAssert(m_dynamicsWorld==0);
btAssert(m_dynamicsWorld == 0);
}
#if 0
@@ -838,51 +790,48 @@ void AllConstraintDemo::displayCallback(void) {
}
#endif
bool AllConstraintDemo::keyboardCallback(int key, int state)
bool AllConstraintDemo::keyboardCallback(int key, int state)
{
bool handled = false;
switch (key)
switch (key)
{
case 'O' :
case 'O':
{
bool offectOnOff;
if (spDoorHinge)
{
bool offectOnOff;
if(spDoorHinge)
{
offectOnOff = spDoorHinge->getUseFrameOffset();
offectOnOff = !offectOnOff;
spDoorHinge->setUseFrameOffset(offectOnOff);
printf("DoorHinge %s frame offset\n", offectOnOff ? "uses" : "does not use");
}
if(spHingeDynAB)
{
offectOnOff = spHingeDynAB->getUseFrameOffset();
offectOnOff = !offectOnOff;
spHingeDynAB->setUseFrameOffset(offectOnOff);
printf("HingeDynAB %s frame offset\n", offectOnOff ? "uses" : "does not use");
}
if(spSlider6Dof)
{
offectOnOff = spSlider6Dof->getUseFrameOffset();
offectOnOff = !offectOnOff;
spSlider6Dof->setUseFrameOffset(offectOnOff);
printf("Slider6Dof %s frame offset\n", offectOnOff ? "uses" : "does not use");
}
offectOnOff = spDoorHinge->getUseFrameOffset();
offectOnOff = !offectOnOff;
spDoorHinge->setUseFrameOffset(offectOnOff);
printf("DoorHinge %s frame offset\n", offectOnOff ? "uses" : "does not use");
}
if (spHingeDynAB)
{
offectOnOff = spHingeDynAB->getUseFrameOffset();
offectOnOff = !offectOnOff;
spHingeDynAB->setUseFrameOffset(offectOnOff);
printf("HingeDynAB %s frame offset\n", offectOnOff ? "uses" : "does not use");
}
if (spSlider6Dof)
{
offectOnOff = spSlider6Dof->getUseFrameOffset();
offectOnOff = !offectOnOff;
spSlider6Dof->setUseFrameOffset(offectOnOff);
printf("Slider6Dof %s frame offset\n", offectOnOff ? "uses" : "does not use");
}
}
handled = true;
break;
default :
{
}
break;
default:
{
}
break;
}
return handled;
}
class CommonExampleInterface* AllConstraintCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* AllConstraintCreateFunc(struct CommonExampleOptions& options)
{
return new AllConstraintDemo(options.m_guiHelper);
}

6
examples/Constraints/ConstraintDemo.h
View File

@@ -15,8 +15,6 @@ subject to the following restrictions:
#ifndef ALL_CONSTRAINT_DEMO_H
#define ALL_CONSTRAINT_DEMO_H
class CommonExampleInterface* AllConstraintCreateFunc(struct CommonExampleOptions& options);
#endif //ALL_CONSTRAINT_DEMO_H
class CommonExampleInterface* AllConstraintCreateFunc(struct CommonExampleOptions& options);
#endif //ALL_CONSTRAINT_DEMO_H

162
examples/Constraints/ConstraintPhysicsSetup.cpp
View File

@@ -1,11 +1,8 @@
#include "ConstraintPhysicsSetup.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
struct ConstraintPhysicsSetup : public CommonRigidBodyBase
{
ConstraintPhysicsSetup(struct GUIHelperInterface* helper);
@@ -14,21 +11,18 @@ struct ConstraintPhysicsSetup : public CommonRigidBodyBase
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 7;
float pitch = -44;
float yaw = 721;
float targetPos[3]={8,1,-11};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
float targetPos[3] = {8, 1, -11};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
ConstraintPhysicsSetup::ConstraintPhysicsSetup(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
: CommonRigidBodyBase(helper)
{
}
ConstraintPhysicsSetup::~ConstraintPhysicsSetup()
@@ -36,63 +30,56 @@ ConstraintPhysicsSetup::~ConstraintPhysicsSetup()
}
static btScalar val;
static btScalar targetVel=0;
static btScalar maxImpulse=10000;
static btHingeAccumulatedAngleConstraint* spDoorHinge=0;
static btScalar actualHingeVelocity=0.f;
static btScalar targetVel = 0;
static btScalar maxImpulse = 10000;
static btHingeAccumulatedAngleConstraint* spDoorHinge = 0;
static btScalar actualHingeVelocity = 0.f;
static btVector3 btAxisA(0,1,0);
static btVector3 btAxisA(0, 1, 0);
void ConstraintPhysicsSetup::stepSimulation(float deltaTime)
{
val=spDoorHinge->getAccumulatedHingeAngle()*SIMD_DEGS_PER_RAD;
if (m_dynamicsWorld)
val = spDoorHinge->getAccumulatedHingeAngle() * SIMD_DEGS_PER_RAD;
if (m_dynamicsWorld)
{
spDoorHinge->enableAngularMotor(true,targetVel,maxImpulse);
spDoorHinge->enableAngularMotor(true, targetVel, maxImpulse);
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
m_dynamicsWorld->stepSimulation(deltaTime, 10, 1. / 240.);
btHingeConstraint* hinge = spDoorHinge;
btHingeConstraint* hinge = spDoorHinge;
if (hinge)
{
const btRigidBody& bodyA = hinge->getRigidBodyA();
const btRigidBody& bodyB = hinge->getRigidBodyB();
if (hinge)
{
btTransform trA = bodyA.getWorldTransform();
btVector3 angVelA = bodyA.getAngularVelocity();
btVector3 angVelB = bodyB.getAngularVelocity();
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);
}
}
{
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);
@@ -100,62 +87,59 @@ void ConstraintPhysicsSetup::initPhysics()
createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
int mode = btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawConstraintLimits;
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("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("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);
}
{
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);
}
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
{ // 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);
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
const btVector3 btPivotA(10.f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
spDoorHinge = new btHingeAccumulatedAngleConstraint( *pDoorBody, btPivotA, btAxisA );
spDoorHinge = new btHingeAccumulatedAngleConstraint(*pDoorBody, btPivotA, btAxisA);
m_dynamicsWorld->addConstraint(spDoorHinge);
m_dynamicsWorld->addConstraint(spDoorHinge);
spDoorHinge->setDbgDrawSize(btScalar(5.f));
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
class CommonExampleInterface* ConstraintCreateFunc(CommonExampleOptions& options)
class CommonExampleInterface* ConstraintCreateFunc(CommonExampleOptions& options)
{
return new ConstraintPhysicsSetup(options.m_guiHelper);
}

4
examples/Constraints/ConstraintPhysicsSetup.h
View File

@@ -1,6 +1,6 @@
#ifndef CONSTAINT_PHYSICS_SETUP_H
#define CONSTAINT_PHYSICS_SETUP_H
class CommonExampleInterface* ConstraintCreateFunc(struct CommonExampleOptions& options);
class CommonExampleInterface* ConstraintCreateFunc(struct CommonExampleOptions& options);
#endif //CONSTAINT_PHYSICS_SETUP_H
#endif //CONSTAINT_PHYSICS_SETUP_H

777
examples/Constraints/Dof6Spring2Setup.cpp
View File

@@ -9,37 +9,32 @@
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#define M_PI 3.14159265358979323846
#endif
#ifndef M_PI_2
#define M_PI_2 1.57079632679489661923
#define M_PI_2 1.57079632679489661923
#endif
#ifndef M_PI_4
#define M_PI_4 0.785398163397448309616
#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
#define CONSTRAINT_TYPE btGeneric6DofSpring2Constraint
#define EXTRAPARAMS
#else
#define CONSTRAINT_TYPE btGeneric6DofSpringConstraint
#define EXTRAPARAMS ,true
#define CONSTRAINT_TYPE btGeneric6DofSpringConstraint
#define EXTRAPARAMS , true
#endif
#include "../CommonInterfaces/CommonRigidBodyBase.h"
struct Dof6Spring2Setup : public CommonRigidBodyBase
{
struct Dof6Spring2SetupInternalData* m_data;
@@ -57,40 +52,37 @@ struct Dof6Spring2Setup : public CommonRigidBodyBase
float dist = 5;
float pitch = -35;
float yaw = 722;
float targetPos[3]={4,2,-11};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
float targetPos[3] = {4, 2, -11};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
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;
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)
: mDt(1. / 60.), frameID(0)
{
}
};
Dof6Spring2Setup::Dof6Spring2Setup(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
: CommonRigidBodyBase(helper)
{
m_data = new Dof6Spring2SetupInternalData;
}
@@ -102,392 +94,397 @@ Dof6Spring2Setup::~Dof6Spring2Setup()
void Dof6Spring2Setup::initPhysics()
{
// Setup the basic world
m_guiHelper->setUpAxis(1);
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);
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());
/////// 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 = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld->getDispatchInfo().m_useContinuous = true;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
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));
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);
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
}
localCreateRigidBody(btScalar(0.), groundTransform, groundShape);
#endif //CREATE_GROUND_COLLISION_OBJECT
}
m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
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;
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);
//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);
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);
constraint->setDbgDrawSize(btScalar(2.f));
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);
constraint->setDbgDrawSize(btScalar(2.f));
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);
constraint->setDbgDrawSize(btScalar(2.f));
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);
constraint->setDbgDrawSize(btScalar(2.f));
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,600.f);
#else
constraint->getRotationalLimitMotor(2)->m_enableMotor = true;
constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f;
constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 600.f;
#endif
constraint->setDbgDrawSize(btScalar(2.f));
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,600.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 = 600.f;
//servo motor is not implemented in 6dofspring constraint
#endif
constraint->setDbgDrawSize(btScalar(2.f));
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);
}
constraint->setDbgDrawSize(btScalar(1.f));
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);
constraintZY->setDbgDrawSize(btScalar(1.f));
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_data->m_ChainLeftConstraint->setDbgDrawSize(btScalar(1.f));
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
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
static float servoNextFrame = -1;
if(servoNextFrame < 0)
{
m_data->m_ServoMotorConstraint->getRotationalLimitMotor(2)->m_servoTarget *= -1;
servoNextFrame = 3.0;
}
servoNextFrame -= m_data->mDt;
constraint->setDamping(0, 0);
#else
constraint->setDamping(0, 1);
#endif
constraint->setEquilibriumPoint(0, 0);
constraint->setDbgDrawSize(btScalar(2.f));
m_dynamicsWorld->addConstraint(constraint, true);
}
/////// constraint chain: pull the chain left and right periodically
static float chainNextFrame = -1;
static bool left = true;
if(chainNextFrame < 0)
/////////// 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);
constraint->setDbgDrawSize(btScalar(2.f));
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);
constraint->setDbgDrawSize(btScalar(2.f));
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);
constraint->setDbgDrawSize(btScalar(2.f));
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, 600.f);
#else
constraint->getRotationalLimitMotor(2)->m_enableMotor = true;
constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f;
constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 600.f;
#endif
constraint->setDbgDrawSize(btScalar(2.f));
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, 600.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 = 600.f;
//servo motor is not implemented in 6dofspring constraint
#endif
constraint->setDbgDrawSize(btScalar(2.f));
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)
{
if(!left)
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)
{
m_data->m_ChainRightBody->setActivationState(ACTIVE_TAG);
m_dynamicsWorld->removeConstraint(m_data->m_ChainRightConstraint);
m_data->m_ChainLeftConstraint->setDbgDrawSize(btScalar(2.f));
m_dynamicsWorld->addConstraint(m_data->m_ChainLeftConstraint, true);
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);
}
constraint->setDbgDrawSize(btScalar(1.f));
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);
constraintZY->setDbgDrawSize(btScalar(1.f));
m_dynamicsWorld->addConstraint(constraintZY, true);
}
}
else
{
m_data->m_ChainLeftBody->setActivationState(ACTIVE_TAG);
m_dynamicsWorld->removeConstraint(m_data->m_ChainLeftConstraint);
m_data->m_ChainRightConstraint->setDbgDrawSize(btScalar(2.f));
m_dynamicsWorld->addConstraint(m_data->m_ChainRightConstraint, true);
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_data->m_ChainLeftConstraint->setDbgDrawSize(btScalar(1.f));
m_dynamicsWorld->addConstraint(m_data->m_ChainLeftConstraint, true);
}
chainNextFrame = 3.0;
left = !left;
prevBody = body;
}
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_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_BouncingTranslateBody->applyCentralImpulse(btVector3(10,0,0));
bounceNextFrame = 3.0;
m_data->m_ChainRightConstraint->setParam(BT_CONSTRAINT_STOP_ERP, limitConstraintStrength, a);
m_data->m_ChainRightConstraint->setParam(BT_CONSTRAINT_STOP_CFM, 0.0, a);
}
bounceNextFrame -= m_data->mDt;
m_data->frameID++;
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void Dof6Spring2Setup::animate()
{
/////// servo motor: flip its target periodically
#ifdef USE_6DOF2
static float servoNextFrame = -1;
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_data->m_ChainLeftConstraint->setDbgDrawSize(btScalar(2.f));
m_dynamicsWorld->addConstraint(m_data->m_ChainLeftConstraint, true);
}
else
{
m_data->m_ChainLeftBody->setActivationState(ACTIVE_TAG);
m_dynamicsWorld->removeConstraint(m_data->m_ChainLeftConstraint);
m_data->m_ChainRightConstraint->setDbgDrawSize(btScalar(2.f));
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)
{
@@ -495,7 +492,7 @@ void Dof6Spring2Setup::stepSimulation(float deltaTime)
m_dynamicsWorld->stepSimulation(deltaTime);
}
class CommonExampleInterface* Dof6Spring2CreateFunc( CommonExampleOptions& options)
class CommonExampleInterface* Dof6Spring2CreateFunc(CommonExampleOptions& options)
{
return new Dof6Spring2Setup(options.m_guiHelper);
}

4
examples/Constraints/Dof6Spring2Setup.h
View File

@@ -1,6 +1,6 @@
#ifndef GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H
#define GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H
class CommonExampleInterface* Dof6Spring2CreateFunc(struct CommonExampleOptions& options);
class CommonExampleInterface* Dof6Spring2CreateFunc(struct CommonExampleOptions& options);
#endif //GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H
#endif //GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H

259
examples/Constraints/TestHingeTorque.cpp
View File

@@ -1,6 +1,5 @@
#include "TestHingeTorque.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
@@ -10,99 +9,88 @@ static btScalar radius(0.2);
struct TestHingeTorque : public CommonRigidBodyBase
{
bool m_once;
btAlignedObjectArray<btJointFeedback*> m_jointFeedback;
bool m_once;
btAlignedObjectArray<btJointFeedback*> m_jointFeedback;
TestHingeTorque(struct GUIHelperInterface* helper);
virtual ~ TestHingeTorque();
virtual ~TestHingeTorque();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 5;
float pitch = -21;
float yaw = 270;
float targetPos[3]={-1.34,3.4,-0.44};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
float dist = 5;
float pitch = -21;
float yaw = 270;
float targetPos[3] = {-1.34, 3.4, -0.44};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
TestHingeTorque::TestHingeTorque(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper),
m_once(true)
: CommonRigidBodyBase(helper),
m_once(true)
{
}
TestHingeTorque::~ TestHingeTorque()
TestHingeTorque::~TestHingeTorque()
{
for (int i=0;i<m_jointFeedback.size();i++)
for (int i = 0; i < m_jointFeedback.size(); i++)
{
delete m_jointFeedback[i];
}
}
void TestHingeTorque::stepSimulation(float deltaTime)
{
if (0)//m_once)
{
m_once=false;
btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0);
btRigidBody& bodyA = hinge->getRigidBodyA();
btTransform trA = bodyA.getWorldTransform();
btVector3 hingeAxisInWorld = trA.getBasis()*hinge->getFrameOffsetA().getBasis().getColumn(2);
hinge->getRigidBodyA().applyTorque(-hingeAxisInWorld*10);
hinge->getRigidBodyB().applyTorque(hingeAxisInWorld*10);
}
m_dynamicsWorld->stepSimulation(1./240,0);
if (0) //m_once)
{
m_once = false;
btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0);
btRigidBody& bodyA = hinge->getRigidBodyA();
btTransform trA = bodyA.getWorldTransform();
btVector3 hingeAxisInWorld = trA.getBasis() * hinge->getFrameOffsetA().getBasis().getColumn(2);
hinge->getRigidBodyA().applyTorque(-hingeAxisInWorld * 10);
hinge->getRigidBodyB().applyTorque(hingeAxisInWorld * 10);
}
m_dynamicsWorld->stepSimulation(1. / 240, 0);
static int count = 0;
if ((count& 0x0f)==0)
if ((count & 0x0f) == 0)
{
btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0],
b3Printf("base angvel = %f,%f,%f", base->getAngularVelocity()[0],
base->getAngularVelocity()[1],
base->getAngularVelocity()[2]);
btRigidBody* child = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[1]);
b3Printf("child angvel = %f,%f,%f",child->getAngularVelocity()[0],
b3Printf("child angvel = %f,%f,%f", child->getAngularVelocity()[0],
child->getAngularVelocity()[1],
child->getAngularVelocity()[2]);
for (int i=0;i<m_jointFeedback.size();i++)
for (int i = 0; i < m_jointFeedback.size(); i++)
{
b3Printf("Applied force at the COM/Inertial frame B[%d]:(%f,%f,%f), torque B:(%f,%f,%f)\n", i,
m_jointFeedback[i]->m_appliedForceBodyB.x(),
m_jointFeedback[i]->m_appliedForceBodyB.y(),
m_jointFeedback[i]->m_appliedForceBodyB.z(),
m_jointFeedback[i]->m_appliedTorqueBodyB.x(),
m_jointFeedback[i]->m_appliedTorqueBodyB.y(),
m_jointFeedback[i]->m_appliedTorqueBodyB.z());
m_jointFeedback[i]->m_appliedForceBodyB.x(),
m_jointFeedback[i]->m_appliedForceBodyB.y(),
m_jointFeedback[i]->m_appliedForceBodyB.z(),
m_jointFeedback[i]->m_appliedTorqueBodyB.x(),
m_jointFeedback[i]->m_appliedTorqueBodyB.y(),
m_jointFeedback[i]->m_appliedTorqueBodyB.z());
}
}
count++;
//CommonRigidBodyBase::stepSimulation(deltaTime);
//CommonRigidBodyBase::stepSimulation(deltaTime);
}
void TestHingeTorque::initPhysics()
{
int upAxis = 1;
@@ -110,92 +98,89 @@ void TestHingeTorque::initPhysics()
createEmptyDynamicsWorld();
m_dynamicsWorld->getSolverInfo().m_splitImpulse = false;
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
int mode = btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawConstraintLimits;
int mode = btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawConstraintLimits;
m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode);
{ // create a door using hinge constraint attached to the world
{ // create a door using hinge constraint attached to the world
int numLinks = 2;
// bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
int numLinks = 2;
// bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btBoxShape* baseBox = new btBoxShape(baseHalfExtents);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
btTransform baseWorldTrans;
baseWorldTrans.setIdentity();
baseWorldTrans.setOrigin(basePosition);
//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 = 0.f;
float linkMass = 1.f;
btRigidBody* base = createRigidBody(baseMass,baseWorldTrans,baseBox);
m_dynamicsWorld->removeRigidBody(base);
base->setDamping(0,0);
m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask);
btBoxShape* linkBox1 = new btBoxShape(linkHalfExtents);
btBoxShape* baseBox = new btBoxShape(baseHalfExtents);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
btTransform baseWorldTrans;
baseWorldTrans.setIdentity();
baseWorldTrans.setOrigin(basePosition);
//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 = 0.f;
float linkMass = 1.f;
btRigidBody* base = createRigidBody(baseMass, baseWorldTrans, baseBox);
m_dynamicsWorld->removeRigidBody(base);
base->setDamping(0, 0);
m_dynamicsWorld->addRigidBody(base, collisionFilterGroup, collisionFilterMask);
btBoxShape* linkBox1 = new btBoxShape(linkHalfExtents);
btSphereShape* linkSphere = new btSphereShape(radius);
btRigidBody* prevBody = base;
for (int i=0;i<numLinks;i++)
{
btTransform linkTrans;
linkTrans = baseWorldTrans;
linkTrans.setOrigin(basePosition-btVector3(0,linkHalfExtents[1]*2.f*(i+1),0));
btRigidBody* prevBody = base;
for (int i = 0; i < numLinks; i++)
{
btTransform linkTrans;
linkTrans = baseWorldTrans;
linkTrans.setOrigin(basePosition - btVector3(0, linkHalfExtents[1] * 2.f * (i + 1), 0));
btCollisionShape* colOb = 0;
if (i==0)
if (i == 0)
{
colOb = linkBox1;
} else
}
else
{
colOb = linkSphere;
}
btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,colOb);
m_dynamicsWorld->removeRigidBody(linkBody);
m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask);
linkBody->setDamping(0,0);
btRigidBody* linkBody = createRigidBody(linkMass, linkTrans, colOb);
m_dynamicsWorld->removeRigidBody(linkBody);
m_dynamicsWorld->addRigidBody(linkBody, collisionFilterGroup, collisionFilterMask);
linkBody->setDamping(0, 0);
btTypedConstraint* con = 0;
if (i==0)
if (i == 0)
{
//create a hinge constraint
btVector3 pivotInA(0,-linkHalfExtents[1],0);
btVector3 pivotInB(0,linkHalfExtents[1],0);
btVector3 axisInA(1,0,0);
btVector3 axisInB(1,0,0);
btVector3 pivotInA(0, -linkHalfExtents[1], 0);
btVector3 pivotInB(0, linkHalfExtents[1], 0);
btVector3 axisInA(1, 0, 0);
btVector3 axisInB(1, 0, 0);
bool useReferenceA = true;
btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody,
pivotInA,pivotInB,
axisInA,axisInB,useReferenceA);
btHingeConstraint* hinge = new btHingeConstraint(*prevBody, *linkBody,
pivotInA, pivotInB,
axisInA, axisInB, useReferenceA);
con = hinge;
} else
}
else
{
btTransform pivotInA(btQuaternion::getIdentity(),btVector3(0, -radius, 0)); //par body's COM to cur body's COM offset
btTransform pivotInB(btQuaternion::getIdentity(),btVector3(0, radius, 0)); //cur body's COM to cur body's PIV offset
btTransform pivotInA(btQuaternion::getIdentity(), btVector3(0, -radius, 0)); //par body's COM to cur body's COM offset
btTransform pivotInB(btQuaternion::getIdentity(), btVector3(0, radius, 0)); //cur body's COM to cur body's PIV offset
btGeneric6DofSpring2Constraint* fixed = new btGeneric6DofSpring2Constraint(*prevBody, *linkBody,
pivotInA,pivotInB);
fixed->setLinearLowerLimit(btVector3(0,0,0));
fixed->setLinearUpperLimit(btVector3(0,0,0));
fixed->setAngularLowerLimit(btVector3(0,0,0));
fixed->setAngularUpperLimit(btVector3(0,0,0));
con = fixed;
pivotInA, pivotInB);
fixed->setLinearLowerLimit(btVector3(0, 0, 0));
fixed->setLinearUpperLimit(btVector3(0, 0, 0));
fixed->setAngularLowerLimit(btVector3(0, 0, 0));
fixed->setAngularUpperLimit(btVector3(0, 0, 0));
con = fixed;
}
btAssert(con);
if (con)
@@ -204,38 +189,36 @@ void TestHingeTorque::initPhysics()
m_jointFeedback.push_back(fb);
con->setJointFeedback(fb);
m_dynamicsWorld->addConstraint(con,true);
m_dynamicsWorld->addConstraint(con, true);
}
prevBody = linkBody;
}
}
}
if (1)
{
btVector3 groundHalfExtents(1,1,0.2);
groundHalfExtents[upAxis]=1.f;
btVector3 groundHalfExtents(1, 1, 0.2);
groundHalfExtents[upAxis] = 1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
btTransform start; start.setIdentity();
btTransform start;
start.setIdentity();
btVector3 groundOrigin(-0.4f, 3.f, 0.f);
// btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
groundOrigin[upAxis] -=.5;
groundOrigin[2]-=0.6;
// btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
btQuaternion groundOrn(btVector3(0, 1, 0), 0.25 * SIMD_PI);
groundOrigin[upAxis] -= .5;
groundOrigin[2] -= 0.6;
start.setOrigin(groundOrigin);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0, start, box);
body->setFriction(0);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
class CommonExampleInterface* TestHingeTorqueCreateFunc(CommonExampleOptions& options)
class CommonExampleInterface* TestHingeTorqueCreateFunc(CommonExampleOptions& options)
{
return new TestHingeTorque(options.m_guiHelper);
}

5
examples/Constraints/TestHingeTorque.h
View File

@@ -1,7 +1,6 @@
#ifndef TEST_HINGE_TORQUE_H
#define TEST_HINGE_TORQUE_H
class CommonExampleInterface* TestHingeTorqueCreateFunc(struct CommonExampleOptions& options);
#endif //TEST_HINGE_TORQUE_H
class CommonExampleInterface* TestHingeTorqueCreateFunc(struct CommonExampleOptions& options);
#endif //TEST_HINGE_TORQUE_H