Exemplo n.º 1
0
void Hinge2Vehicle::initPhysics()
{
	m_guiHelper->setUpAxis(1);


	btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
	m_collisionShapes.push_back(groundShape);
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	btVector3 worldMin(-1000,-1000,-1000);
	btVector3 worldMax(1000,1000,1000);
	m_broadphase = new btAxisSweep3(worldMin,worldMax);
	if (useMCLPSolver)
	{
		btDantzigSolver* mlcp = new btDantzigSolver();
		//btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel;
		btMLCPSolver* sol = new btMLCPSolver(mlcp);
		m_solver = sol;
	} else
	{
		m_solver = new btSequentialImpulseConstraintSolver();
	}
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
	if (useMCLPSolver)
	{
		m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 1;//for direct solver it is better to have a small A matrix
	} else
	{
		m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 128;//for direct solver, it is better to solve multiple objects together, small batches have high overhead
	}
	m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
	

	//m_dynamicsWorld->setGravity(btVector3(0,0,0));
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0,-3,0));

//either use heightfield or triangle mesh


	//create ground object
	localCreateRigidBody(0,tr,groundShape);

	btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,0.5f,2.f));
	m_collisionShapes.push_back(chassisShape);

	btCompoundShape* compound = new btCompoundShape();
	m_collisionShapes.push_back(compound);
	btTransform localTrans;
	localTrans.setIdentity();
	//localTrans effectively shifts the center of mass with respect to the chassis
	localTrans.setOrigin(btVector3(0,1,0));

	compound->addChildShape(localTrans,chassisShape);

	{
		btCollisionShape* suppShape = new btBoxShape(btVector3(0.5f,0.1f,0.5f));
		btTransform suppLocalTrans;
		suppLocalTrans.setIdentity();
		//localTrans effectively shifts the center of mass with respect to the chassis
		suppLocalTrans.setOrigin(btVector3(0,1.0,2.5));
		compound->addChildShape(suppLocalTrans, suppShape);
	}

	tr.setOrigin(btVector3(0,0.f,0));

	btScalar chassisMass = 800;
	m_carChassis = localCreateRigidBody(chassisMass,tr,compound);//chassisShape);
	//m_carChassis->setDamping(0.2,0.2);
	
	//m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth,wheelRadius,wheelRadius));
	m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth,wheelRadius,wheelRadius));
	

	//const float position[4]={0,10,10,0};
	//const float quaternion[4]={0,0,0,1};
	//const float color[4]={0,1,0,1};
	//const float scaling[4] = {1,1,1,1};

	btVector3 wheelPos[4] = {
		btVector3(btScalar(-1.), btScalar(-0.25), btScalar(1.25)),
		btVector3(btScalar(1.), btScalar(-0.25), btScalar(1.25)),
		btVector3(btScalar(1.), btScalar(-0.25), btScalar(-1.25)),
		btVector3(btScalar(-1.), btScalar(-0.25), btScalar(-1.25))
	};

	
	for (int i=0;i<4;i++)
	{ 
		// create a Hinge2 joint
		// create two rigid bodies
		// static bodyA (parent) on top:
		
		
		btRigidBody* pBodyA = this->m_carChassis;//m_chassis;//createRigidBody( 0.0, tr, m_wheelShape);
		pBodyA->setActivationState(DISABLE_DEACTIVATION);
		// dynamic bodyB (child) below it :
		btTransform tr;
		tr.setIdentity();
		tr.setOrigin(wheelPos[i]);
		
		btRigidBody* pBodyB = createRigidBody(10.0, tr, m_wheelShape);
		pBodyB->setFriction(1110);
		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 = tr.getOrigin();//(0.f, 0.f, 0.f);
		btHinge2Constraint* pHinge2 = new btHinge2Constraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis);
		
		//m_guiHelper->get2dCanvasInterface();
		

		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
		{
			int motorAxis = 3;
			pHinge2->enableMotor(motorAxis,true);
			pHinge2->setMaxMotorForce(motorAxis,1000);
			pHinge2->setTargetVelocity(motorAxis,-1);
		}

		{
			int motorAxis = 5;
			pHinge2->enableMotor(motorAxis,true);
			pHinge2->setMaxMotorForce(motorAxis,1000);
			pHinge2->setTargetVelocity(motorAxis,0);
		}

		pHinge2->setDbgDrawSize(btScalar(5.f));
	}


	{
		btCollisionShape* liftShape = new btBoxShape(btVector3(0.5f,2.0f,0.05f));
		m_collisionShapes.push_back(liftShape);
		btTransform liftTrans;
		m_liftStartPos = btVector3(0.0f, 2.5f, 3.05f);
		liftTrans.setIdentity();
		liftTrans.setOrigin(m_liftStartPos);
		m_liftBody = localCreateRigidBody(10,liftTrans, liftShape);

		btTransform localA, localB;
		localA.setIdentity();
		localB.setIdentity();
		localA.getBasis().setEulerZYX(0, M_PI_2, 0);
		localA.setOrigin(btVector3(0.0, 1.0, 3.05));
		localB.getBasis().setEulerZYX(0, M_PI_2, 0);
		localB.setOrigin(btVector3(0.0, -1.5, -0.05));
		m_liftHinge = new btHingeConstraint(*m_carChassis,*m_liftBody, localA, localB);
//		m_liftHinge->setLimit(-LIFT_EPS, LIFT_EPS);
		m_liftHinge->setLimit(0.0f, 0.0f);
		m_dynamicsWorld->addConstraint(m_liftHinge, true);

		btCollisionShape* forkShapeA = new btBoxShape(btVector3(1.0f,0.1f,0.1f));
		m_collisionShapes.push_back(forkShapeA);
		btCompoundShape* forkCompound = new btCompoundShape();
		m_collisionShapes.push_back(forkCompound);
		btTransform forkLocalTrans;
		forkLocalTrans.setIdentity();
		forkCompound->addChildShape(forkLocalTrans, forkShapeA);

		btCollisionShape* forkShapeB = new btBoxShape(btVector3(0.1f,0.02f,0.6f));
		m_collisionShapes.push_back(forkShapeB);
		forkLocalTrans.setIdentity();
		forkLocalTrans.setOrigin(btVector3(-0.9f, -0.08f, 0.7f));
		forkCompound->addChildShape(forkLocalTrans, forkShapeB);

		btCollisionShape* forkShapeC = new btBoxShape(btVector3(0.1f,0.02f,0.6f));
		m_collisionShapes.push_back(forkShapeC);
		forkLocalTrans.setIdentity();
		forkLocalTrans.setOrigin(btVector3(0.9f, -0.08f, 0.7f));
		forkCompound->addChildShape(forkLocalTrans, forkShapeC);

		btTransform forkTrans;
		m_forkStartPos = btVector3(0.0f, 0.6f, 3.2f);
		forkTrans.setIdentity();
		forkTrans.setOrigin(m_forkStartPos);
		m_forkBody = localCreateRigidBody(5, forkTrans, forkCompound);

		localA.setIdentity();
		localB.setIdentity();
		localA.getBasis().setEulerZYX(0, 0, M_PI_2);
		localA.setOrigin(btVector3(0.0f, -1.9f, 0.05f));
		localB.getBasis().setEulerZYX(0, 0, M_PI_2);
		localB.setOrigin(btVector3(0.0, 0.0, -0.1));
		m_forkSlider = new btSliderConstraint(*m_liftBody, *m_forkBody, localA, localB, true);
		m_forkSlider->setLowerLinLimit(0.1f);
		m_forkSlider->setUpperLinLimit(0.1f);
//		m_forkSlider->setLowerAngLimit(-LIFT_EPS);
//		m_forkSlider->setUpperAngLimit(LIFT_EPS);
		m_forkSlider->setLowerAngLimit(0.0f);
		m_forkSlider->setUpperAngLimit(0.0f);
		m_dynamicsWorld->addConstraint(m_forkSlider, true);


		btCompoundShape* loadCompound = new btCompoundShape();
		m_collisionShapes.push_back(loadCompound);
		btCollisionShape* loadShapeA = new btBoxShape(btVector3(2.0f,0.5f,0.5f));
		m_collisionShapes.push_back(loadShapeA);
		btTransform loadTrans;
		loadTrans.setIdentity();
		loadCompound->addChildShape(loadTrans, loadShapeA);
		btCollisionShape* loadShapeB = new btBoxShape(btVector3(0.1f,1.0f,1.0f));
		m_collisionShapes.push_back(loadShapeB);
		loadTrans.setIdentity();
		loadTrans.setOrigin(btVector3(2.1f, 0.0f, 0.0f));
		loadCompound->addChildShape(loadTrans, loadShapeB);
		btCollisionShape* loadShapeC = new btBoxShape(btVector3(0.1f,1.0f,1.0f));
		m_collisionShapes.push_back(loadShapeC);
		loadTrans.setIdentity();
		loadTrans.setOrigin(btVector3(-2.1f, 0.0f, 0.0f));
		loadCompound->addChildShape(loadTrans, loadShapeC);
		loadTrans.setIdentity();
		m_loadStartPos = btVector3(0.0f, 3.5f, 7.0f);
		loadTrans.setOrigin(m_loadStartPos);
		m_loadBody  = localCreateRigidBody(loadMass, loadTrans, loadCompound);
	}



	
	resetForklift();
	
//	setCameraDistance(26.f);

	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
Exemplo n.º 2
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

}
Exemplo n.º 3
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);


}