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 elfRecalcJoint(elfJoint* joint)
{
elfVec3f pos1;
elfVec3f pos2;
elfVec4f qua1;
elfVec4f qua2;
elfVec4f aqua1;
elfVec4f aqua2;
elfVec3f worldCoordPivot1;
elfVec3f localCoordPivot2;
elfVec3f localAxis1;
elfVec3f localAxis2;
float matrix1[16];
float matrix2[16];
float matrix3[16];
pos1 = elfGetActorPosition(joint->actor1);
pos2 = elfGetActorPosition(joint->actor2);
qua1 = elfGetActorOrientation(joint->actor1);
qua2 = elfGetActorOrientation(joint->actor2);
worldCoordPivot1.x = joint->pivot[0];
worldCoordPivot1.y = joint->pivot[1];
worldCoordPivot1.z = joint->pivot[2];
worldCoordPivot1 = elfAddVec3fVec3f(elfMulQuaVec3f(qua1, worldCoordPivot1), pos1);
localCoordPivot2 = elfMulQuaVec3f(elfGetQuaInverted(qua2), elfSubVec3fVec3f(worldCoordPivot1, pos2));
btVector3 pivotInA(joint->pivot[0], joint->pivot[1], joint->pivot[2]);
btVector3 pivotInB(localCoordPivot2.x, localCoordPivot2.y, localCoordPivot2.z);
aqua1 = elfCreateQuaFromEuler(joint->axis[0], joint->axis[1], joint->axis[2]);
localAxis1 = elfMulQuaVec3f(aqua1, elfCreateVec3f(0.0f, 0.0f, 1.0f));
aqua2 = elfMulQuaQua(aqua1, elfMulQuaQua(qua1, elfGetQuaInverted(qua2)));
localAxis2 = elfMulQuaVec3f(aqua2, elfCreateVec3f(0.0f, 0.0f, 1.0f));
btVector3 axisInA(localAxis1.x, localAxis1.y, localAxis1.z);
btVector3 axisInB(localAxis2.x, localAxis2.y, localAxis2.z);
if(joint->jointType == ELF_HINGE)
{
joint->constraint = new btHingeConstraint(*joint->actor1->object->body,* joint->actor2->object->body,
pivotInA, pivotInB, axisInA, axisInB);
joint->actor1->scene->world->world->addConstraint(joint->constraint);
}
else if(joint->jointType == ELF_BALL)
{
joint->constraint = new btPoint2PointConstraint(*joint->actor1->object->body,* joint->actor2->object->body, pivotInA, pivotInB);
joint->actor1->scene->world->world->addConstraint(joint->constraint);
}
else if(joint->jointType == ELF_CONE_TWIST)
{
gfxMatrix4SetIdentity(matrix1);
matrix1[12] = joint->pivot[0];
matrix1[13] = joint->pivot[1];
matrix1[14] = joint->pivot[2];
gfxQuaToMatrix4(&aqua1.x, matrix2);
gfxMulMatrix4Matrix4(matrix1, matrix2, matrix3);
btTransform frameInA;
frameInA.setFromOpenGLMatrix(matrix3);
gfxMatrix4SetIdentity(matrix1);
matrix1[12] = localCoordPivot2.x;
matrix1[13] = localCoordPivot2.y;
matrix1[14] = localCoordPivot2.z;
gfxQuaToMatrix4(&aqua2.x, matrix2);
gfxMulMatrix4Matrix4(matrix1, matrix2, matrix3);
btTransform frameInB;
frameInB.setFromOpenGLMatrix(matrix3);
joint->constraint = new btConeTwistConstraint(*joint->actor1->object->body,
*joint->actor2->object->body, frameInA, frameInB);
joint->actor1->scene->world->world->addConstraint(joint->constraint);
}
}
int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
float pivotX,float pivotY,float pivotZ,
float axisX,float axisY,float axisZ)
{
CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
ASSERT(rb0);
SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
SimdVector3 axisInA(axisX,axisY,axisZ);
SimdVector3 axisInB = rb1 ?
(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) :
rb0->getCenterOfMassTransform().getBasis() * axisInA;
bool angularOnly = false;
switch (type)
{
case PHY_POINT2POINT_CONSTRAINT:
{
Point2PointConstraint* p2p = 0;
if (rb1)
{
p2p = new Point2PointConstraint(*rb0,
*rb1,pivotInA,pivotInB);
} else
{
p2p = new Point2PointConstraint(*rb0,
pivotInA);
}
m_constraints.push_back(p2p);
p2p->SetUserConstraintId(gConstraintUid++);
p2p->SetUserConstraintType(type);
//64 bit systems can't cast pointer to int. could use size_t instead.
return p2p->GetUserConstraintId();
break;
}
case PHY_GENERIC_6DOF_CONSTRAINT:
{
Generic6DofConstraint* genericConstraint = 0;
if (rb1)
{
SimdTransform frameInA;
SimdTransform frameInB;
SimdVector3 axis1, axis2;
SimdPlaneSpace1( axisInA, axis1, axis2 );
frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
axisInA.y(), axis1.y(), axis2.y(),
axisInA.z(), axis1.z(), axis2.z() );
SimdPlaneSpace1( axisInB, axis1, axis2 );
frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
axisInB.y(), axis1.y(), axis2.y(),
axisInB.z(), axis1.z(), axis2.z() );
frameInA.setOrigin( pivotInA );
frameInB.setOrigin( pivotInB );
genericConstraint = new Generic6DofConstraint(
*rb0,*rb1,
frameInA,frameInB);
} else
{
// TODO: Implement single body case...
}
m_constraints.push_back(genericConstraint);
genericConstraint->SetUserConstraintId(gConstraintUid++);
genericConstraint->SetUserConstraintType(type);
//64 bit systems can't cast pointer to int. could use size_t instead.
return genericConstraint->GetUserConstraintId();
break;
}
case PHY_ANGULAR_CONSTRAINT:
angularOnly = true;
case PHY_LINEHINGE_CONSTRAINT:
{
HingeConstraint* hinge = 0;
if (rb1)
{
hinge = new HingeConstraint(
*rb0,
*rb1,pivotInA,pivotInB,axisInA,axisInB);
} else
{
hinge = new HingeConstraint(*rb0,
pivotInA,axisInA);
}
hinge->setAngularOnly(angularOnly);
m_constraints.push_back(hinge);
hinge->SetUserConstraintId(gConstraintUid++);
hinge->SetUserConstraintType(type);
//64 bit systems can't cast pointer to int. could use size_t instead.
return hinge->GetUserConstraintId();
break;
}
#ifdef NEW_BULLET_VEHICLE_SUPPORT
case PHY_VEHICLE_CONSTRAINT:
{
RaycastVehicle::VehicleTuning* tuning = new RaycastVehicle::VehicleTuning();
RigidBody* chassis = rb0;
DefaultVehicleRaycaster* raycaster = new DefaultVehicleRaycaster(this,ctrl0);
RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster);
WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
m_wrapperVehicles.push_back(wrapperVehicle);
vehicle->SetUserConstraintId(gConstraintUid++);
vehicle->SetUserConstraintType(type);
return vehicle->GetUserConstraintId();
break;
};
#endif //NEW_BULLET_VEHICLE_SUPPORT
default:
{
}
};
//RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB
return 0;
}
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);
}
int main(int argc,char** argv)
{
int i;
for (i=0;i<numObjects;i++)
{
if (i>0)
{
shapePtr[i] = prebuildShapePtr[1];
shapeIndex[i] = 1;//sphere
}
else
{
shapeIndex[i] = 0;
shapePtr[i] = prebuildShapePtr[0];
}
}
ConvexDecomposition::WavefrontObj wo;
char* filename = "file.obj";
tcount = wo.loadObj(filename);
class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
{
public:
MyConvexDecomposition (FILE* outputFile)
:mBaseCount(0),
mHullCount(0),
mOutputFile(outputFile)
{
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{
TriangleMesh* trimesh = new TriangleMesh();
SimdVector3 localScaling(6.f,6.f,6.f);
//export data to .obj
printf("ConvexResult\n");
if (mOutputFile)
{
fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );
fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount);
fprintf(mOutputFile,"o Object%i\r\n",mBaseCount);
for (unsigned int i=0; i<result.mHullVcount; i++)
{
const float *p = &result.mHullVertices[i*3];
fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] );
}
//calc centroid, to shift vertices around center of mass
centroids[numObjects] = SimdVector3(0,0,0);
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
unsigned int index0 = *src++;
unsigned int index1 = *src++;
unsigned int index2 = *src++;
SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
centroids[numObjects] += vertex0;
centroids[numObjects]+= vertex1;
centroids[numObjects]+= vertex2;
}
}
centroids[numObjects] *= 1.f/(float(result.mHullTcount) * 3);
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
unsigned int index0 = *src++;
unsigned int index1 = *src++;
unsigned int index2 = *src++;
SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
vertex0 -= centroids[numObjects];
vertex1 -= centroids[numObjects];
vertex2 -= centroids[numObjects];
trimesh->AddTriangle(vertex0,vertex1,vertex2);
index0+=mBaseCount;
index1+=mBaseCount;
index2+=mBaseCount;
fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 );
}
}
shapeIndex[numObjects] = numObjects;
shapePtr[numObjects++] = new ConvexTriangleMeshShape(trimesh);
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
int mBaseCount;
int mHullCount;
FILE* mOutputFile;
};
if (tcount)
{
numObjects = 1; //always have the ground object first
TriangleMesh* trimesh = new TriangleMesh();
SimdVector3 localScaling(6.f,6.f,6.f);
for (int i=0;i<wo.mTriCount;i++)
{
int index0 = wo.mIndices[i*3];
int index1 = wo.mIndices[i*3+1];
int index2 = wo.mIndices[i*3+2];
SimdVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]);
SimdVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]);
SimdVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
trimesh->AddTriangle(vertex0,vertex1,vertex2);
}
shapePtr[numObjects++] = new ConvexTriangleMeshShape(trimesh);
}
if (tcount)
{
char outputFileName[512];
strcpy(outputFileName,filename);
char *dot = strstr(outputFileName,".");
if ( dot )
*dot = 0;
strcat(outputFileName,"_convex.obj");
FILE* outputFile = fopen(outputFileName,"wb");
unsigned int depth = 7;
float cpercent = 5;
float ppercent = 15;
unsigned int maxv = 16;
float skinWidth = 0.01;
printf("WavefrontObj num triangles read %i",tcount);
ConvexDecomposition::DecompDesc desc;
desc.mVcount = wo.mVertexCount;
desc.mVertices = wo.mVertices;
desc.mTcount = wo.mTriCount;
desc.mIndices = (unsigned int *)wo.mIndices;
desc.mDepth = depth;
desc.mCpercent = cpercent;
desc.mPpercent = ppercent;
desc.mMaxVertices = maxv;
desc.mSkinWidth = skinWidth;
MyConvexDecomposition convexDecomposition(outputFile);
desc.mCallback = &convexDecomposition;
//convexDecomposition.performConvexDecomposition(desc);
ConvexBuilder cb(desc.mCallback);
int ret = cb.process(desc);
if (outputFile)
fclose(outputFile);
}
CollisionDispatcher* dispatcher = new CollisionDispatcher();
SimdVector3 worldAabbMin(-10000,-10000,-10000);
SimdVector3 worldAabbMax(10000,10000,10000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax);
//OverlappingPairCache* broadphase = new SimpleBroadphase();
physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
physicsEnvironmentPtr->setDeactivationTime(2.f);
physicsEnvironmentPtr->setGravity(0,-10,0);
PHY_ShapeProps shapeProps;
shapeProps.m_do_anisotropic = false;
shapeProps.m_do_fh = false;
shapeProps.m_do_rot_fh = false;
shapeProps.m_friction_scaling[0] = 1.;
shapeProps.m_friction_scaling[1] = 1.;
shapeProps.m_friction_scaling[2] = 1.;
shapeProps.m_inertia = 1.f;
shapeProps.m_lin_drag = 0.2f;
shapeProps.m_ang_drag = 0.1f;
shapeProps.m_mass = 10.0f;
PHY_MaterialProps materialProps;
materialProps.m_friction = 10.5f;
materialProps.m_restitution = 0.0f;
CcdConstructionInfo ccdObjectCi;
ccdObjectCi.m_friction = 0.5f;
ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag;
ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag;
SimdTransform tr;
tr.setIdentity();
for (i=0;i<numObjects;i++)
{
shapeProps.m_shape = shapePtr[shapeIndex[i]];
shapeProps.m_shape->SetMargin(0.05f);
bool isDyna = i>0;
//if (i==1)
// isDyna=false;
if (0)//i==1)
{
SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI);
ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
}
if (i>0)
{
switch (i)
{
case 1:
{
ms[i].setWorldPosition(0,10,0);
//for testing, rotate the ground cube so the stack has to recover a bit
break;
}
case 2:
{
ms[i].setWorldPosition(0,8,2);
break;
}
default:
ms[i].setWorldPosition(0,i*CUBE_HALF_EXTENTS*2 - CUBE_HALF_EXTENTS,0);
}
float quatIma0,quatIma1,quatIma2,quatReal;
SimdQuaternion quat;
SimdVector3 axis(0,0,1);
SimdScalar angle=0.5f;
quat.setRotation(axis,angle);
ms[i].setWorldOrientation(quat.getX(),quat.getY(),quat.getZ(),quat[3]);
} else
{
ms[i].setWorldPosition(0,-10+EXTRA_HEIGHT,0);
}
ccdObjectCi.m_MotionState = &ms[i];
ccdObjectCi.m_gravity = SimdVector3(0,0,0);
ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0);
if (!isDyna)
{
shapeProps.m_mass = 0.f;
ccdObjectCi.m_mass = shapeProps.m_mass;
ccdObjectCi.m_collisionFlags = CollisionObject::isStatic;
}
else
{
shapeProps.m_mass = 1.f;
ccdObjectCi.m_mass = shapeProps.m_mass;
ccdObjectCi.m_collisionFlags = 0;
}
SimdVector3 localInertia;
if (shapePtr[shapeIndex[i]]->GetShapeType() == EMPTY_SHAPE_PROXYTYPE)
{
//take inertia from first shape
shapePtr[1]->CalculateLocalInertia(shapeProps.m_mass,localInertia);
} else
{
shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia);
}
ccdObjectCi.m_localInertiaTensor = localInertia;
ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]];
physObjects[i]= new CcdPhysicsController( ccdObjectCi);
// Only do CCD if motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS
physObjects[i]->GetRigidBody()->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS;
//Experimental: better estimation of CCD Time of Impact:
//physObjects[i]->GetRigidBody()->m_ccdSweptShereRadius = 0.5*CUBE_HALF_EXTENTS;
physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]);
if (i==1)
{
//physObjects[i]->SetAngularVelocity(0,0,-2,true);
}
physicsEnvironmentPtr->setDebugDrawer(&debugDrawer);
}
//create a constraint
if (createConstraint)
{
//physObjects[i]->SetAngularVelocity(0,0,-2,true);
int constraintId;
float pivotX=CUBE_HALF_EXTENTS,
pivotY=-CUBE_HALF_EXTENTS,
pivotZ=CUBE_HALF_EXTENTS;
float axisX=1,axisY=0,axisZ=0;
HingeConstraint* hinge = 0;
SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
SimdVector3 axisInA(0,1,0);
SimdVector3 axisInB(0,-1,0);
RigidBody* rb0 = physObjects[1]->GetRigidBody();
RigidBody* rb1 = physObjects[2]->GetRigidBody();
hinge = new HingeConstraint(
*rb0,
*rb1,pivotInA,pivotInB,axisInA,axisInB);
physicsEnvironmentPtr->m_constraints.push_back(hinge);
hinge->SetUserConstraintId(100);
hinge->SetUserConstraintType(PHY_LINEHINGE_CONSTRAINT);
}
clientResetScene();
setCameraDistance(26.f);
return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://www.continuousphysics.com/Bullet/phpBB2/");
}
void BasicDemo::initPhysics(){
//printing
debug_print_ = false;
//set up world
broadphase_ = new btDbvtBroadphase();
collisionConfiguration_ = new btDefaultCollisionConfiguration();
dispatcher_ = new btCollisionDispatcher(collisionConfiguration_);
solver_ = new btSequentialImpulseConstraintSolver;
dynamicsWorld_ = new btDiscreteDynamicsWorld(dispatcher_,broadphase_,solver_,collisionConfiguration_);
//set gravity. y is apparently up.
dynamicsWorld_->setGravity(btVector3(0,-10,0));
//--------------OBJECT CREATION SECTION-----------------//
//make the ground plane
groundCollisionShape_ = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
//collisionShapes_.push_back(groundCollisionShape_);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0.0f,-55.0f,0.0f));
btScalar groundMass(0.0);
//create the ground plane rigid body
groundRigidBody_ = createRigidBody(groundCollisionShape_,groundMass,groundTransform);
//add it to the dynamics world
dynamicsWorld_->addRigidBody(groundRigidBody_);
//Calculate the required origins based on startPose_ given by the runSimulation function
btVector3 bravoOrigin = startPose_;
btVector3 tangoOrigin = startPose_;
//tangoOrigin.setX(tangoOrigin.getX()+1.5);
//make bravo rigid body (the sliding box)
const btVector3 bravoBoxHalfExtents( 1.0f, 1.0f, 1.0f );
bravoCollisionShape_ = new btBoxShape(bravoBoxHalfExtents);
//collisionShapes_.push_back(bravoCollisionShape_);
btTransform bravoTransform;
bravoTransform.setIdentity();
//bravoTransform.setOrigin(btVector3(0.0f,1.0f,0.0f));
bravoTransform.setOrigin(bravoOrigin);
btScalar bravoMass = 1.0f;
//create the bravo rigid body
bravoRigidBody_ = createRigidBody(bravoCollisionShape_,bravoMass,bravoTransform);
//add it to the dynamics world
dynamicsWorld_->addRigidBody(bravoRigidBody_);
//make tango rigid body (the robots manipulator)
const btVector3 tangoBoxHalfExtents( 0.1f, 0.5f, 0.1f );
tangoCollisionShape_ = new btBoxShape(tangoBoxHalfExtents);
//collisionShapes_.push_back(tangoCollisionShape_);
btTransform tangoTransform;
tangoTransform.setIdentity();
//tangoTransform.setOrigin(btVector3(1.5f,1.0f,0.0f));
tangoTransform.setOrigin(tangoOrigin);
btScalar tangoMass = 0.1f;
//create the tango rigid body
tangoRigidBody_ = createRigidBody(tangoCollisionShape_,tangoMass,tangoTransform);
//add it to the dynamics world
dynamicsWorld_->addRigidBody(tangoRigidBody_);
//--------------END OBJECT CREATION SECTION-----------------//
//--------------CONSTRAINT CREATION SECTION-----------------//
// create a constraint
//const btVector3 pivotInA( 1.5f, 0.0f, 0.0f );
const btVector3 pivotInA( 0.0f, 0.0f, 0.0f );
const btVector3 pivotInB( 0.0f, 0.0f, 0.0f );
btVector3 axisInA( 0.0f, 1.0f, 0.0f );
btVector3 axisInB( 0.0f, 1.0f, 0.0f );
bool useReferenceFrameA = false;
hingeConstraint_ = new btHingeConstraint(*bravoRigidBody_,*tangoRigidBody_,pivotInA,pivotInB,axisInA,axisInB,useReferenceFrameA);
// set joint feedback
hingeConstraint_->setJointFeedback(&jfRobot_);
// add constraint to the world
const bool isDisableCollisionsBetweenLinkedBodies = true; //this used to be false
dynamicsWorld_->addConstraint(hingeConstraint_,
isDisableCollisionsBetweenLinkedBodies);
//--------------END CONSTRAINT CREATION SECTION-----------------//
//--------------INITIALIZE REMAINING PARAMETERS SECTION-----------------//
//set controller values
pGains_.setValue(5.0f,50.0f,5.0f);
dGains_.setValue(2.0f,2.0f,2.0f);
//initialize
tangoRigidBody_->getMotionState()->getWorldTransform(tangoBodyTrans_);
currentPose_ = tangoBodyTrans_.getOrigin();
desiredPose_ = currentPose_;
bravoRigidBody_->forceActivationState(4);
count_ = 0;
//--------------END INITIALIZE REMAINING PARAMETERS SECTION-----------------//
}
void TestHingeTorque::initPhysics()
{
int upAxis = 1;
m_guiHelper->setUpAxis(upAxis);
createEmptyDynamicsWorld();
m_dynamicsWorld->getSolverInfo().m_splitImpulse = false;
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
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
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
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));
btCollisionShape* colOb = 0;
if (i==0)
{
colOb = linkBox1;
} else
{
colOb = linkSphere;
}
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)
{
//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);
bool useReferenceA = true;
btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody,
pivotInA,pivotInB,
axisInA,axisInB,useReferenceA);
con = hinge;
} 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
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;
}
btAssert(con);
if (con)
{
btJointFeedback* fb = new btJointFeedback();
m_jointFeedback.push_back(fb);
con->setJointFeedback(fb);
m_dynamicsWorld->addConstraint(con,true);
}
prevBody = linkBody;
}
}
if (1)
{
btVector3 groundHalfExtents(1,1,0.2);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
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;
start.setOrigin(groundOrigin);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box);
body->setFriction(0);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
