int main(int argc, char** argv)
{
GLDebugDrawer debugDrawer;
debugDrawer.setDebugMode(btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawWireframe);
CollideDetectionDemo demo;
demo.initPhysics();
demo.getDynamicsWorld()->setDebugDrawer(&debugDrawer);
return glutmain(argc, argv, 640, 480,"First Bullet Physics Demo. http://blog.csdn.net/vagrixe", &demo);
}
void RagdollDemo:: renderme()
{
extern GLDebugDrawer gDebugDrawer;
// Call the parent method.
GlutDemoApplication::renderme();
for(int i = 5; i<11; i++)
{
if(touches[i] == 1)
{
gDebugDrawer.drawSphere(touchPoints[i], 0.2, btVector3(1., 0., 0.));
}
}
}
void Physics::init() {
configuration = new btDefaultCollisionConfiguration();
dispatcher = new btCollisionDispatcher(configuration);
broadphase = new btDbvtBroadphase();
solver = new btSequentialImpulseConstraintSolver();
world = new btDiscreteDynamicsWorld(dispatcher, broadphase, solver, configuration);
debug_drawer.init();
world->setDebugDrawer(&debug_drawer);
world->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
world->setGravity(btVector3(0.0, -9.6f, 0.0f));
}
PhysWorld::PhysWorld(){
//Initialize all the things that bullet needs to work
broadphase = new btDbvtBroadphase();
conf = new btDefaultCollisionConfiguration();
dispatcher = new btCollisionDispatcher(conf);
btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher);
solver = new btSequentialImpulseConstraintSolver();
dynWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,conf);
dynWorld->setGravity(btVector3(0,-4.91,0));
debugDraw.setDebugMode(1);
dynWorld->setDebugDrawer(&debugDraw);
}
void Physics::draw_debug(Camera* camera) {
debug_drawer.set_camera(camera);
world->debugDrawWorld();
}
void RagdollDemo::clientMoveAndDisplay() {
#ifdef GRAPHICS
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
extern GLDebugDrawer gDebugDrawer;
#endif
if (m_dynamicsWorld) {
double delta_x = brain->get_output(0);
delta_x *= MOTOR_SCALE;
//printf("delta_x: %f\n", delta_x);
btVector3 agent_position = move_kinematic_body(agent, btVector3(delta_x, 0, 0));
agent->setActivationState(ACTIVE_TAG);
btVector3 target_a_position = move_kinematic_body(target_a, btVector3(target_a_dx, 0, target_a_dz));
btVector3 target_b_position = move_kinematic_body(target_b, btVector3(target_b_dx, 0, target_b_dz));
target_a->setActivationState(ACTIVE_TAG);
target_b->setActivationState(ACTIVE_TAG);
if (create_draw_file) {
draw_fh << agent_position.getX() << " " << agent_position.getZ() << " ";
draw_fh << target_a_position.getX() << " " << target_a_position.getZ() << " ";
draw_fh << target_b_position.getX() << " " << target_b_position.getZ() << std::endl;
}
for (int i = 0; i < NUM_RAYS; i++) {
double angle = (-RAY_ANGLE / 2) + (i * (RAY_ANGLE / (NUM_RAYS - 1)));
btVector3 ray_offset(RAY_LENGTH * sin(angle), 0, RAY_LENGTH * cos(angle));
btVector3 ray_end = agent_position - ray_offset;
int touch = raycast(agent_position, ray_end);
btVector3 ray = ray_end - agent_position;
double sensor = (1.0 - (ray.length() / RAY_LENGTH)) * 10;
ray_sensors[i] = sensor;
#ifdef GRAPHICS
btVector3 color = touch ? btVector3(0, .8, 0) : btVector3(0, 0, 0);
gDebugDrawer.drawLine(agent_position, ray_end, color);
#endif
}
if (!target_a_passed && (target_a_position.getZ() >= AGENT_Z)) {
target_a_passed = true;
target_a_distance = fabs(agent_position.getX() - target_a_position.getX());
}
if (!target_b_passed && (target_b_position.getZ() >= AGENT_Z)) {
target_b_passed = true;
target_b_distance = fabs(agent_position.getX() - target_b_position.getX());
}
if (target_a_passed && target_b_passed) {
double fitness = target_a_distance + target_b_distance;
std::ofstream fh;
fh.open(ERROR_FILENAME);
fh << fitness;
fh.close();
if (create_draw_file) {
draw_fh.close();
}
exit(0);
}
frame_count++;
m_dynamicsWorld->stepSimulation(TIME_STEP);
brain->step(.01);
#ifdef TEST_SPEED
printf("%d %f\n", frame_count, agent_position.getX());
#endif
//optional but useful: debug drawing
m_dynamicsWorld->debugDrawWorld();
} //world exists
#ifdef GRAPHICS
renderme();
glFlush();
glutSwapBuffers();
#endif
}
//to be implemented by the demo
void renderme()
{
debugDrawer.SetDebugMode(getDebugMode());
//render the hinge axis
if (createConstraint)
{
SimdVector3 color(1,0,0);
SimdVector3 dirLocal(0,1,0);
SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
SimdVector3 from = physObjects[1]->GetRigidBody()->getCenterOfMassTransform()(pivotInA);
SimdVector3 fromB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform()(pivotInB);
SimdVector3 dirWorldA = physObjects[1]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ;
SimdVector3 dirWorldB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ;
debugDrawer.DrawLine(from,from+dirWorldA,color);
debugDrawer.DrawLine(fromB,fromB+dirWorldB,color);
}
float m[16];
int i;
if (getDebugMode() & IDebugDraw::DBG_DisableBulletLCP)
{
//don't use Bullet, use quickstep
physicsEnvironmentPtr->setSolverType(0);
} else
{
//Bullet LCP solver
physicsEnvironmentPtr->setSolverType(1);
}
if (getDebugMode() & IDebugDraw::DBG_EnableCCD)
{
physicsEnvironmentPtr->setCcdMode(3);
} else
{
physicsEnvironmentPtr->setCcdMode(0);
}
bool isSatEnabled = (getDebugMode() & IDebugDraw::DBG_EnableSatComparison);
physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled);
#ifdef USE_HULL
//some testing code for SAT
if (isSatEnabled)
{
for (int s=0;s<numShapes;s++)
{
CollisionShape* shape = shapePtr[s];
if (shape->IsPolyhedral())
{
PolyhedralConvexShape* polyhedron = static_cast<PolyhedralConvexShape*>(shape);
if (!polyhedron->m_optionalHull)
{
//first convert vertices in 'Point3' format
int numPoints = polyhedron->GetNumVertices();
Point3* points = new Point3[numPoints+1];
//first 4 points should not be co-planar, so add central point to satisfy MakeHull
points[0] = Point3(0.f,0.f,0.f);
SimdVector3 vertex;
for (int p=0;p<numPoints;p++)
{
polyhedron->GetVertex(p,vertex);
points[p+1] = Point3(vertex.getX(),vertex.getY(),vertex.getZ());
}
Hull* hull = Hull::MakeHull(numPoints+1,points);
polyhedron->m_optionalHull = hull;
}
}
}
}
#endif //USE_HULL
for (i=0;i<numObjects;i++)
{
SimdTransform transA;
transA.setIdentity();
float pos[3];
float rot[4];
ms[i].getWorldPosition(pos[0],pos[1],pos[2]);
ms[i].getWorldOrientation(rot[0],rot[1],rot[2],rot[3]);
SimdQuaternion q(rot[0],rot[1],rot[2],rot[3]);
transA.setRotation(q);
SimdPoint3 dpos;
dpos.setValue(pos[0],pos[1],pos[2]);
transA.setOrigin( dpos );
transA.getOpenGLMatrix( m );
SimdVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation
if (i & 1)
{
wireColor = SimdVector3(0.f,0.0f,1.f);
}
///color differently for active, sleeping, wantsdeactivation states
if (physObjects[i]->GetRigidBody()->GetActivationState() == 1) //active
{
if (i & 1)
{
wireColor += SimdVector3 (1.f,0.f,0.f);
} else
{
wireColor += SimdVector3 (.5f,0.f,0.f);
}
}
if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING
{
if (i & 1)
{
wireColor += SimdVector3 (0.f,1.f, 0.f);
} else
{
wireColor += SimdVector3 (0.f,0.5f,0.f);
}
}
char extraDebug[125];
sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId);
physObjects[i]->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug);
GL_ShapeDrawer::DrawOpenGL(m,physObjects[i]->GetRigidBody()->GetCollisionShape(),wireColor,getDebugMode());
///this block is just experimental code to show some internal issues with replacing shapes on the fly.
if (getDebugMode()!=0 && (i>0))
{
if (physObjects[i]->GetRigidBody()->GetCollisionShape()->GetShapeType() == EMPTY_SHAPE_PROXYTYPE)
{
physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[1]);
//remove the persistent collision pairs that were created based on the previous shape
BroadphaseProxy* bpproxy = physObjects[i]->GetRigidBody()->m_broadphaseHandle;
physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy);
SimdVector3 newinertia;
SimdScalar newmass = 10.f;
physObjects[i]->GetRigidBody()->GetCollisionShape()->CalculateLocalInertia(newmass,newinertia);
physObjects[i]->GetRigidBody()->setMassProps(newmass,newinertia);
physObjects[i]->GetRigidBody()->updateInertiaTensor();
}
}
}
if (!(getDebugMode() & IDebugDraw::DBG_NoHelpText))
{
float xOffset = 10.f;
float yStart = 20.f;
float yIncr = -2.f;
char buf[124];
glColor3f(0, 0, 0);
#ifdef USE_QUICKPROF
if ( getDebugMode() & IDebugDraw::DBG_ProfileTimings)
{
static int counter = 0;
counter++;
std::map<std::string, hidden::ProfileBlock*>::iterator iter;
for (iter = Profiler::mProfileBlocks.begin(); iter != Profiler::mProfileBlocks.end(); ++iter)
{
char blockTime[128];
sprintf(blockTime, "%s: %lf",&((*iter).first[0]),Profiler::getBlockTime((*iter).first, Profiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT));
glRasterPos3f(xOffset,yStart,0);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),blockTime);
yStart += yIncr;
}
}
#endif //USE_QUICKPROF
//profiling << Profiler::createStatsString(Profiler::BLOCK_TOTAL_PERCENT);
//<< std::endl;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"mouse to interact");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"space to reset");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"cursor keys and z,x to navigate");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"i to toggle simulation, s single step");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"q to quit");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"d to toggle deactivation");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"a to draw temporal AABBs");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"h to toggle help text");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
bool useBulletLCP = !(getDebugMode() & IDebugDraw::DBG_DisableBulletLCP);
bool useCCD = (getDebugMode() & IDebugDraw::DBG_EnableCCD);
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"m Bullet GJK = %i",!isSatEnabled);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"n Bullet LCP = %i",useBulletLCP);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"+- shooting speed = %10.2f",bulletSpeed);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
}
}
void RaytestDemo::castRays()
{
static float up = 0.f;
static float dir = 1.f;
//add some simple animation
if (!m_idle)
{
up+=0.01*dir;
if (btFabs(up)>2)
{
dir*=-1.f;
}
btTransform tr = m_dynamicsWorld->getCollisionObjectArray()[1]->getWorldTransform();
static float angle = 0.f;
angle+=0.01f;
tr.setRotation(btQuaternion(btVector3(0,1,0),angle));
m_dynamicsWorld->getCollisionObjectArray()[1]->setWorldTransform(tr);
}
///step the simulation
if (m_dynamicsWorld)
{
m_dynamicsWorld->updateAabbs();
m_dynamicsWorld->computeOverlappingPairs();
btVector3 red(1,0,0);
btVector3 blue(0,0,1);
///all hits
{
btVector3 from(-30,1+up,0);
btVector3 to(30,1,0);
sDebugDraw.drawLine(from,to,btVector4(0,0,0,1));
btCollisionWorld::AllHitsRayResultCallback allResults(from,to);
allResults.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal;
//kF_UseGjkConvexRaytest flag is now enabled by default, use the faster but more approximate algorithm
allResults.m_flags |= btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest;
m_dynamicsWorld->rayTest(from,to,allResults);
for (int i=0;i<allResults.m_hitFractions.size();i++)
{
btVector3 p = from.lerp(to,allResults.m_hitFractions[i]);
sDebugDraw.drawSphere(p,0.1,red);
}
}
///first hit
{
btVector3 from(-30,1.2,0);
btVector3 to(30,1.2,0);
sDebugDraw.drawLine(from,to,btVector4(0,0,1,1));
btCollisionWorld::ClosestRayResultCallback closestResults(from,to);
closestResults.m_flags |= btTriangleRaycastCallback::kF_FilterBackfaces;
m_dynamicsWorld->rayTest(from,to,closestResults);
if (closestResults.hasHit())
{
btVector3 p = from.lerp(to,closestResults.m_closestHitFraction);
sDebugDraw.drawSphere(p,0.1,blue);
sDebugDraw.drawLine(p,p+closestResults.m_hitNormalWorld,blue);
}
}
}
}
