void AddBox(const btTransform& T, float mass) {
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
boxShape->calculateLocalInertia(mass,localInertia);
btDefaultMotionState* myMotionState = new btDefaultMotionState(T);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,boxShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
dynamicsWorld->addRigidBody(body);
}
void AddGroundPlane() {
btTransform groundTransform;
groundTransform.setIdentity();
btScalar mass = 0.0f;
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
dynamicsWorld->addRigidBody(body);
}
void DynamicsWorld::GetAllEntitiesWithinBroadphase(btCollisionShape& shape, btTransform& t, std::vector<Entity*> &entities)
{
// Get a list of entities that could be intersecting with the shape object using bullet's broadphase test
btVector3 min, max;
shape.getAabb(t, min, max);
btAlignedObjectArray < btCollisionObject* > collisionObjectArray;
BroadphaseAabbCallback callback(collisionObjectArray);
dynamicsWorld->getBroadphase()->aabbTest(min, max, callback);
for (int i = 0; i < collisionObjectArray.size(); i++)
{
entities.push_back(BodyMap[collisionObjectArray[i]]->Entity);
}
}
btScalar getObjectMaximumGravityTorqueLength(const btCollisionShape &object_shape)
{
// Calculate the maximum acceleration that can happen for a particular object under gravity
std::vector<btVector3> object_corner_point_list;
object_corner_point_list.reserve(150);
// get all points in the compound object
if (object_shape.isCompound())
{
const btCompoundShape * object_compound_shape = (btCompoundShape *)&object_shape;
for (int i = 0; i < object_compound_shape->getNumChildShapes(); i++)
{
const btCollisionShape * child_shape = object_compound_shape->getChildShape(i);
const btTransform child_transform = object_compound_shape->getChildTransform(i);
if (child_shape->isConvex())
{
const btConvexHullShape * child_hull_shape = (btConvexHullShape *)child_shape;
for (int pts = 0; pts < child_hull_shape->getNumPoints(); pts++)
{
btVector3 corner_points = child_hull_shape->getScaledPoint(pts);
object_corner_point_list.push_back(child_transform * corner_points);
}
}
}
}
// get the farthest distance from the object center of gravity
// const btVector3 cog = object.getCenterOfMassPosition();
const btVector3 cog(0.,0.,0.);
btScalar max_dist_squared = 0;
for (std::vector<btVector3>::iterator it = object_corner_point_list.begin(); it!= object_corner_point_list.end(); ++it)
{
btScalar point_distance_to_cog = cog.distance2(*it);
max_dist_squared = (point_distance_to_cog > max_dist_squared) ? point_distance_to_cog : max_dist_squared;
}
return sqrt(max_dist_squared);
}
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);
}
void createBulletSim(void) {
// EN:: collision configuration contains default setup for memory, collision setup.
// EN:: Advanced users can create their own configuration.
// BR:: configuração de colisão contem configurações padrão da memória.
// BR:: usuários avançados podem criar suas próprias configurações.
collisionConfiguration = new btDefaultCollisionConfiguration();
// EN:: use the default collision dispatcher. For parallel processing
// EN:: you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
// BR:: use o dispatcher padrão. para processamento paralelo
// BR:: você pode usar um dispatcher diferente. (ver Doc)
dispatcher = new btCollisionDispatcher(collisionConfiguration);
// EN:: btDbvtBroadphase is a good general purpose broadphase.
// EN:: You can also try out btAxis3Sweep.
// BR:: btDbvtBroadphase é um bom broadphase de propósito geral.
// BR:: Você pode tentar também btAxis3Sweep.
overlappingPairCache = new btDbvtBroadphase();
// EN:: the default constraint solver. For parallel processing
// EN:: you can use a different solver (see Extras/BulletMultiThreaded)
// BR:: usa a constraint solver padrão. Para processamento paralelo
// BR:: você pode ver um solver diferente (ver Doc)
solver = new btSequentialImpulseConstraintSolver;
dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver,collisionConfiguration);
dynamicsWorld->setGravity(btVector3(0,-10,0));
// EN:: create a few basic rigid bodies
// EN:: start with ground plane, 1500, 1500
// BR:: cria alguns corpos rígidos básicos
// BR:: inicializa com um chão plano.
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(1500.),btScalar(1.),btScalar(1500.)));
collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-2,0));
{
btScalar mass(0.);
// EN:: rigidbody is dynamic if and only if mass is non zero, otherwise static
// BR:: corpo rigido é dimâmico apenas se massa for diferente de 0.
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
// EN:: add the body to the dynamics world
// BR:: adiciona o corpo às dinâmicas do mundo
dynamicsWorld->addRigidBody(body);
}
{
// EN:: create a dynamic rigidbody
// BR:: cria um corpo rígido dinâmico
btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
collisionShapes.push_back(colShape);
// EN:: Create Dynamic Objects
// BR:: Cria objetos dinâmicos
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
// EN:: rigidbody is dynamic if and only if mass is non zero, otherwise static
// BR:: corpo rigido é dimâmico apenas se massa for diferente de 0.
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,-1.0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
startTransform.setOrigin(btVector3(0,250,0));
// *** give it a slight twist so it bouncees more interesting
startTransform.setRotation(btQuaternion(btVector3(1.0, 1.0, 0.0), 0.6));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
//btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
MyMotionState* motionState = new MyMotionState(startTransform, boxNode);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,motionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
dynamicsWorld->addRigidBody(body);
}
}
static inline void setConId(btCollisionShape & shape, int id)
{
shape.setUserPointer(cast<void*>(id + 1));
}
static inline int getConId(const btCollisionShape & shape)
{
return cast<int>(shape.getUserPointer()) - 1;
}
void createBulletSim(void) {
///collision configuration contains default setup for memory, collision setup. Advanced users can create their own configuration.
collisionConfiguration = new btDefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
dispatcher = new btCollisionDispatcher(collisionConfiguration);
///btDbvtBroadphase is a good general purpose broadphase. You can also try out btAxis3Sweep.
overlappingPairCache = new btDbvtBroadphase();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
solver = new btSequentialImpulseConstraintSolver;
dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver,collisionConfiguration);
dynamicsWorld->setGravity(btVector3(0,-10,0));
///create a few basic rigid bodies
// start with ground plane, 1500, 1500
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(1500.),btScalar(1.),btScalar(1500.)));
collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-2,0));
{
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);
// lathe - this plane isnt going to be moving so i dont care about setting the motion state
//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
dynamicsWorld->addRigidBody(body);
}
{
//create a dynamic rigidbody
btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
// btCollisionShape* colShape = new btSphereShape(btScalar(1.));
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,-1.0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
startTransform.setOrigin(btVector3(0,250,0));
// *** give it a slight twist so it bouncees more interesting
startTransform.setRotation(btQuaternion(btVector3(1.0, 1.0, 0.0), 0.6));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
//btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
MyMotionState* motionState = new MyMotionState(startTransform, boxNode);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,motionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
dynamicsWorld->addRigidBody(body);
}
}
