void BulletXmlImportDemo::initPhysics()
{
setTexturing(true);
setShadows(true);
setupEmptyDynamicsWorld();
m_dynamicsWorld->setDebugDrawer(&gDebugDrawer);
btBulletXmlWorldImporter* importer = new btBulletXmlWorldImporter(m_dynamicsWorld);
importer->loadFile("bullet_basic.xml");
// importer->loadFile("bulletser.xml");
// importer->loadFile("bullet_constraints.xml");
}
void SerializeDemo::initPhysics()
{
setTexturing(true);
setShadows(true);
setCameraDistance(btScalar(SCALING*50.));
setupEmptyDynamicsWorld();
#ifdef DESERIALIZE_SOFT_BODIES
btBulletWorldImporter* fileLoader = new MySoftBulletWorldImporter((btSoftRigidDynamicsWorld*)m_dynamicsWorld);
#else
btBulletWorldImporter* fileLoader = new btBulletWorldImporter(m_dynamicsWorld);
#endif //DESERIALIZE_SOFT_BODIES
// fileLoader->setVerboseMode(true);
if (!fileLoader->loadFile("testFile.bullet"))
{
btAssert(0);
exit(0);
}
}
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 SerializeDemo::initPhysics()
{
setTexturing(true);
setShadows(false);//true);
setCameraDistance(btScalar(SCALING*30.));
setupEmptyDynamicsWorld();
#ifdef DESERIALIZE_SOFT_BODIES
m_fileLoader = new MySoftBulletWorldImporter((btSoftRigidDynamicsWorld*)m_dynamicsWorld);
#else
m_fileLoader = new btBulletWorldImporter(m_dynamicsWorld);
#endif //DESERIALIZE_SOFT_BODIES
m_fileLoader->setVerboseMode(m_verboseMode);
if (!m_fileLoader->loadFile("testFile.bullet"))
// if (!m_fileLoader->loadFile("../SoftDemo/testFile.bullet"))
{
///create a few basic rigid bodies and save them to testFile.bullet
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
btCollisionObject* groundObject = 0;
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
{
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
groundObject = body;
}
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
int numSpheres = 2;
btVector3 positions[2] = {btVector3(0.1f,0.2f,0.3f),btVector3(0.4f,0.5f,0.6f)};
btScalar radii[2] = {0.3f,0.4f};
btMultiSphereShape* colShape = new btMultiSphereShape(positions,radii,numSpheres);
//btCollisionShape* colShape = new btCapsuleShapeZ(SCALING*1,SCALING*1);
//btCollisionShape* colShape = new btCylinderShapeZ(btVector3(SCALING*1,SCALING*1,SCALING*1));
//btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
float start_x = START_POS_X - ARRAY_SIZE_X/2;
float start_y = START_POS_Y;
float start_z = START_POS_Z - ARRAY_SIZE_Z/2;
for (int k=0;k<ARRAY_SIZE_Y;k++)
{
for (int i=0;i<ARRAY_SIZE_X;i++)
{
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
startTransform.setOrigin(SCALING*btVector3(
btScalar(2.0*i + start_x),
btScalar(20+2.0*k + start_y),
btScalar(2.0*j + start_z)));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body);
//body->setActivationState(ISLAND_SLEEPING);
}
}
}
}
int maxSerializeBufferSize = 1024*1024*5;
btDefaultSerializer* serializer = new btDefaultSerializer(maxSerializeBufferSize);
static const char* groundName = "GroundName";
serializer->registerNameForPointer(groundObject, groundName);
for (int i=0;i<m_collisionShapes.size();i++)
{
char* name = new char[20];
sprintf(name,"name%d",i);
serializer->registerNameForPointer(m_collisionShapes[i],name);
}
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*(btRigidBody*)getDynamicsWorld()->getCollisionObjectArray()[2],btVector3(0,1,0));
m_dynamicsWorld->addConstraint(p2p);
const char* name = "constraintje";
serializer->registerNameForPointer(p2p,name);
m_dynamicsWorld->serialize(serializer);
#if 1
FILE* f2 = fopen("testFile.bullet","wb");
fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1,f2);
fclose(f2);
#endif
}
//clientResetScene();
}
void ConvexDecompositionDemo::initPhysics(const char* filename)
{
gContactAddedCallback = &MyContactCallback;
setupEmptyDynamicsWorld();
getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
setTexturing(true);
setShadows(true);
setCameraDistance(26.f);
#ifndef NO_OBJ_TO_BULLET
ConvexDecomposition::WavefrontObj wo;
tcount = 0;
const char* prefix[]={"./","../","../../","../../../","../../../../", "ConvexDecompositionDemo/", "Demos/ConvexDecompositionDemo/",
"../Demos/ConvexDecompositionDemo/","../../Demos/ConvexDecompositionDemo/"};
int numPrefixes = sizeof(prefix)/sizeof(const char*);
char relativeFileName[1024];
for (int i=0;i<numPrefixes;i++)
{
sprintf(relativeFileName,"%s%s",prefix[i],filename);
tcount = wo.loadObj(relativeFileName);
if (tcount)
break;
}
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-4.5,0));
btCollisionShape* boxShape = new btBoxShape(btVector3(30,2,30));
m_collisionShapes.push_back(boxShape);
localCreateRigidBody(0.f,startTransform,boxShape);
class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
{
ConvexDecompositionDemo* m_convexDemo;
public:
btAlignedObjectArray<btConvexHullShape*> m_convexShapes;
btAlignedObjectArray<btVector3> m_convexCentroids;
MyConvexDecomposition (FILE* outputFile,ConvexDecompositionDemo* demo)
:m_convexDemo(demo),
mBaseCount(0),
mHullCount(0),
mOutputFile(outputFile)
{
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_convexDemo->m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
//export data to .obj
printf("ConvexResult. ");
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
centroid.setValue(0,0,0);
btAlignedObjectArray<btVector3> vertices;
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
centroid += vertex;
}
}
centroid *= 1.f/(float(result.mHullVcount) );
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
vertex -= centroid ;
vertices.push_back(vertex);
}
}
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++;
btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
vertex0 -= centroid;
vertex1 -= centroid;
vertex2 -= centroid;
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 );
}
}
// float mass = 1.f;
//this is a tools issue: due to collision margin, convex objects overlap, compensate for it here:
//#define SHRINK_OBJECT_INWARDS 1
#ifdef SHRINK_OBJECT_INWARDS
float collisionMargin = 0.01f;
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(vertices,planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int p=0;p<planeEquations.size();p++)
{
btVector3 plane = planeEquations[p];
plane[3] += collisionMargin;
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray<btVector3> shiftedVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,shiftedVertices);
btConvexHullShape* convexShape = new btConvexHullShape(&(shiftedVertices[0].getX()),shiftedVertices.size());
#else //SHRINK_OBJECT_INWARDS
btConvexHullShape* convexShape = new btConvexHullShape(&(vertices[0].getX()),vertices.size());
#endif
if (sEnableSAT)
convexShape->initializePolyhedralFeatures();
convexShape->setMargin(0.01f);
m_convexShapes.push_back(convexShape);
m_convexCentroids.push_back(centroid);
m_convexDemo->m_collisionShapes.push_back(convexShape);
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
int mBaseCount;
int mHullCount;
FILE* mOutputFile;
};
if (tcount)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
int i;
for ( 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];
btVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]);
btVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]);
btVector3 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);
}
btConvexShape* tmpConvexShape = new btConvexTriangleMeshShape(trimesh);
printf("old numTriangles= %d\n",wo.mTriCount);
printf("old numIndices = %d\n",wo.mTriCount*3);
printf("old numVertices = %d\n",wo.mVertexCount);
printf("reducing vertices by creating a convex hull\n");
//create a hull approximation
btShapeHull* hull = new btShapeHull(tmpConvexShape);
btScalar margin = tmpConvexShape->getMargin();
hull->buildHull(margin);
tmpConvexShape->setUserPointer(hull);
printf("new numTriangles = %d\n", hull->numTriangles ());
printf("new numIndices = %d\n", hull->numIndices ());
printf("new numVertices = %d\n", hull->numVertices ());
btConvexHullShape* convexShape = new btConvexHullShape();
bool updateLocalAabb = false;
for (i=0;i<hull->numVertices();i++)
{
convexShape->addPoint(hull->getVertexPointer()[i],updateLocalAabb);
}
convexShape->recalcLocalAabb();
if (sEnableSAT)
convexShape->initializePolyhedralFeatures();
delete tmpConvexShape;
delete hull;
m_collisionShapes.push_back(convexShape);
float mass = 1.f;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,2,14));
localCreateRigidBody(mass, startTransform,convexShape);
bool useQuantization = true;
btCollisionShape* concaveShape = new btBvhTriangleMeshShape(trimesh,useQuantization);
startTransform.setOrigin(convexDecompositionObjectOffset);
localCreateRigidBody(0.f,startTransform,concaveShape);
m_collisionShapes.push_back (concaveShape);
}
if (tcount)
{
//-----------------------------------
// Bullet Convex Decomposition
//-----------------------------------
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 = 5;
float cpercent = 5;
float ppercent = 15;
unsigned int maxv = 16;
float skinWidth = 0.0;
printf("WavefrontObj num triangles read %i\n",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,this);
desc.mCallback = &convexDecomposition;
//-----------------------------------------------
// HACD
//-----------------------------------------------
std::vector< HACD::Vec3<HACD::Real> > points;
std::vector< HACD::Vec3<long> > triangles;
for(int i=0; i<wo.mVertexCount; i++ )
{
int index = i*3;
HACD::Vec3<HACD::Real> vertex(wo.mVertices[index], wo.mVertices[index+1],wo.mVertices[index+2]);
points.push_back(vertex);
}
for(int i=0;i<wo.mTriCount;i++)
{
int index = i*3;
HACD::Vec3<long> triangle(wo.mIndices[index], wo.mIndices[index+1], wo.mIndices[index+2]);
triangles.push_back(triangle);
}
HACD::HACD myHACD;
myHACD.SetPoints(&points[0]);
myHACD.SetNPoints(points.size());
myHACD.SetTriangles(&triangles[0]);
myHACD.SetNTriangles(triangles.size());
myHACD.SetCompacityWeight(0.1);
myHACD.SetVolumeWeight(0.0);
// HACD parameters
// Recommended parameters: 2 100 0 0 0 0
size_t nClusters = 2;
double concavity = 100;
bool invert = false;
bool addExtraDistPoints = false;
bool addNeighboursDistPoints = false;
bool addFacesPoints = false;
myHACD.SetNClusters(nClusters); // minimum number of clusters
myHACD.SetNVerticesPerCH(100); // max of 100 vertices per convex-hull
myHACD.SetConcavity(concavity); // maximum concavity
myHACD.SetAddExtraDistPoints(addExtraDistPoints);
myHACD.SetAddNeighboursDistPoints(addNeighboursDistPoints);
myHACD.SetAddFacesPoints(addFacesPoints);
myHACD.Compute();
nClusters = myHACD.GetNClusters();
myHACD.Save("output.wrl", false);
//convexDecomposition.performConvexDecomposition(desc);
// ConvexBuilder cb(desc.mCallback);
// cb.process(desc);
//now create some bodies
if (1)
{
btCompoundShape* compound = new btCompoundShape();
m_collisionShapes.push_back (compound);
btTransform trans;
trans.setIdentity();
for (int c=0;c<nClusters;c++)
{
//generate convex result
size_t nPoints = myHACD.GetNPointsCH(c);
size_t nTriangles = myHACD.GetNTrianglesCH(c);
float* vertices = new float[nPoints*3];
unsigned int* triangles = new unsigned int[nTriangles*3];
HACD::Vec3<HACD::Real> * pointsCH = new HACD::Vec3<HACD::Real>[nPoints];
HACD::Vec3<long> * trianglesCH = new HACD::Vec3<long>[nTriangles];
myHACD.GetCH(c, pointsCH, trianglesCH);
// points
for(size_t v = 0; v < nPoints; v++)
{
vertices[3*v] = pointsCH[v].X();
vertices[3*v+1] = pointsCH[v].Y();
vertices[3*v+2] = pointsCH[v].Z();
}
// triangles
for(size_t f = 0; f < nTriangles; f++)
{
triangles[3*f] = trianglesCH[f].X();
triangles[3*f+1] = trianglesCH[f].Y();
triangles[3*f+2] = trianglesCH[f].Z();
}
delete [] pointsCH;
delete [] trianglesCH;
ConvexResult r(nPoints, vertices, nTriangles, triangles);
convexDecomposition.ConvexDecompResult(r);
}
for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
{
btVector3 centroid = convexDecomposition.m_convexCentroids[i];
trans.setOrigin(centroid);
btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
compound->addChildShape(trans,convexShape);
btRigidBody* body;
body = localCreateRigidBody( 1.0, trans,convexShape);
}
/* for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
{
btVector3 centroid = convexDecomposition.m_convexCentroids[i];
trans.setOrigin(centroid);
btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
compound->addChildShape(trans,convexShape);
btRigidBody* body;
body = localCreateRigidBody( 1.0, trans,convexShape);
}*/
#if 1
btScalar mass=10.f;
trans.setOrigin(-convexDecompositionObjectOffset);
btRigidBody* body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
convexDecompositionObjectOffset.setZ(6);
trans.setOrigin(-convexDecompositionObjectOffset);
body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
convexDecompositionObjectOffset.setZ(-6);
trans.setOrigin(-convexDecompositionObjectOffset);
body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
#endif
}
if (outputFile)
fclose(outputFile);
}
#ifdef TEST_SERIALIZATION
//test serializing this
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();
//now try again from the loaded file
setupEmptyDynamicsWorld();
#endif //TEST_SERIALIZATION
#endif //NO_OBJ_TO_BULLET
#ifdef TEST_SERIALIZATION
btBulletWorldImporter* fileLoader = new btBulletWorldImporter(m_dynamicsWorld);
//fileLoader->setVerboseMode(true);
fileLoader->loadFile("testFile.bullet");
//fileLoader->loadFile("testFile64Double.bullet");
//fileLoader->loadFile("testFile64Single.bullet");
//fileLoader->loadFile("testFile32Single.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);
}
void GraspTest::initPhysics()
{
setTexturing(true);
setShadows(true);
setCameraDistance(5.f);
m_Time = 0;
setupEmptyDynamicsWorld();
m_dynamicsWorld->setDebugDrawer(&gDebugDrawer);
m_dynamicsWorld->setGravity(btVector3(0.,0.,0.));
//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,0.05,0.05));
m_collisionShapes.push_back(shape);
btTransform trans;
trans.setIdentity();
trans.setOrigin(btVector3(0,20,0));
float mass = 1.f;
if(true){
btTransform tr;
btTransform childTransform;
/*************************** links ***************************/
float fingerX0_width = 0.01;
btCollisionShape* fingerX0_shape = new btBoxShape(btVector3(scale*0.05/2, scale*fingerX0_width/2, scale*0.034/2));
/***************** handbase ******************/
btCompoundShape* handbase_shape = new btCompoundShape();
childTransform.setIdentity();
childTransform.setOrigin(btVector3(0,0,scale*0.0415/2));
handbase_shape->addChildShape(childTransform,new btCylinderShapeZ(btVector3(scale*0.089/2,scale*0.09/2,scale*0.0415/2)));
childTransform.setIdentity();
childTransform.setOrigin(btVector3(scale*-0.05/2, 0, scale*(0.0415 +0.034/2)));
handbase_shape->addChildShape(childTransform,fingerX0_shape);
tr.setIdentity();
tr.setOrigin(btVector3(0., 0., 0.));
tr.getBasis().setEulerZYX(0,0,0);
handbase = localCreateRigidBody( pow(scale,mass_exp)*0.0, tr, handbase_shape);
handbase->setActivationState(DISABLE_DEACTIVATION);
/***************** fingerX0 ******************/
/******** finger00 *********/
btCompoundShape* finger00_shape = new btCompoundShape();
childTransform.setIdentity();
childTransform.setOrigin(btVector3(scale*0.05/2, 0, scale*0.034/2));
finger00_shape->addChildShape(childTransform,fingerX0_shape);
tr.setIdentity();
//tr.setOrigin(btVector3(scale*0.05/2, scale* -0.025, scale*(0.0415 + 0.034/2)));
tr.setOrigin(btVector3(0, scale* -0.025, scale*0.0415));
tr.getBasis().setEulerZYX(0,0,0);
finger00 = localCreateRigidBody( pow(scale,mass_exp)*0.1, tr, finger00_shape);
if (finger00) finger00->setActivationState(DISABLE_DEACTIVATION);
/******** finger10 *********/
btCompoundShape* finger10_shape = new btCompoundShape();
childTransform.setIdentity();
childTransform.setOrigin(btVector3(scale*0.05/2, 0, scale*0.034/2));
finger10_shape->addChildShape(childTransform,fingerX0_shape);
tr.setIdentity();
//tr.setOrigin(btVector3(scale*0.05/2, scale*0.025, scale*(0.0415 + 0.034/2)));
tr.setOrigin(btVector3(0, scale*0.025, scale*(0.0415)));
tr.getBasis().setEulerZYX(0,0,0);
finger10 = localCreateRigidBody( pow(scale,mass_exp)*0.1, tr, finger10_shape);
if (finger10) finger10->setActivationState(DISABLE_DEACTIVATION);
/******** "finger20" *********/
//Using compound shape now
/*
tr.setIdentity();
tr.setOrigin(btVector3(-scale*0.05/2, 0, scale*(0.0415 + 0.034/2)));
tr.getBasis().setEulerZYX(0,0,0);
handbase_finger20 = localCreateRigidBody( 0.1, tr, fingerX0_shape);
if (handbase_finger20) handbase_finger20->setActivationState(DISABLE_DEACTIVATION);
*/
/***************** fingerX1 ******************/
float fingerX1_radius = 0.01;
btCollisionShape* fingerX1_capsule_shape = new btCapsuleShapeX(scale * fingerX1_radius, scale * (0.07 - 2 * fingerX1_radius));
btCompoundShape* fingerX1_shape = new btCompoundShape();
childTransform.setIdentity();
childTransform.setOrigin(btVector3(scale*0.07/2, 0, 0));
fingerX1_shape->addChildShape(childTransform,fingerX1_capsule_shape);
/******** finger01 *********/
tr.setIdentity();
//tr.setOrigin(btVector3(scale*(0.05 + 0.07/2), -scale*0.025, scale*(0.0415 + 0.034)));
tr.setOrigin(btVector3(scale*0.05, -scale*0.025, scale*(0.0415 + 0.034)));
//tr.setBasis(btMatrix3x3(1,0,0,0,0,-1,0,1,0));
tr.getBasis().setEulerZYX(-SIMD_PI_2,0,0);
finger01 = localCreateRigidBody( pow(scale,mass_exp)*0.1, tr, fingerX1_shape);
if (finger01) finger01->setActivationState(DISABLE_DEACTIVATION);
/******** finger11 *********/
tr.setIdentity();
//tr.setOrigin(btVector3(scale*(0.05 + 0.07/2), scale*0.025, scale*(0.0415 + 0.034)));
tr.setOrigin(btVector3(scale*0.05, scale*0.025, scale*(0.0415 + 0.034)));
//tr.setBasis(btMatrix3x3(1,0,0,0,0,-1,0,1,0));
tr.getBasis().setEulerZYX(-SIMD_PI_2,0,0);
finger11 = localCreateRigidBody( 0.1, tr, fingerX1_shape);
if (finger11) finger11->setActivationState(DISABLE_DEACTIVATION);
/******** finger21 *********/
tr.setIdentity();
//tr.setOrigin(btVector3(-scale*(0.05 + 0.07/2), 0., scale*(0.0415 + 0.034)));
tr.setOrigin(btVector3(-scale*0.05, 0., scale*(0.0415 + 0.034)));
tr.setBasis(btMatrix3x3(-1,0,0,0,0,1,0,1,0));
finger21 = localCreateRigidBody( pow(scale,mass_exp)*0.1, tr, fingerX1_shape);
if (finger21) finger21->setActivationState(DISABLE_DEACTIVATION);
/***************** fingerX2 ******************/
float fingerX2_radius = 0.01;
btCollisionShape* fingerX2_capsule_shape = new btCapsuleShapeX(scale * fingerX2_radius, scale * (0.058 - 2 * fingerX2_radius));
btCompoundShape* fingerX2_shape = new btCompoundShape();
childTransform.setIdentity();
childTransform.setOrigin(btVector3(scale*0.058/2, 0, 0));
fingerX2_shape->addChildShape(childTransform,fingerX2_capsule_shape);
btMatrix3x3 basis;
btScalar adjustAngle = 0.8727;
btMatrix3x3 adjust = btMatrix3x3::getIdentity();
adjust.setEulerZYX(0,0,-adjustAngle);
/******** finger02 *********/
tr.setIdentity();
//tr.setOrigin(btVector3(scale*(0.05 + 0.07 + (0.058/2)*cos(adjustAngle)), -scale*0.025, scale*(0.0415 + 0.034 + (0.058/2)*sin(adjustAngle))));
tr.setOrigin(btVector3(scale*(0.05 + 0.07), -scale*0.025, scale*(0.0415 + 0.034)));
//basis = btMatrix3x3(1,0,0,0,0,-1,0,1,0);
basis.setEulerZYX(-SIMD_PI_2,0,0);
basis *= adjust;
tr.setBasis(basis);
finger02 = localCreateRigidBody( pow(scale,mass_exp)*0.1, tr, fingerX2_shape);
if (finger02) finger02->setActivationState(DISABLE_DEACTIVATION);
/******** finger12 *********/
tr.setIdentity();
tr.setOrigin(btVector3(scale*(0.05 + 0.07), scale*0.025, scale*(0.0415 + 0.034)));
//basis = btMatrix3x3(1,0,0,0,0,-1,0,1,0);
basis.setEulerZYX(-SIMD_PI_2,0,0);
basis *= adjust;
tr.setBasis(basis);
finger12 = localCreateRigidBody( pow(scale,mass_exp)*0.1, tr, fingerX2_shape);
if (finger12) finger12->setActivationState(DISABLE_DEACTIVATION);
/******** finger22 *********/
adjust.setEulerZYX(0,0,adjustAngle);
tr.setIdentity();
tr.setOrigin(btVector3(-scale*(0.05 + 0.07), 0., scale*(0.0415 + 0.034)));
basis = btMatrix3x3(-1,0,0,0,0,1,0,1,0);
basis *= adjust;
tr.setBasis(basis);
finger22 = localCreateRigidBody( pow(scale,mass_exp)*0.1, tr, fingerX2_shape);
if (finger22) finger22->setActivationState(DISABLE_DEACTIVATION);
/*************************** constraints ***************************/
btTransform frameInA, frameInB;
/***************** handbase-X0 ******************/
/******** handbase-00 *********/
if (finger00 && true) {
frameInA.setIdentity();
frameInA.setOrigin(btVector3(0,scale*-0.025,scale*0.0415));
frameInA.getBasis().setEulerZYX(0,0,SIMD_PI);
frameInB.setIdentity();
//frameInB.setOrigin(btVector3(scale * -0.05/2,0,scale* -0.034/2));
frameInB.getBasis().setEulerZYX(0,0,SIMD_PI);
j_hb_00_jf4 = new btGeneric6DofSpringConstraint(*handbase,*finger00,frameInA,frameInB,true);
j_hb_00_jf4->setLinearUpperLimit(btVector3(0., 0., 0.));
j_hb_00_jf4->setLinearLowerLimit(btVector3(0., 0., 0.));
j_hb_00_jf4->setAngularLowerLimit(btVector3(0.f, 0.f, -1. * SIMD_PI / 180.));
j_hb_00_jf4->setAngularUpperLimit(btVector3(0.f, 0.f, 180. * SIMD_PI / 180.));
m_dynamicsWorld->addConstraint(j_hb_00_jf4, true);
//j_hb_00_jf4->setDbgDrawSize(btScalar(5.f));
}
/******** handbase-10 *********/
if (finger10 && true) {
frameInA.setIdentity();
frameInA.setOrigin(btVector3(0,scale*0.025,scale*0.0415));
//frameInA.getBasis().setEulerZYX(0,0,SIMD_PI);
frameInB.setIdentity();
//frameInB.setOrigin(btVector3(scale * -0.05/2,0,scale* -0.034/2));
frameInB.getBasis().setEulerZYX(0,0,SIMD_PI);
j_hb_10_jf4mimic = new btGeneric6DofSpringConstraint(*handbase,*finger10,frameInA,frameInB,true);
j_hb_10_jf4mimic->setLinearUpperLimit(btVector3(0., 0., 0.));
j_hb_10_jf4mimic->setLinearLowerLimit(btVector3(0., 0., 0.));
j_hb_10_jf4mimic->setAngularLowerLimit(btVector3(0.f, 0.f, -1. * SIMD_PI / 180.));
j_hb_10_jf4mimic->setAngularUpperLimit(btVector3(0.f, 0.f, 180. * SIMD_PI / 180.));
m_dynamicsWorld->addConstraint(j_hb_10_jf4mimic, true);
//j_hb_10_jf4mimic->setDbgDrawSize(btScalar(5.f));
}
/******** 00-10 gear ********/
if (finger00 && finger01 && true) {
jf4_gear = new btGearConstraint(*finger00,*finger10,btVector3(0,0,1),btVector3(0,0,1),1);
m_dynamicsWorld->addConstraint(jf4_gear, true);
jf4_gear->setDbgDrawSize(10.);
}
/******** handbase-"20" *********/
//Using compound shape now
/*
frameInA.setIdentity();
frameInA.setOrigin(btVector3(0,0,scale*0.0415/2));
frameInA.getBasis().setEulerZYX(SIMD_PI,0,0);
frameInB.setIdentity();
frameInB.setOrigin(btVector3(scale * -0.05/2,0,scale* -0.034/2));
j_hb_20_fixed = new btGeneric6DofSpringConstraint(*handbase,*handbase_finger20,frameInA,frameInB,true);
j_hb_20_fixed->setLinearUpperLimit(btVector3(0., 0., 0.));
j_hb_20_fixed->setLinearLowerLimit(btVector3(0., 0., 0.));
j_hb_20_fixed->setAngularLowerLimit(btVector3(0.f, 0.f, 0));
j_hb_20_fixed->setAngularUpperLimit(btVector3(0.f, 0.f, 0));
m_dynamicsWorld->addConstraint(j_hb_20_fixed, true);
//j_hb_20_fixed->setDbgDrawSize(btScalar(5.f));
*/
/***************** X0-X1 ******************/
/******** 00-01 *********/
if (finger00 && finger01 && true) {
frameInA.setIdentity();
frameInA.setOrigin(btVector3(scale*0.05,0,scale * 0.034));
//frameInA.setBasis(btMatrix3x3(1,0,0,0,0,-1,0,1,0));
frameInA.getBasis().setEulerZYX(-SIMD_PI_2,0,0);
frameInB.setIdentity();
//frameInB.setOrigin(btVector3(scale * -0.07/2,0,0));
frameInB.getBasis().setEulerZYX(0,0,0);
j_00_01_jf1 = new btGeneric6DofSpringConstraint(*finger00,*finger01,frameInA,frameInB,true);
j_00_01_jf1->setLinearUpperLimit(btVector3(0., 0., 0.));
j_00_01_jf1->setLinearLowerLimit(btVector3(0., 0., 0.));
j_00_01_jf1->setAngularLowerLimit(btVector3(0.f, 0.f, 0. * SIMD_PI / 180.));
j_00_01_jf1->setAngularUpperLimit(btVector3(0.f, 0.f, 140. * SIMD_PI / 180.));
m_dynamicsWorld->addConstraint(j_00_01_jf1, true);
//j_00_01_jf1->setDbgDrawSize(btScalar(5.f));
}
/******** 10-11 *********/
if (finger10 && finger11 && true) {
frameInA.setIdentity();
frameInA.setOrigin(btVector3(scale*0.05,0,scale * 0.034));
//frameInA.setBasis(btMatrix3x3(1,0,0,0,0,-1,0,1,0));
frameInA.getBasis().setEulerZYX(-SIMD_PI_2,0,0);
frameInB.setIdentity();
//frameInB.setOrigin(btVector3(scale * -0.07/2,0,0));
frameInB.getBasis().setEulerZYX(0,0,0);
j_10_11_jf2 = new btGeneric6DofSpringConstraint(*finger10,*finger11,frameInA,frameInB,true);
j_10_11_jf2->setLinearUpperLimit(btVector3(0., 0., 0.));
j_10_11_jf2->setLinearLowerLimit(btVector3(0., 0., 0.));
j_10_11_jf2->setAngularLowerLimit(btVector3(0.f, 0.f, 0. * SIMD_PI / 180.));
j_10_11_jf2->setAngularUpperLimit(btVector3(0.f, 0.f, 140. * SIMD_PI / 180.));
m_dynamicsWorld->addConstraint(j_10_11_jf2, true);
//j_10_11_jf2->setDbgDrawSize(btScalar(5.f));
}
/******** handbase-21 *********/
if (finger21 && true) {
frameInA.setIdentity();
//frameInA.setOrigin(btVector3(scale* -0.05,0,scale * (0.0415/2 + 0.034)));
frameInA.setOrigin(btVector3(scale* -0.05,0,scale * (0.0415 + 0.034)));
//frameInA.setBasis(btMatrix3x3(-1,0,0,0,0,1,0,1,0));
frameInA.getBasis().setEulerZYX(-SIMD_PI_2,0,SIMD_PI);
frameInB.setIdentity();
//frameInB.setOrigin(btVector3(scale * -0.07/2,0,0));
frameInB.getBasis().setEulerZYX(SIMD_PI,0,0);
j_20_21_jf3 = new btGeneric6DofSpringConstraint(*handbase,*finger21,frameInA,frameInB,true);
j_20_21_jf3->setLinearUpperLimit(btVector3(0., 0., 0.));
j_20_21_jf3->setLinearLowerLimit(btVector3(0., 0., 0.));
j_20_21_jf3->setAngularLowerLimit(btVector3(0.f, 0.f, 0. * SIMD_PI / 180.));
j_20_21_jf3->setAngularUpperLimit(btVector3(0.f, 0.f, 140. * SIMD_PI / 180.));
m_dynamicsWorld->addConstraint(j_20_21_jf3, true);
//j_20_21_jf3->setDbgDrawSize(btScalar(5.f));
}
/***************** X1-X2 ******************/
/******** 01-02 *********/
if (finger01 && finger02 && true) {
frameInA.setIdentity();
frameInA.setOrigin(btVector3(scale*0.07,0,0));
frameInB.setIdentity();
//frameInB.setOrigin(btVector3(scale * -0.058/2,0,0));
j_01_02_jf1mimic = new btGeneric6DofSpringConstraint(*finger01,*finger02,frameInA,frameInB,true);
j_01_02_jf1mimic->setLinearUpperLimit(btVector3(0., 0., 0.));
j_01_02_jf1mimic->setLinearLowerLimit(btVector3(0., 0., 0.));
j_01_02_jf1mimic->setAngularLowerLimit(btVector3(0.f, 0.f, 50. * SIMD_PI / 180.));
j_01_02_jf1mimic->setAngularUpperLimit(btVector3(0.f, 0.f, 97. * SIMD_PI / 180.));
m_dynamicsWorld->addConstraint(j_01_02_jf1mimic, true);
//j_01_02_jf1mimic->setDbgDrawSize(btScalar(5.f));
}
/******** 01-02 gear ********/
if (finger01 && finger02 && true) {
jf1_gear = new btGearConstraint(*finger01,*finger02,btVector3(0,0,1),btVector3(0,0,1),-0.7);
m_dynamicsWorld->addConstraint(jf1_gear, true);
}
/******** 11-12 *********/
if (finger11 && finger12 && true) {
frameInA.setIdentity();
frameInA.setOrigin(btVector3(scale*0.07,0,0));
frameInB.setIdentity();
//frameInB.setOrigin(btVector3(scale * -0.058/2,0,0));
j_11_12_jf2mimic = new btGeneric6DofSpringConstraint(*finger11,*finger12,frameInA,frameInB,true);
j_11_12_jf2mimic->setLinearUpperLimit(btVector3(0., 0., 0.));
j_11_12_jf2mimic->setLinearLowerLimit(btVector3(0., 0., 0.));
j_11_12_jf2mimic->setAngularLowerLimit(btVector3(0.f, 0.f, 50. * SIMD_PI / 180.));
j_11_12_jf2mimic->setAngularUpperLimit(btVector3(0.f, 0.f, 97. * SIMD_PI / 180.));
m_dynamicsWorld->addConstraint(j_11_12_jf2mimic, true);
//j_11_12_jf2mimic->setDbgDrawSize(btScalar(5.f));
}
/******** 11-12 gear ********/
if (finger11 && finger12 && true) {
jf2_gear = new btGearConstraint(*finger11,*finger12,btVector3(0,0,1),btVector3(0,0,1),-0.7);
m_dynamicsWorld->addConstraint(jf2_gear, true);
}
/******** 21-22 *********/
if (finger21 && finger22 && true) {
frameInA.setIdentity();
frameInA.setOrigin(btVector3(scale*0.07,0,0));
frameInA.getBasis().setEulerZYX(SIMD_PI,0,0);
frameInB.setIdentity();
//frameInB.setOrigin(btVector3(scale * -0.058/2,0,0));
frameInB.getBasis().setEulerZYX(SIMD_PI,0,0);
j_21_22_jf3mimic = new btGeneric6DofSpringConstraint(*finger21,*finger22,frameInA,frameInB,true);
j_21_22_jf3mimic->setLinearUpperLimit(btVector3(0., 0., 0.));
j_21_22_jf3mimic->setLinearLowerLimit(btVector3(0., 0., 0.));
j_21_22_jf3mimic->setAngularLowerLimit(btVector3(0.f, 0.f, 50. * SIMD_PI / 180.));
j_21_22_jf3mimic->setAngularUpperLimit(btVector3(0.f, 0.f, 97. * SIMD_PI / 180.));
m_dynamicsWorld->addConstraint(j_21_22_jf3mimic, true);
//j_21_22_jf3mimic->setDbgDrawSize(btScalar(5.f));
}
/******** 21-22 gear ********/
if (finger21 && finger22 && true) {
jf3_gear = new btGearConstraint(*finger21,*finger22,btVector3(0,0,1),btVector3(0,0,1),-0.7);
m_dynamicsWorld->addConstraint(jf3_gear, true);
}
/******** p2p ********/
if (true) {
btVector3 pivot = (finger12->getCenterOfMassTransform() * btTransform(btMatrix3x3::getIdentity(),btVector3(scale*0.058,0,0))).invXform(btVector3(0,0,scale *0.120));
printf("%f %f %f\n",pivot.getX(), pivot.getY(), pivot.getZ());
frameInB.setIdentity();
frameInB.setOrigin(btVector3(scale*0.058,0,0));
p2p = new btGeneric6DofSpringConstraint(*finger12,frameInB,true);
//p2p->setLinearLowerLimit(btVector3(-1,-1,-1));
//p2p->setLinearUpperLimit(btVector3(1,1,1));
p2p->setLinearLowerLimit(btVector3(pivot.getX(), pivot.getY(), pivot.getZ()));
p2p->setLinearUpperLimit(btVector3(pivot.getX(), pivot.getY(), pivot.getZ()));
//btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*finger22,pivot);
m_dynamicsWorld->addConstraint(p2p, true);
p2p->setDbgDrawSize(btScalar(5.f));
}
}
if (false) {
{
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
link1 = localCreateRigidBody( 1.0, tr, shape);
link1->setActivationState(DISABLE_DEACTIVATION);
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(2.), btScalar(0.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
btRigidBody* link2 = localCreateRigidBody(1.0, tr, shape);
link2->setActivationState(DISABLE_DEACTIVATION);
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInA.setOrigin(btVector3(btScalar(1.), btScalar(0.), btScalar(0.)));
frameInB = btTransform::getIdentity();
frameInB.setOrigin(btVector3(btScalar(-1.), btScalar(0.), btScalar(0.)));
c12 = new btGeneric6DofSpringConstraint(*link1, *link2, frameInA, frameInB, true);
c12->setLinearUpperLimit(btVector3(0., 0., 0.));
c12->setLinearLowerLimit(btVector3(0., 0., 0.));
c12->setAngularLowerLimit(btVector3(0.f, 0.f, -1.5f));
c12->setAngularUpperLimit(btVector3(0.f, 0.f, 1.5f));
m_dynamicsWorld->addConstraint(c12, true);
c12->setDbgDrawSize(btScalar(2.5f));
//c12->enableSpring(0, true);
//c12->setStiffness(0, 39.478f);
//c12->setDamping(0, 0.5f);
//c12->enableSpring(5, true);
//c12->setStiffness(5, 39.478f);
//c12->setDamping(5, 0.3f);
//c12->setEquilibriumPoint(5,-1.5f/3);
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(4.), btScalar(0.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
link3 = localCreateRigidBody(1.0, tr, shape);
link3->setActivationState(DISABLE_DEACTIVATION);
c23 = new btGeneric6DofSpringConstraint(*link2, *link3, frameInA, frameInB, true);
c23->setLinearUpperLimit(btVector3(0., 0., 0.));
c23->setLinearLowerLimit(btVector3(0., 0., 0.));
c23->setAngularLowerLimit(btVector3(0.f, 0.f, -1.5f));
c23->setAngularUpperLimit(btVector3(0.f, 0.f, 1.5f));
m_dynamicsWorld->addConstraint(c23, true);
c23->setDbgDrawSize(btScalar(2.5f));
}
{
btTransform frameInA;
frameInA = btTransform::getIdentity();
frameInA.setOrigin(btVector3(btScalar(-1), btScalar(0.), btScalar(0.)));
c1w = new btGeneric6DofSpringConstraint(*link1, frameInA, true);
c1w->setLinearUpperLimit(btVector3(0., 0., 0.));
c1w->setLinearLowerLimit(btVector3(0., 0., 0.));
c1w->setAngularLowerLimit(btVector3(0.f, 0.f, -1.5f));
c1w->setAngularUpperLimit(btVector3(0.f, 0.f, 1.5f));
m_dynamicsWorld->addConstraint(c1w, true);
c1w->setDbgDrawSize(btScalar(5.f));
}
{
btTransform frameInA;
frameInA = btTransform::getIdentity();
frameInA.setOrigin(btVector3(btScalar(1), btScalar(0.), btScalar(0.)));
c3w = new btGeneric6DofSpringConstraint(*link3, frameInA, true);
c3w->setLinearUpperLimit(btVector3(-0.5, 0., 0.));
c3w->setLinearLowerLimit(btVector3(-0.5, 0., 0.));
c3w->setAngularLowerLimit(btVector3(0.f, 0.f, -1.5f));
c3w->setAngularUpperLimit(btVector3(0.f, 0.f, 1.5f));
m_dynamicsWorld->addConstraint(c3w, true);
c3w->setDbgDrawSize(btScalar(5.f));
}
}
}
void ConvexDecompositionDemo::initPhysics(const char* filename)
{
gContactAddedCallback = &MyContactCallback;
setupEmptyDynamicsWorld();
setTexturing(true);
setShadows(true);
setCameraDistance(26.f);
#ifndef NO_OBJ_TO_BULLET
ConvexDecomposition::WavefrontObj wo;
tcount = wo.loadObj(filename);
if (!tcount)
{
//when running this app from visual studio, the default starting folder is different, so make a second attempt...
tcount = wo.loadObj("../../file.obj");
}
if (!tcount)
{
//cmake generated msvc files need 4 levels deep back... so make a 3rd attempt...
tcount = wo.loadObj("../../../../file.obj");
}
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-4.5,0));
btCollisionShape* boxShape = new btBoxShape(btVector3(30,2,30));
m_collisionShapes.push_back(boxShape);
localCreateRigidBody(0.f,startTransform,boxShape);
class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
{
ConvexDecompositionDemo* m_convexDemo;
public:
btAlignedObjectArray<btConvexHullShape*> m_convexShapes;
btAlignedObjectArray<btVector3> m_convexCentroids;
MyConvexDecomposition (FILE* outputFile,ConvexDecompositionDemo* demo)
:m_convexDemo(demo),
mBaseCount(0),
mHullCount(0),
mOutputFile(outputFile)
{
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_convexDemo->m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
//export data to .obj
printf("ConvexResult. ");
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
centroid.setValue(0,0,0);
btAlignedObjectArray<btVector3> vertices;
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
centroid += vertex;
}
}
centroid *= 1.f/(float(result.mHullVcount) );
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
vertex -= centroid ;
vertices.push_back(vertex);
}
}
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++;
btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
vertex0 -= centroid;
vertex1 -= centroid;
vertex2 -= centroid;
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 );
}
}
// float mass = 1.f;
//this is a tools issue: due to collision margin, convex objects overlap, compensate for it here:
//#define SHRINK_OBJECT_INWARDS 1
#ifdef SHRINK_OBJECT_INWARDS
float collisionMargin = 0.01f;
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(vertices,planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int p=0;p<planeEquations.size();p++)
{
btVector3 plane = planeEquations[p];
plane[3] += collisionMargin;
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray<btVector3> shiftedVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,shiftedVertices);
btConvexHullShape* convexShape = new btConvexHullShape(&(shiftedVertices[0].getX()),shiftedVertices.size());
#else //SHRINK_OBJECT_INWARDS
btConvexHullShape* convexShape = new btConvexHullShape(&(vertices[0].getX()),vertices.size());
#endif
convexShape->setMargin(0.01f);
m_convexShapes.push_back(convexShape);
m_convexCentroids.push_back(centroid);
m_convexDemo->m_collisionShapes.push_back(convexShape);
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
int mBaseCount;
int mHullCount;
FILE* mOutputFile;
};
if (tcount)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
int i;
for ( 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];
btVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]);
btVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]);
btVector3 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);
}
btConvexShape* tmpConvexShape = new btConvexTriangleMeshShape(trimesh);
printf("old numTriangles= %d\n",wo.mTriCount);
printf("old numIndices = %d\n",wo.mTriCount*3);
printf("old numVertices = %d\n",wo.mVertexCount);
printf("reducing vertices by creating a convex hull\n");
//create a hull approximation
btShapeHull* hull = new btShapeHull(tmpConvexShape);
btScalar margin = tmpConvexShape->getMargin();
hull->buildHull(margin);
tmpConvexShape->setUserPointer(hull);
printf("new numTriangles = %d\n", hull->numTriangles ());
printf("new numIndices = %d\n", hull->numIndices ());
printf("new numVertices = %d\n", hull->numVertices ());
btConvexHullShape* convexShape = new btConvexHullShape();
for (i=0;i<hull->numVertices();i++)
{
convexShape->addPoint(hull->getVertexPointer()[i]);
}
delete tmpConvexShape;
delete hull;
m_collisionShapes.push_back(convexShape);
float mass = 1.f;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,2,14));
localCreateRigidBody(mass, startTransform,convexShape);
bool useQuantization = true;
btCollisionShape* concaveShape = new btBvhTriangleMeshShape(trimesh,useQuantization);
startTransform.setOrigin(convexDecompositionObjectOffset);
localCreateRigidBody(0.f,startTransform,concaveShape);
m_collisionShapes.push_back (concaveShape);
}
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 = 5;
float cpercent = 5;
float ppercent = 15;
unsigned int maxv = 16;
float skinWidth = 0.0;
printf("WavefrontObj num triangles read %i\n",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,this);
desc.mCallback = &convexDecomposition;
//convexDecomposition.performConvexDecomposition(desc);
ConvexBuilder cb(desc.mCallback);
cb.process(desc);
//now create some bodies
if (1)
{
btCompoundShape* compound = new btCompoundShape();
m_collisionShapes.push_back (compound);
btTransform trans;
trans.setIdentity();
for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
{
btVector3 centroid = convexDecomposition.m_convexCentroids[i];
trans.setOrigin(centroid);
btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
compound->addChildShape(trans,convexShape);
btRigidBody* body;
body = localCreateRigidBody( 1.0, trans,convexShape);
}
#if 1
btScalar mass=10.f;
trans.setOrigin(-convexDecompositionObjectOffset);
btRigidBody* body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
convexDecompositionObjectOffset.setZ(6);
trans.setOrigin(-convexDecompositionObjectOffset);
body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
convexDecompositionObjectOffset.setZ(-6);
trans.setOrigin(-convexDecompositionObjectOffset);
body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
#endif
}
if (outputFile)
fclose(outputFile);
}
#ifdef TEST_SERIALIZATION
//test serializing this
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();
//now try again from the loaded file
setupEmptyDynamicsWorld();
#endif //TEST_SERIALIZATION
#endif //NO_OBJ_TO_BULLET
#ifdef TEST_SERIALIZATION
btBulletWorldImporter* fileLoader = new btBulletWorldImporter(m_dynamicsWorld);
//fileLoader->setVerboseMode(true);
fileLoader->loadFile("testFile.bullet");
//fileLoader->loadFile("testFile64Double.bullet");
//fileLoader->loadFile("testFile64Single.bullet");
//fileLoader->loadFile("testFile32Single.bullet");
#endif //TEST_SERIALIZATION
}
