void ConstraintDemo::initPhysics()
{
setTexturing(true);
setShadows(true);
setCameraDistance(26.f);
m_Time = 0;
setupEmptyDynamicsWorld();
//btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(40.),btScalar(50.)));
btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),40);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-56,0));
btRigidBody* groundBody;
groundBody= localCreateRigidBody(0, groundTransform, groundShape);
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));
float mass = 1.f;
#if ENABLE_ALL_DEMOS
//point to point constraint (ball socket)
{
btRigidBody* body0 = localCreateRigidBody( mass,trans,shape);
trans.setOrigin(btVector3(2*CUBE_HALF_EXTENTS,20,0));
mass = 1.f;
btRigidBody* body1 = 0;//localCreateRigidBody( mass,trans,shape);
// btRigidBody* body1 = localCreateRigidBody( 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 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,*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);
//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);
m_dynamicsWorld->addConstraint(hinge);
hinge->setDbgDrawSize(btScalar(5.f));
#endif //P2P
}
#endif
#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));
trans.setIdentity();
trans.setOrigin(sliderWorldPos);
//trans.setBasis(sliderOrientation);
sliderTransform = trans;
d6body0 = localCreateRigidBody( mass,trans,shape);
d6body0->setActivationState(DISABLE_DEACTIVATION);
btRigidBody* fixedBody1 = localCreateRigidBody(0,trans,0);
m_dynamicsWorld->addRigidBody(fixedBody1);
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInB = btTransform::getIdentity();
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);
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->getTranslationalLimitMotor()->m_enableMotor[0] = true;
spSlider6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f;
spSlider6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
m_dynamicsWorld->addConstraint(spSlider6Dof);
spSlider6Dof->setDbgDrawSize(btScalar(5.f));
}
#endif
#if ENABLE_ALL_DEMOS
{ // 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 = localCreateRigidBody( 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
spDoorHinge = new btHingeConstraint( *pDoorBody, btPivotA, btAxisA );
// spDoorHinge->setLimit( 0.0f, SIMD_PI_2 );
// test problem values
// 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 );
m_dynamicsWorld->addConstraint(spDoorHinge);
spDoorHinge->setDbgDrawSize(btScalar(5.f));
//doorTrans.setOrigin(btVector3(-5.0f, 2.0f, 0.0f));
//btRigidBody* pDropBody = localCreateRigidBody( 10.0, doorTrans, shape);
}
#endif
#if ENABLE_ALL_DEMOS
{ // 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 = localCreateRigidBody( mass, tr, shape);
btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape);
// btRigidBody* pBodyA = localCreateRigidBody( 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);
btRigidBody* pBodyB = localCreateRigidBody(mass, tr, shape);
// btRigidBody* pBodyB = localCreateRigidBody(0.f, tr, shape);
pBodyB->setActivationState(DISABLE_DEACTIVATION);
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInA.setOrigin(btVector3(btScalar(-5.), btScalar(0.), btScalar(0.)));
frameInB = btTransform::getIdentity();
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);
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->getTranslationalLimitMotor()->m_enableMotor[0] = true;
// pGen6DOF->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
// pGen6DOF->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
// 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.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
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-10.), btScalar(5.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
btRigidBody* pBodyA = localCreateRigidBody( 1.0, tr, shape);
// btRigidBody* pBodyA = localCreateRigidBody( 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);
btRigidBody* pBodyB = localCreateRigidBody(0.0, tr, shape);
// btRigidBody* pBodyB = localCreateRigidBody(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.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_dynamicsWorld->addConstraint(m_ctc, true);
m_ctc->setDbgDrawSize(btScalar(5.f));
// s_bTestConeTwistMotor = true; // use only with old solver for now
s_bTestConeTwistMotor = false;
}
#endif
#if ENABLE_ALL_DEMOS
{ // 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 = localCreateRigidBody( 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 );
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 = localCreateRigidBody( 0.0, tr, shape);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
// dynamic bodyB (child) below it :
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(20.), btScalar(0.), btScalar(0.)));
btRigidBody* pBodyB = localCreateRigidBody(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 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);
// add constraint to world
m_dynamicsWorld->addConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv->setDbgDrawSize(btScalar(5.f));
}
#endif
#if ENABLE_ALL_DEMOS
{ // 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 = localCreateRigidBody( 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);
btRigidBody* pBodyB = localCreateRigidBody(1.0, tr, shape);
pBodyB->setActivationState(DISABLE_DEACTIVATION);
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInA.setOrigin(btVector3(btScalar(10.), btScalar(0.), btScalar(0.)));
frameInB = btTransform::getIdentity();
frameInB.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
btGeneric6DofSpringConstraint* pGen6DOFSpring = new btGeneric6DofSpringConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true);
pGen6DOFSpring->setLinearUpperLimit(btVector3(5., 0., 0.));
pGen6DOFSpring->setLinearLowerLimit(btVector3(-5., 0., 0.));
pGen6DOFSpring->setAngularLowerLimit(btVector3(0.f, 0.f, -1.5f));
pGen6DOFSpring->setAngularUpperLimit(btVector3(0.f, 0.f, 1.5f));
m_dynamicsWorld->addConstraint(pGen6DOFSpring, true);
pGen6DOFSpring->setDbgDrawSize(btScalar(5.f));
pGen6DOFSpring->enableSpring(0, true);
pGen6DOFSpring->setStiffness(0, 39.478f);
pGen6DOFSpring->setDamping(0, 0.5f);
pGen6DOFSpring->enableSpring(5, true);
pGen6DOFSpring->setStiffness(5, 39.478f);
pGen6DOFSpring->setDamping(0, 0.3f);
pGen6DOFSpring->setEquilibriumPoint();
}
#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 = localCreateRigidBody( 0.0, tr, shape);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
// dynamic bodyB (child) below it :
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-20.), btScalar(0.), btScalar(0.)));
btRigidBody* pBodyB = localCreateRigidBody(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 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);
// 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
{
// 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 = localCreateRigidBody( 1.0f, tr, shape);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
// dynamic bodyB:
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-30.), btScalar(-2.), btScalar(0.)));
btRigidBody* pBodyB = localCreateRigidBody(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 pivotA(-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
m_dynamicsWorld->addConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB->setDbgDrawSize(btScalar(5.f));
}
#endif
#ifdef TEST_SERIALIZATION
int maxSerializeBufferSize = 1024*1024*5;
btDefaultSerializer* serializer = new btDefaultSerializer(maxSerializeBufferSize);
m_dynamicsWorld->serialize(serializer);
FILE* f2 = fopen("testFile.bullet","wb");
fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1,f2);
fclose(f2);
exitPhysics();
setupEmptyDynamicsWorld();
btBulletWorldImporter* fileLoader = new btBulletWorldImporter(m_dynamicsWorld);
fileLoader->loadFile("testFile.bullet");
#endif //TEST_SERIALIZATION
}
void AllConstraintDemo::initPhysics()
{
m_guiHelper->setUpAxis(1);
m_Time = 0;
setupEmptyDynamicsWorld();
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);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-56,0));
btRigidBody* groundBody;
groundBody= createRigidBody(0, groundTransform, groundShape);
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));
float mass = 1.f;
#if ENABLE_ALL_DEMOS
///gear constraint demo
#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;
{
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);
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));
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));
bodyA = body;
}
{
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);
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));
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);
m_dynamicsWorld->addConstraint(hinge);
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);
btMatrix3x3 mat(orn);
axisB = mat.getRow(1);
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));
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);
m_dynamicsWorld->addConstraint(p2p);
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));
mass = 1.f;
// 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 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,*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);
//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);
m_dynamicsWorld->addConstraint(hinge);
hinge->setDbgDrawSize(btScalar(5.f));
#endif //P2P
}
#endif
#if ENABLE_ALL_DEMOS
{
btTransform trans;
trans.setIdentity();
btVector3 worldPos(-20,0,30);
trans.setOrigin(worldPos);
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);
// 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);
spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F);
spSlider1->setUpperAngLimit( SIMD_PI / 3.0F);
m_dynamicsWorld->addConstraint(spSlider1, true);
spSlider1->setDbgDrawSize(btScalar(5.f));
}
#endif
#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));
trans.setIdentity();
trans.setOrigin(sliderWorldPos);
//trans.setBasis(sliderOrientation);
sliderTransform = trans;
d6body0 = createRigidBody( mass,trans,shape);
d6body0->setActivationState(DISABLE_DEACTIVATION);
btRigidBody* fixedBody1 = createRigidBody(0,trans,0);
m_dynamicsWorld->addRigidBody(fixedBody1);
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInB = btTransform::getIdentity();
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);
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->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
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
btVector3 btAxisA( 0.0f, 1.0f, 0.0f ); // pointing upwards, aka Y-axis
spDoorHinge = new btHingeConstraint( *pDoorBody, btPivotA, btAxisA );
// spDoorHinge->setLimit( 0.0f, SIMD_PI_2 );
// test problem values
// 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 );
m_dynamicsWorld->addConstraint(spDoorHinge);
spDoorHinge->setDbgDrawSize(btScalar(5.f));
//doorTrans.setOrigin(btVector3(-5.0f, 2.0f, 0.0f));
//btRigidBody* pDropBody = createRigidBody( 10.0, doorTrans, shape);
}
#endif
#if ENABLE_ALL_DEMOS
{ // 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);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(0.), btScalar(6.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
btRigidBody* pBodyB = createRigidBody(mass, tr, shape);
// btRigidBody* pBodyB = createRigidBody(0.f, tr, shape);
pBodyB->setActivationState(DISABLE_DEACTIVATION);
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInA.setOrigin(btVector3(btScalar(-5.), btScalar(0.), btScalar(0.)));
frameInB = btTransform::getIdentity();
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);
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->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(-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.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
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);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-10.), btScalar(-5.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
btRigidBody* pBodyB = createRigidBody(0.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.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_dynamicsWorld->addConstraint(m_ctc, true);
m_ctc->setDbgDrawSize(btScalar(5.f));
// s_bTestConeTwistMotor = true; // use only with old solver for now
s_bTestConeTwistMotor = false;
}
#endif
#if ENABLE_ALL_DEMOS
{ // 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);
pBody->setActivationState(DISABLE_DEACTIVATION);
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
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);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
// dynamic bodyB (child) below it :
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(20.), btScalar(0.), btScalar(0.)));
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 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);
// add constraint to world
m_dynamicsWorld->addConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv->setDbgDrawSize(btScalar(5.f));
}
#endif
#if ENABLE_ALL_DEMOS
{ // 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);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
btRigidBody* pBodyB = createRigidBody(1.0, tr, shape);
pBodyB->setActivationState(DISABLE_DEACTIVATION);
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInA.setOrigin(btVector3(btScalar(10.), btScalar(0.), btScalar(0.)));
frameInB = btTransform::getIdentity();
frameInB.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
btGeneric6DofSpringConstraint* pGen6DOFSpring = new btGeneric6DofSpringConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true);
pGen6DOFSpring->setLinearUpperLimit(btVector3(5., 0., 0.));
pGen6DOFSpring->setLinearLowerLimit(btVector3(-5., 0., 0.));
pGen6DOFSpring->setAngularLowerLimit(btVector3(0.f, 0.f, -1.5f));
pGen6DOFSpring->setAngularUpperLimit(btVector3(0.f, 0.f, 1.5f));
m_dynamicsWorld->addConstraint(pGen6DOFSpring, true);
pGen6DOFSpring->setDbgDrawSize(btScalar(5.f));
pGen6DOFSpring->enableSpring(0, true);
pGen6DOFSpring->setStiffness(0, 39.478f);
pGen6DOFSpring->setDamping(0, 0.5f);
pGen6DOFSpring->enableSpring(5, true);
pGen6DOFSpring->setStiffness(5, 39.478f);
pGen6DOFSpring->setDamping(0, 0.3f);
pGen6DOFSpring->setEquilibriumPoint();
}
#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);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
// dynamic bodyB (child) below it :
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-20.), btScalar(0.), btScalar(0.)));
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 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);
// 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
{
// 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);
pBodyA->setActivationState(DISABLE_DEACTIVATION);
// dynamic bodyB:
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(-30.), btScalar(-2.), btScalar(0.)));
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 pivotA(-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
m_dynamicsWorld->addConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB->setDbgDrawSize(btScalar(5.f));
}
#endif
#if ENABLE_ALL_DEMOS
{ // 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);
pBody->setActivationState(DISABLE_DEACTIVATION);
btTransform frameB;
frameB.setIdentity();
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->setLinearLowerLimit(btVector3(-10., 0, 0));
pGen6Dof->setLinearUpperLimit(btVector3(10., 0, 0));
pGen6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true;
pGen6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
pGen6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f;
}
#endif
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
// The backpack is a single constraint which should have only one degree of freedom, but will suffer from the following
// artifacts if implemented using a single hinge constraint:
// 1. Undesired motion (excessive, potentially implausible, or just not artisitically desired) due to the large
// accelerations of the character.
// 2. A potential performance impact if collisions are used to prevent interpenetration with the character.
// To address this we use a limited hinge constraint which keeps the backpack from penetrating the spline and
// from moving to far upwards.
// The alternative approach of ensuring collisions between the character and the backpack keep it in place has the following flaws:
// (a) Constant collision (such as when the character is standing idle) is expensive, especially if colliding with several bodies
// such as are in the spine - a single constraint with fixed limits avoids this completely with minimal expense.
// (b) Again, without limits, even if the backpack rests against the base of the spine when moving down, there is nothing to prevent
// the large accelerations of the character forcing the backpack to swing up and forward over the shoulders until it collides with
// the back of the head, which is probably not desirable from an artistic standpoint.
void HK_CALL CharacterAttachmentsHelpers::addBackpack(hkpWorld* world,
const hkaRagdollInstance* ragdollInstance,
const hkQsTransform& currentTransform,
const char* startBoneName,
hkpGroupFilter* filter,
const ConstraintStabilityTricks& tricks )
{
// Get the relevant RBs to which we attach this backpack.
hkpRigidBody* torso = HK_NULL;
{
int torsoIndex = hkaSkeletonUtils::findBoneWithName( *ragdollInstance->m_skeleton, startBoneName);
HK_ASSERT2( 0, torsoIndex >= 0, "Couldn't find " << startBoneName << " in ragdoll" );
torso = ragdollInstance->getRigidBodyOfBone( torsoIndex );
}
hkpRigidBody* backpack;
{
hkVector4 boxHalfExtents( 0.1f, 0.05f, 0.2f);
// Create a body
hkpRigidBodyCinfo info;
info.m_shape = new hkpBoxShape( boxHalfExtents, 0.01f );
// We are going to add a single body which does not collide with the ragdoll *at all* so we
// will put in a different group and different layer and disable collision using layers.
info.m_collisionFilterInfo = hkpGroupFilter::calcFilterInfo( 2, 3 );
filter->disableCollisionsBetween(1, 2);
const hkReal mass = 1.0f;
hkpInertiaTensorComputer::setShapeVolumeMassProperties( info.m_shape, mass, info );
info.m_mass = mass;
// Place roughly behind the character model space - we'll let a short simulation run settle the constraint properly.
info.m_position.set( 0.2f, 0.0f, -0.2f );
// Move into world space behind character
info.m_position.setTransformedPos(currentTransform, info.m_position);
backpack = new hkpRigidBody( info );
info.m_shape->removeReference();
world->addEntity( backpack );
backpack->removeReference();
}
//
// Create constraint (to attach and also restrict motion)
//
{
hkpLimitedHingeConstraintData* lhc = new hkpLimitedHingeConstraintData();
hkVector4 pivotA, pivotB;
// Backpack
pivotA.set(0.0f, -0.05f, 0.2f);
hkVector4 axisA(1.0f, 0.0f, 0.0f);
hkVector4 axisAPerp(0.0f, 0.0f, 1.0f);
// Torso
pivotB.set(0.1f, -0.07f, 0.0f);
hkVector4 axisB(0.0f, 0.0f, 1.0f);
hkVector4 axisBPerp(1.0f, 0.0f, 0.0f);
lhc->setInBodySpace(pivotA, pivotB, axisA, axisB, axisAPerp, axisBPerp);
lhc->setMinAngularLimit(HK_REAL_PI/40.0f);
lhc->setMaxAngularLimit(HK_REAL_PI/4.0f);
//
// Create and add the constraint
//
{
hkpConstraintInstance* constraint = new hkpConstraintInstance(backpack, torso, lhc );
world->addConstraint(constraint);
constraint->removeReference();
}
if( !tricks.m_useLimits )
{
lhc->disableLimits();
}
lhc->removeReference();
}
}
/* ----------------------------------------------------------------------- */
void SIMPLE_Agents::addRobotPhysics( const vector <double> &_pos, const vector <double> &_rot ){
mass = 0.150;
btQuaternion rotation;
rotation.setEulerZYX( 0.0, _rot[1], 0.0);
btVector3 position = btVector3(_pos[0],robot_height * 0.65,_pos[2]);
btMotionState* motion = new btDefaultMotionState(btTransform(rotation, position));
btCylinderShape* cylinder1=new btCylinderShape(btVector3(robot_radius,robot_height*0.5,robot_radius));
cylinder1->setMargin(robot_radius*0.5);
btVector3 inertia(0.0,0.0,0.0);
cylinder1->calculateLocalInertia(mass,inertia);
btRigidBody::btRigidBodyConstructionInfo info(mass,motion,cylinder1,inertia);
//info.m_restitution = 0.0f;
info.m_friction = 0.5f;
//info.m_rollingFriction = 0.1;
body=new btRigidBody(info);
body->setLinearFactor(btVector3(1,1,1));
body->setAngularFactor(btVector3(0,1,0));
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK );
world->addRigidBody(body);
btCylinderShape* wheel1 = new btCylinderShape(btVector3(wheel_radius,wheel_width,wheel_radius));
double mass1 = 0.15;
btVector3 inertia1(0.0,0.0,0.0);
wheel1->calculateLocalInertia(mass1,inertia1);
btTransform t1;
t1.setIdentity();
t1.setOrigin(btVector3(_pos[0], wheel_radius + pos[1], half_wheel_distance + pos[2]));
btQuaternion rotation1;
rotation1.setEulerZYX( 0.0, _rot[1], 1.57072428);
t1.setRotation(rotation1);
btMotionState* motion1=new btDefaultMotionState(t1);
btRigidBody::btRigidBodyConstructionInfo info1(mass1,motion1,wheel1,inertia1);
//info1.m_restitution = 0.0f;
info1.m_friction = 0.45f;
//info1.m_rollingFriction = 10.0;
right_wheel=new btRigidBody(info1);
world->addRigidBody(right_wheel);
right_wheel->setLinearFactor(btVector3(1,1,1));
right_wheel->setAngularFactor(btVector3(1,1,1));
btVector3 axisA(0.f, 0.f, 1.0f);
btVector3 axisB(0.f, 1.f, 0.f);
btVector3 pivotA(0.f, -robot_height* 0.22, half_wheel_distance);
btVector3 pivotB(0.0f, 0.0f, 0.0f);
right_hinge = new btHingeConstraint(*body, *right_wheel, pivotA, pivotB, axisA, axisB);
btJointFeedback* feedback1 = new btJointFeedback;
feedback1->m_appliedForceBodyA = btVector3(0, 0, 0);
feedback1->m_appliedForceBodyB = btVector3(0, 0, 0);
feedback1->m_appliedTorqueBodyA = btVector3(0, 0, 0);
feedback1->m_appliedTorqueBodyB = btVector3(0, 0, 0);
right_hinge->setJointFeedback(feedback1);
right_hinge->enableFeedback(true);
right_hinge->setLimit(1,-1,1.0,0.3,1);
right_hinge->enableAngularMotor(false, 0,0);
//right_hinge->setOverrideNumSolverIterations(1000);
world->addConstraint(right_hinge, true);
btCylinderShape* wheel2 = new btCylinderShape(btVector3(wheel_radius,wheel_width,wheel_radius));
double mass2 = 0.15;
btVector3 inertia2(0.0,0.0,0.0);
wheel2->calculateLocalInertia(mass2,inertia2);
btTransform t2;
t2.setIdentity();
t2.setOrigin(btVector3(pos[0], wheel_radius + pos[1], half_wheel_distance + pos[2]));
btQuaternion rotation2;
rotation2.setEulerZYX( 0.0, _rot[1], 1.57072428);
t2.setRotation(rotation2);
btMotionState* motion2=new btDefaultMotionState(t2);
//wheel2->setMargin(0.0);
btRigidBody::btRigidBodyConstructionInfo info2(mass2,motion2,wheel2,inertia2);
//info2.m_restitution = 0.0f;
info2.m_friction = 0.45f;
// info2.m_rollingFriction = 10.0;
left_wheel=new btRigidBody(info2);
right_wheel->setLinearFactor(btVector3(1,1,1));
right_wheel->setAngularFactor(btVector3(1,1,1));
world->addRigidBody(left_wheel);
btVector3 axisA1(0.0f, 0.0f, 1.0f);
btVector3 axisB1(0.0f, 1.0f, 0.0f);
btVector3 pivotA1(0.f, -robot_height* 0.22,-half_wheel_distance);
btVector3 pivotB1(0.0f, 0.0f, 0.0f);
left_hinge = new btHingeConstraint(*body, *left_wheel, pivotA1, pivotB1, axisA1, axisB1);
left_hinge->enableFeedback(true);
btJointFeedback* feedback = new btJointFeedback;
feedback->m_appliedForceBodyA = btVector3(0, 0, 0);
feedback->m_appliedForceBodyB = btVector3(0, 0, 0);
feedback->m_appliedTorqueBodyA = btVector3(0, 0, 0);
feedback->m_appliedTorqueBodyB = btVector3(0, 0, 0);
left_hinge->setJointFeedback(feedback);
left_hinge->setLimit(1,-1,1.0,0.3,1);
left_hinge->enableAngularMotor(false, 0,0);
//left_hinge->setOverrideNumSolverIterations(1000);
world->addConstraint(left_hinge, true);
}
void SliderConstraintDemo::initPhysics()
{
setTexturing(true);
setShadows(true);
setCameraDistance(26.f);
// init world
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
#if SLIDER_DEMO_USE_ODE_SOLVER
m_constraintSolver = new btOdeQuickstepConstraintSolver();
#else
m_constraintSolver = new btSequentialImpulseConstraintSolver();
#endif
btDiscreteDynamicsWorld* wp = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
// wp->getSolverInfo().m_numIterations = 20; // default is 10
m_dynamicsWorld = wp;
// wp->getSolverInfo().m_erp = 0.8;
// add floor
//btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-76,0));
btRigidBody* groundBody;
groundBody = localCreateRigidBody(0, groundTransform, groundShape);
// add box shape (will be reused for all bodies)
btCollisionShape* shape = new btBoxShape(btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
m_collisionShapes.push_back(shape);
// mass of dymamic bodies
btScalar mass = btScalar(1.);
// add dynamic rigid body A1
btTransform trans;
trans.setIdentity();
btVector3 worldPos(-20,0,0);
trans.setOrigin(worldPos);
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInB = btTransform::getIdentity();
#if SLIDER_ENABLE_ALL_DEMOS
btRigidBody* pRbA1 = localCreateRigidBody(mass, trans, shape);
// btRigidBody* pRbA1 = localCreateRigidBody(0.f, trans, shape);
pRbA1->setActivationState(DISABLE_DEACTIVATION);
// add dynamic rigid body B1
worldPos.setValue(-30,0,0);
trans.setOrigin(worldPos);
btRigidBody* pRbB1 = localCreateRigidBody(mass, trans, shape);
// btRigidBody* pRbB1 = localCreateRigidBody(0.f, trans, shape);
pRbB1->setActivationState(DISABLE_DEACTIVATION);
// create slider constraint between A1 and B1 and add it to world
#if SLIDER_DEMO_USE_6DOF
spSlider1 = new btGeneric6DofConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true);
btVector3 lowerSliderLimit = btVector3(-20,0,0);
btVector3 hiSliderLimit = btVector3(-10,0,0);
// btVector3 lowerSliderLimit = btVector3(-20,-5,-5);
// btVector3 hiSliderLimit = btVector3(-10,5,5);
spSlider1->setLinearLowerLimit(lowerSliderLimit);
spSlider1->setLinearUpperLimit(hiSliderLimit);
spSlider1->setAngularLowerLimit(btVector3(0,0,0));
spSlider1->setAngularUpperLimit(btVector3(0,0,0));
#else
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);
#endif
m_dynamicsWorld->addConstraint(spSlider1, true);
spSlider1->setDbgDrawSize(btScalar(5.f));
#endif
#if SLIDER_ENABLE_ALL_DEMOS
// add kinematic rigid body A2
// worldPos.setValue(20,4,0);
worldPos.setValue(5,-20,0);
trans.setOrigin(worldPos);
btRigidBody* pRbA2 = localCreateRigidBody(0., trans, shape);
// btRigidBody* pRbA2 = localCreateRigidBody(mass, trans, shape);
// btRigidBody* pRbA2 = localCreateRigidBody(mass * 10000, trans, shape);
pRbA2->setActivationState(DISABLE_DEACTIVATION);
// add dynamic rigid body B2
// worldPos.setValue(-20,4,0);
worldPos.setValue(-5,-20,0);
trans.setOrigin(worldPos);
// btRigidBody* pRbB2 = localCreateRigidBody(0., trans, shape);
btRigidBody* pRbB2 = localCreateRigidBody(mass, trans, shape);
// btRigidBody* pRbB2 = localCreateRigidBody(mass * 10000, trans, shape);
pRbB2->setActivationState(DISABLE_DEACTIVATION);
// frameInA.getBasis().setEulerZYX(1.f, 1.f, 1.f);
// frameInB.getBasis().setEulerZYX(1.f, 1.f, 1.f);
// frameInA.getBasis().setEulerZYX(1.f, 1.f, 1.f);
// frameInB.getBasis().setEulerZYX(1.f, 1.f, 1.f);
// frameInA.setOrigin(btVector3(-20., 5., 0));
// frameInB.setOrigin(btVector3( 20., 5., 0));
frameInA.setOrigin(btVector3(-5., 20., 0));
frameInB.setOrigin(btVector3( 5., 20., 0));
// create slider constraint between A2 and B2 and add it to world
#if SLIDER_DEMO_USE_6DOF
spSlider2 = new btGeneric6DofConstraint(*pRbA2, *pRbB2, frameInA, frameInB, true);
spSlider2->setLinearLowerLimit(lowerSliderLimit);
spSlider2->setLinearUpperLimit(hiSliderLimit);
spSlider2->setAngularLowerLimit(btVector3(0,0,0));
spSlider2->setAngularUpperLimit(btVector3(0,0,0));
#else
spSlider2 = new btSliderConstraint(*pRbA2, *pRbB2, frameInA, frameInB, true);
// spSlider2 = new btSliderConstraint(*pRbA2, *pRbB2, frameInA, frameInB, false);
// spSlider2->setLowerLinLimit(0.0F);
// spSlider2->setUpperLinLimit(0.0F);
spSlider2->setLowerLinLimit(-2.0F);
spSlider2->setUpperLinLimit(2.0F);
// spSlider2->setLowerLinLimit(5.0F);
// spSlider2->setUpperLinLimit(25.0F);
// spSlider2->setUpperLinLimit(-5.0F);
// spSlider2->setUpperLinLimit(-9.99F);
// spSlider2->setLowerAngLimit(SIMD_PI / 2.0F);
// spSlider2->setUpperAngLimit(-SIMD_PI / 2.0F);
// spSlider2->setLowerAngLimit(-SIMD_PI / 2.0F);
// spSlider2->setUpperAngLimit(SIMD_PI / 2.0F);
// spSlider2->setLowerAngLimit(-SIMD_PI);
// spSlider2->setUpperAngLimit(SIMD_PI *0.8F);
// spSlider2->setLowerAngLimit(-0.01F);
// spSlider2->setUpperAngLimit(0.01F);
spSlider2->setLowerAngLimit(-1.570796326F * 0.5f);
spSlider2->setUpperAngLimit(1.570796326F * 0.5f);
// spSlider2->setLowerAngLimit(1.F);
// spSlider2->setUpperAngLimit(-1.F);
// spSlider2->setDampingLimLin(0.5f);
#if 0
// add motors
spSlider2->setPoweredLinMotor(true);
spSlider2->setMaxLinMotorForce(0.1);
spSlider2->setTargetLinMotorVelocity(-5.0);
spSlider2->setPoweredAngMotor(true);
// spSlider2->setMaxAngMotorForce(0.01);
spSlider2->setMaxAngMotorForce(10.0);
spSlider2->setTargetAngMotorVelocity(1.0);
// change default damping and restitution
spSlider2->setDampingDirLin(0.005F);
spSlider2->setRestitutionLimLin(1.1F);
#endif
// various ODE tests
// spSlider2->setDampingLimLin(0.1F); // linear bounce factor for ODE == 1.0 - DampingLimLin;
// spSlider2->setDampingLimAng(0.1F); // angular bounce factor for ODE == 1.0 - DampingLimAng;
// spSlider2->setSoftnessOrthoAng(0.1);
// spSlider2->setSoftnessOrthoLin(0.1);
// spSlider2->setSoftnessLimLin(0.1);
// spSlider2->setSoftnessLimAng(0.1);
#endif
m_dynamicsWorld->addConstraint(spSlider2, true);
spSlider2->setDbgDrawSize(btScalar(5.f));
#endif
#if SLIDER_ENABLE_ALL_DEMOS
{
// add dynamic rigid body A1
trans.setIdentity();
worldPos.setValue(20,0,0);
trans.setOrigin(worldPos);
btRigidBody* pRbA3 = localCreateRigidBody(0.0F, trans, shape);
pRbA3->setActivationState(DISABLE_DEACTIVATION);
// add dynamic rigid body B1
worldPos.setValue(25,0,0);
trans.setOrigin(worldPos);
btRigidBody* pRbB3 = localCreateRigidBody(mass, trans, shape);
pRbB3->setActivationState(DISABLE_DEACTIVATION);
btVector3 pivA( 2.5, 0., 0.);
btVector3 pivB(-2.5, 0., 0.);
spP2PConst = new btPoint2PointConstraint(*pRbA3, *pRbB3, pivA, pivB);
m_dynamicsWorld->addConstraint(spP2PConst, true);
spP2PConst->setDbgDrawSize(btScalar(5.f));
}
#endif
#if 0 // SLIDER_ENABLE_ALL_DEMOS
// add dynamic rigid body A4
trans.setIdentity();
worldPos.setValue(20,10,0);
trans.setOrigin(worldPos);
btRigidBody* pRbA4 = localCreateRigidBody(0.0F, trans, shape);
pRbA4->setActivationState(DISABLE_DEACTIVATION);
// add dynamic rigid body B1
worldPos.setValue(27,10,0);
trans.setOrigin(worldPos);
btRigidBody* pRbB4 = localCreateRigidBody(mass, trans, shape);
pRbB1->setActivationState(DISABLE_DEACTIVATION);
btVector3 pivA( 2., 0., 0.);
btVector3 pivB(-5., 0., 0.);
btVector3 axisA(0., 0., 1.);
btVector3 axisB(0., 0., 1.);
spHingeConst = new btHingeConstraint(*pRbA4, *pRbB4, pivA, pivB, axisA, axisB);
// spHingeConst->setLimit(-1.57, 1.57);
spHingeConst->setLimit(1.57, -1.57);
spHingeConst->enableAngularMotor(true, 10.0, 0.19);
m_dynamicsWorld->addConstraint(spHingeConst, true);
spHingeConst->setDbgDrawSize(btScalar(5.f));
#endif
} // SliderConstraintDemo::initPhysics()
int runCTest( const std::string& testName )
{
const std::string fileName( "testconstraint.osg" );
// Create two rigid bodies for testing.
osg::ref_ptr< osg::Group > root = new osg::Group;
osg::Node* node = osgDB::readNodeFile( "tetra.osg" );
if( node == NULL )
ERROR("Init:","Can't load model data file.");
osg::Matrix aXform = osg::Matrix::translate( 4., 2., 0. );
osgwTools::AbsoluteModelTransform* amt = new osgwTools::AbsoluteModelTransform;
amt->setDataVariance( osg::Object::DYNAMIC );
amt->addChild( node );
root->addChild( amt );
osg::ref_ptr< osgbDynamics::CreationRecord > cr = new osgbDynamics::CreationRecord;
cr->_sceneGraph = amt;
cr->_shapeType = BOX_SHAPE_PROXYTYPE;
cr->_mass = .5;
cr->_parentTransform = aXform;
btRigidBody* rbA = osgbDynamics::createRigidBody( cr.get() );
node = osgDB::readNodeFile( "block.osg" );
if( node == NULL )
ERROR("Init:","Can't load model data file.");
osg::Matrix bXform = osg::Matrix::identity();
amt = new osgwTools::AbsoluteModelTransform;
amt->setDataVariance( osg::Object::DYNAMIC );
amt->addChild( node );
root->addChild( amt );
cr = new osgbDynamics::CreationRecord;
cr->_sceneGraph = amt;
cr->_shapeType = BOX_SHAPE_PROXYTYPE;
cr->_mass = 4.;
cr->_parentTransform = bXform;
btRigidBody* rbB = osgbDynamics::createRigidBody( cr.get() );
//
// SliderConstraint
if( testName == std::string( "Slider" ) )
{
osg::Vec3 axis( 0., 0., 1. );
osg::Vec2 limits( -4., 4. );
osg::ref_ptr< osgbDynamics::SliderConstraint > cons = new osgbDynamics::SliderConstraint(
rbA, aXform, rbB, bXform, axis, limits );
if( cons->getAsBtSlider() == NULL )
ERROR(testName,"won't typecast as btSliderConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::SliderConstraint > cons2 = dynamic_cast<
osgbDynamics::SliderConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// TwistSliderConstraint
if( testName == std::string( "TwistSlider" ) )
{
osg::Vec3 axis( 0., 0., 1. );
osg::Vec3 point( 1., 2., 3. );
osg::Vec2 linLimits( -4., 4. );
osg::Vec2 angLimits( -1., 2. );
osg::ref_ptr< osgbDynamics::TwistSliderConstraint > cons = new osgbDynamics::TwistSliderConstraint(
rbA, aXform, rbB, bXform, axis, point, linLimits, angLimits );
if( cons->getAsBtSlider() == NULL )
ERROR(testName,"won't typecast as btSliderConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::TwistSliderConstraint > cons2 = dynamic_cast<
osgbDynamics::TwistSliderConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// BallAndSocketConstraint
if( testName == std::string( "BallAndSocket" ) )
{
osg::Vec3 point( -5., 5., 3. );
osg::ref_ptr< osgbDynamics::BallAndSocketConstraint > cons = new osgbDynamics::BallAndSocketConstraint(
rbA, aXform, rbB, bXform, point );
if( cons->getAsBtPoint2Point() == NULL )
ERROR(testName,"won't typecast as btPoint2PointConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::BallAndSocketConstraint > cons2 = dynamic_cast<
osgbDynamics::BallAndSocketConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// FixedConstraint
if( testName == std::string( "Fixed" ) )
{
osg::ref_ptr< osgbDynamics::FixedConstraint > cons = new osgbDynamics::FixedConstraint(
rbA, aXform, rbB, bXform );
if( cons->getAsBtGeneric6Dof() == NULL )
ERROR(testName,"won't typecast as btGeneric6DofConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::FixedConstraint > cons2 = dynamic_cast<
osgbDynamics::FixedConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// PlanarConstraint
if( testName == std::string( "Planar" ) )
{
osg::Vec2 loLimit( -2., -3. );
osg::Vec2 hiLimit( 1., 4. );
osg::Matrix orient( osg::Matrix::identity() );
osg::ref_ptr< osgbDynamics::PlanarConstraint > cons = new osgbDynamics::PlanarConstraint(
rbA, aXform, rbB, bXform, loLimit, hiLimit, orient );
if( cons->getAsBtGeneric6Dof() == NULL )
ERROR(testName,"won't typecast as btGeneric6DofConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::PlanarConstraint > cons2 = dynamic_cast<
osgbDynamics::PlanarConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// BoxConstraint
if( testName == std::string( "Box" ) )
{
osg::Vec3 loLimit( -2., -3., 0. );
osg::Vec3 hiLimit( 1., 4., 2. );
osg::Matrix orient( osg::Matrix::identity() );
osg::ref_ptr< osgbDynamics::BoxConstraint > cons = new osgbDynamics::BoxConstraint(
rbA, aXform, rbB, bXform, loLimit, hiLimit, orient );
if( cons->getAsBtGeneric6Dof() == NULL )
ERROR(testName,"won't typecast as btGeneric6DofConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::BoxConstraint > cons2 = dynamic_cast<
osgbDynamics::BoxConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// LinearSpringConstraint
if( testName == std::string( "LinearSpring" ) )
{
osg::ref_ptr< osgbDynamics::LinearSpringConstraint > cons = new osgbDynamics::LinearSpringConstraint(
rbA, aXform, rbB, bXform, osg::Vec3( 2., 1., 0. ) );
cons->setLimit( osg::Vec2( -2., 3. ) );
cons->setStiffness( 40.f );
cons->setDamping( .5f );
if( cons->getAsBtGeneric6DofSpring() == NULL )
ERROR(testName,"won't typecast as btGeneric6DofSpringConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::LinearSpringConstraint > cons2 = dynamic_cast<
osgbDynamics::LinearSpringConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// AngleSpringConstraint
if( testName == std::string( "AngleSpring" ) )
{
osg::ref_ptr< osgbDynamics::AngleSpringConstraint > cons = new osgbDynamics::AngleSpringConstraint(
rbA, aXform, rbB, bXform, osg::Vec3( 2., 1., 0. ), osg::Vec3( 5., 6., -7. ) );
cons->setLimit( osg::Vec2( -2., 1. ) );
cons->setStiffness( 50.f );
cons->setDamping( 0.f );
if( cons->getAsBtGeneric6DofSpring() == NULL )
ERROR(testName,"won't typecast as btGeneric6DofSpringConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::AngleSpringConstraint > cons2 = dynamic_cast<
osgbDynamics::AngleSpringConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// LinearAngleSpringConstraint
if( testName == std::string( "LinearAngleSpring" ) )
{
osg::ref_ptr< osgbDynamics::LinearAngleSpringConstraint > cons = new osgbDynamics::LinearAngleSpringConstraint(
rbA, aXform, rbB, bXform, osg::Vec3( 2., 1., 0. ), osg::Vec3( 5., 6., -7. ) );
cons->setLinearLimit( osg::Vec2( -2., 2. ) );
cons->setAngleLimit( osg::Vec2( -3., 3. ) );
cons->setLinearStiffness( 41.f );
cons->setLinearDamping( 1.f );
cons->setAngleStiffness( 42.f );
cons->setAngleDamping( 2.f );
if( cons->getAsBtGeneric6DofSpring() == NULL )
ERROR(testName,"won't typecast as btGeneric6DofSpringConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::LinearAngleSpringConstraint > cons2 = dynamic_cast<
osgbDynamics::LinearAngleSpringConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// HingeConstraint
if( testName == std::string( "Hinge" ) )
{
osg::Vec3 axis( -1., 0., 0. );
osg::Vec3 point( 4., 3., 2. );
osg::Vec2 limit( -2., 2. );
osg::ref_ptr< osgbDynamics::HingeConstraint > cons = new osgbDynamics::HingeConstraint(
rbA, aXform, rbB, bXform, axis, point, limit );
if( cons->getAsBtHinge() == NULL )
ERROR(testName,"won't typecast as btHingeConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::HingeConstraint > cons2 = dynamic_cast<
osgbDynamics::HingeConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// CardanConstraint
if( testName == std::string( "Cardan" ) )
{
osg::Vec3 axisA( -1., 0., 0. );
osg::Vec3 axisB( 0., 0., 1. );
osg::ref_ptr< osgbDynamics::CardanConstraint > cons = new osgbDynamics::CardanConstraint(
rbA, aXform, rbB, bXform, axisA, axisB );
if( cons->getAsBtUniversal() == NULL )
ERROR(testName,"won't typecast as btUniversalConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::CardanConstraint > cons2 = dynamic_cast<
osgbDynamics::CardanConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// RagdollConstraint
if( testName == std::string( "Ragdoll" ) )
{
osg::Vec3 point( 0., 1., 2. );
osg::Vec3 axis( 0., 1., 0. );
double angle = 2.;
osg::ref_ptr< osgbDynamics::RagdollConstraint > cons = new osgbDynamics::RagdollConstraint(
rbA, aXform, rbB, bXform, point, axis, angle );
if( cons->getAsBtConeTwist() == NULL )
ERROR(testName,"won't typecast as btConeTwistConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::RagdollConstraint > cons2 = dynamic_cast<
osgbDynamics::RagdollConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// WheelSuspensionConstraint
if( testName == std::string( "WheelSuspension" ) )
{
osg::Vec3 springAxis( 0., 0., 1. );
osg::Vec3 axleAxis( 0., 1., 0. );
osg::Vec2 linearLimit( -2., 3. );
osg::Vec2 angleLimit( -1., 1. );
osg::Vec3 anchor( 0., 1., 2. );
osg::ref_ptr< osgbDynamics::WheelSuspensionConstraint > cons = new osgbDynamics::WheelSuspensionConstraint(
rbA, rbB, springAxis, axleAxis, linearLimit, angleLimit, anchor );
if( cons->getAsBtHinge2() == NULL )
ERROR(testName,"won't typecast as btHinge2Constraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::WheelSuspensionConstraint > cons2 = dynamic_cast<
osgbDynamics::WheelSuspensionConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
ERROR(testName,"unknown test name.");
}
