void createBulletObject(Shape shape)
{
switch (shape) {
case ConvexHull : {
btConvexHullShape* bcs = new btConvexHullShape();
for (size_t i=0; i<data.size(); i++) {
glm::vec3& v = data[i].vertex;
bcs->addPoint( glmvec3_to_btVector3(v) );
}
btScalar mass(ball_mass);
btVector3 localInertia(0,0,0);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
// position and motion
btTransform ballTransform;
ballTransform.setIdentity();
ballTransform.setOrigin(btVector3(0,ball_initial_height,0));
btDefaultMotionState* bulletMotionState = new btDefaultMotionState(btTransform(ballTransform));
// ball rigidbody info
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,bulletMotionState,bcs,localInertia);
// tweak rigidbody info
rbInfo.m_restitution = grestitution;
rbInfo.m_friction = gfriction;
// add ball to the world
rigidbody = new btRigidBody(rbInfo);
if(!rigidbody) std::cout << "bulletBallBody pointer null" << std::endl;
dynamicsWorld->addRigidBody(rigidbody);
break;
}
case TriangleMesh : {
// Shape
btBvhTriangleMeshShape* boardShape = new btBvhTriangleMeshShape( &bt_triangles, true );
btScalar mass(board_mass);
btVector3 localInertia(0,0,0);
// transform : default position
btTransform boardTransform;
boardTransform.setIdentity();
boardTransform.setOrigin(btVector3(0,0,0));
btDefaultMotionState* bulletMotionState = new btDefaultMotionState(btTransform(boardTransform));
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,bulletMotionState,boardShape,localInertia);
// tweak rigidbody properties
rbInfo.m_restitution = grestitution;
rbInfo.m_friction = gfriction;
// add to the world
rigidbody = new btRigidBody(rbInfo);
if(!rigidbody) std::cout << "bulletBoardBody pointer null" << std::endl;
rigidbody->setActivationState(DISABLE_DEACTIVATION);
dynamicsWorld->addRigidBody(rigidbody);
break;
}
case None : {
break;
}
} // end switch
}
void addPickingConstraint(const btVector3& rayFrom, const btVector3& rayTo) {
if (!dynamicsWorld) {
return;
}
removePickingConstraint();
if (pickedObjectIndex <= 0 || pickedObjectIndex >= dynamicsWorld->getNumCollisionObjects()) {
return;
}
pickedBody = btRigidBody::upcast(dynamicsWorld->getCollisionObjectArray()[pickedObjectIndex]);
btVector3 pickPos = rayTo;
btVector3 localPivot = pickedBody->getCenterOfMassTransform().inverse() * pickPos;
pickConstraint = new btPoint2PointConstraint(*pickedBody,localPivot);
pickedBody->setActivationState(DISABLE_DEACTIVATION);
dynamicsWorld->addConstraint(pickConstraint,true);
pickingDistance = (rayFrom-rayTo).length();
pickConstraint->m_setting.m_impulseClamp = 3.0f;
pickConstraint->m_setting.m_tau = 0.001f;
}
bool mouseButtonCallback(int button, int state, float x, float y)
{
if (state==1)
{
if(button==0)// && (m_data->m_altPressed==0 && m_data->m_controlPressed==0))
{
btVector3 camPos;
m_glApp->m_instancingRenderer->getCameraPosition(camPos);
btVector3 rayFrom = camPos;
btVector3 rayTo = getRayTo(x,y);
btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
if (rayCallback.hasHit())
{
btVector3 pickPos = rayCallback.m_hitPointWorld;
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
if (body)
{
//other exclusions?
if (!(body->isStaticObject() || body->isKinematicObject()))
{
m_pickedBody = body;
m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot);
m_dynamicsWorld->addConstraint(p2p,true);
m_pickedConstraint = p2p;
btScalar mousePickClamping = 30.f;
p2p->m_setting.m_impulseClamp = mousePickClamping;
//very weak constraint for picking
p2p->m_setting.m_tau = 0.001f;
}
} else
{
btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
if (multiCol && multiCol->m_multiBody)
{
multiCol->m_multiBody->setCanSleep(false);
btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos);
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos);
//if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
p2p->setMaxAppliedImpulse(20*scaling);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
world->addMultiBodyConstraint(p2p);
m_pickingMultiBodyPoint2Point =p2p;
}
}
// pickObject(pickPos, rayCallback.m_collisionObject);
m_oldPickingPos = rayTo;
m_hitPos = pickPos;
m_oldPickingDist = (pickPos-rayFrom).length();
// printf("hit !\n");
//add p2p
}
}
} else
{
if (button==0)
{
if (m_pickedConstraint)
{
m_dynamicsWorld->removeConstraint(m_pickedConstraint);
delete m_pickedConstraint;
m_pickedConstraint=0;
m_pickedBody = 0;
}
if (m_pickingMultiBodyPoint2Point)
{
m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(true);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point);
delete m_pickingMultiBodyPoint2Point;
m_pickingMultiBodyPoint2Point = 0;
}
//remove p2p
}
}
//printf("button=%d, state=%d\n",button,state);
return false;
}
void ForkLiftDemo::initPhysics()
{
int upAxis = 1;
m_guiHelper->setUpAxis(upAxis);
btVector3 groundExtents(50,50,50);
groundExtents[upAxis]=3;
btCollisionShape* groundShape = new btBoxShape(groundExtents);
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_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
if (useMCLPSolver)
{
btDantzigSolver* mlcp = new btDantzigSolver();
//btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel;
btMLCPSolver* sol = new btMLCPSolver(mlcp);
m_constraintSolver = sol;
} else
{
m_constraintSolver = new btSequentialImpulseConstraintSolver();
}
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,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_globalCfm = 0.00001;
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));
m_carChassis = localCreateRigidBody(800,tr,compound);//chassisShape);
//m_carChassis->setDamping(0.2,0.2);
m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth,wheelRadius,wheelRadius));
m_guiHelper->createCollisionShapeGraphicsObject(m_wheelShape);
int wheelGraphicsIndex = m_wheelShape->getUserIndex();
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};
for (int i=0;i<4;i++)
{
m_wheelInstances[i] = m_guiHelper->registerGraphicsInstance(wheelGraphicsIndex, position, quaternion, color, scaling);
}
{
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);
}
/// create vehicle
{
m_vehicleRayCaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
m_vehicle = new btRaycastVehicle(m_tuning,m_carChassis,m_vehicleRayCaster);
///never deactivate the vehicle
m_carChassis->setActivationState(DISABLE_DEACTIVATION);
m_dynamicsWorld->addVehicle(m_vehicle);
float connectionHeight = 1.2f;
bool isFrontWheel=true;
//choose coordinate system
m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);
btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius);
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius);
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius);
isFrontWheel = false;
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius);
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
for (int i=0;i<m_vehicle->getNumWheels();i++)
{
btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
wheel.m_suspensionStiffness = suspensionStiffness;
wheel.m_wheelsDampingRelaxation = suspensionDamping;
wheel.m_wheelsDampingCompression = suspensionCompression;
wheel.m_frictionSlip = wheelFriction;
wheel.m_rollInfluence = rollInfluence;
}
}
resetForklift();
// setCameraDistance(26.f);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
