btRigidBody* Room::CreateBulletWall(btVector3 originVector, btVector3 shape, Physics* physicsEngine){
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(originVector);
btScalar groundMass(0.); //the mass is 0, because the ground is immovable (static)
btVector3 localGroundInertia(0, 0, 0);
btCollisionShape* groundShape = new btBoxShape(shape);
btDefaultMotionState *groundMotionState = new btDefaultMotionState(groundTransform);
groundShape->calculateLocalInertia(groundMass, localGroundInertia);
btRigidBody::btRigidBodyConstructionInfo groundRBInfo(groundMass, groundMotionState, groundShape, localGroundInertia);
groundRBInfo.m_restitution = 1.1f;
groundRBInfo.m_friction = 1.0f;
btRigidBody *groundBody = new btRigidBody(groundRBInfo);
//add the body to the dynamics world
btDiscreteDynamicsWorld* dynamicsWorld = physicsEngine->getDynamicsWorld();
dynamicsWorld->addRigidBody(groundBody);
return groundBody;
}
void BasicDemo::initPhysics(){
//printing
debug_print_ = false;
//set up world
broadphase_ = new btDbvtBroadphase();
collisionConfiguration_ = new btDefaultCollisionConfiguration();
dispatcher_ = new btCollisionDispatcher(collisionConfiguration_);
solver_ = new btSequentialImpulseConstraintSolver;
dynamicsWorld_ = new btDiscreteDynamicsWorld(dispatcher_,broadphase_,solver_,collisionConfiguration_);
//set gravity. y is apparently up.
dynamicsWorld_->setGravity(btVector3(0,-10,0));
//--------------OBJECT CREATION SECTION-----------------//
//make the ground plane
groundCollisionShape_ = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
//collisionShapes_.push_back(groundCollisionShape_);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0.0f,-55.0f,0.0f));
btScalar groundMass(0.0);
//create the ground plane rigid body
groundRigidBody_ = createRigidBody(groundCollisionShape_,groundMass,groundTransform);
//add it to the dynamics world
dynamicsWorld_->addRigidBody(groundRigidBody_);
//Calculate the required origins based on startPose_ given by the runSimulation function
btVector3 bravoOrigin = startPose_;
btVector3 tangoOrigin = startPose_;
//tangoOrigin.setX(tangoOrigin.getX()+1.5);
//make bravo rigid body (the sliding box)
const btVector3 bravoBoxHalfExtents( 1.0f, 1.0f, 1.0f );
bravoCollisionShape_ = new btBoxShape(bravoBoxHalfExtents);
//collisionShapes_.push_back(bravoCollisionShape_);
btTransform bravoTransform;
bravoTransform.setIdentity();
//bravoTransform.setOrigin(btVector3(0.0f,1.0f,0.0f));
bravoTransform.setOrigin(bravoOrigin);
btScalar bravoMass = 1.0f;
//create the bravo rigid body
bravoRigidBody_ = createRigidBody(bravoCollisionShape_,bravoMass,bravoTransform);
//add it to the dynamics world
dynamicsWorld_->addRigidBody(bravoRigidBody_);
//make tango rigid body (the robots manipulator)
const btVector3 tangoBoxHalfExtents( 0.1f, 0.5f, 0.1f );
tangoCollisionShape_ = new btBoxShape(tangoBoxHalfExtents);
//collisionShapes_.push_back(tangoCollisionShape_);
btTransform tangoTransform;
tangoTransform.setIdentity();
//tangoTransform.setOrigin(btVector3(1.5f,1.0f,0.0f));
tangoTransform.setOrigin(tangoOrigin);
btScalar tangoMass = 0.1f;
//create the tango rigid body
tangoRigidBody_ = createRigidBody(tangoCollisionShape_,tangoMass,tangoTransform);
//add it to the dynamics world
dynamicsWorld_->addRigidBody(tangoRigidBody_);
//--------------END OBJECT CREATION SECTION-----------------//
//--------------CONSTRAINT CREATION SECTION-----------------//
// create a constraint
//const btVector3 pivotInA( 1.5f, 0.0f, 0.0f );
const btVector3 pivotInA( 0.0f, 0.0f, 0.0f );
const btVector3 pivotInB( 0.0f, 0.0f, 0.0f );
btVector3 axisInA( 0.0f, 1.0f, 0.0f );
btVector3 axisInB( 0.0f, 1.0f, 0.0f );
bool useReferenceFrameA = false;
hingeConstraint_ = new btHingeConstraint(*bravoRigidBody_,*tangoRigidBody_,pivotInA,pivotInB,axisInA,axisInB,useReferenceFrameA);
// set joint feedback
hingeConstraint_->setJointFeedback(&jfRobot_);
// add constraint to the world
const bool isDisableCollisionsBetweenLinkedBodies = true; //this used to be false
dynamicsWorld_->addConstraint(hingeConstraint_,
isDisableCollisionsBetweenLinkedBodies);
//--------------END CONSTRAINT CREATION SECTION-----------------//
//--------------INITIALIZE REMAINING PARAMETERS SECTION-----------------//
//set controller values
pGains_.setValue(5.0f,50.0f,5.0f);
dGains_.setValue(2.0f,2.0f,2.0f);
//initialize
tangoRigidBody_->getMotionState()->getWorldTransform(tangoBodyTrans_);
currentPose_ = tangoBodyTrans_.getOrigin();
desiredPose_ = currentPose_;
bravoRigidBody_->forceActivationState(4);
count_ = 0;
//--------------END INITIALIZE REMAINING PARAMETERS SECTION-----------------//
}