Demos/ForkLiftDemo/ForkLiftDemo.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 ///September 2006: VehicleDemo is work in progress, this file is mostly just a placeholder
  17 ///This VehicleDemo file is very early in development, please check it later
  18 ///@todo is a basic engine model:
  19 ///A function that maps user input (throttle) into torque/force applied on the wheels
  20 ///with gears etc.
  21 #include "btBulletDynamicsCommon.h"
  22 #include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
  23 #include "GLDebugFont.h"
  24
  25 #ifndef M_PI
  26 #define M_PI       3.14159265358979323846
  27 #endif
  28
  29 #ifndef M_PI_2
  30 #define M_PI_2     1.57079632679489661923
  31 #endif
  32
  33 #ifndef M_PI_4
  34 #define M_PI_4     0.785398163397448309616
  35 #endif
  36
  37 //#define LIFT_EPS 0.0000001f
  38 //
  39 // By default, Bullet Vehicle uses Y as up axis.
  40 // You can override the up axis, for example Z-axis up. Enable this define to see how to:
  41 //#define FORCE_ZAXIS_UP 1
  42 //
  43
  44 #ifdef FORCE_ZAXIS_UP
  45                 int rightIndex = 0;
  46                 int upIndex = 2;
  47                 int forwardIndex = 1;
  48                 btVector3 wheelDirectionCS0(0,0,-1);
  49                 btVector3 wheelAxleCS(1,0,0);
  50 #else
  51                 int rightIndex = 0;
  52                 int upIndex = 1;
  53                 int forwardIndex = 2;
  54                 btVector3 wheelDirectionCS0(0,-1,0);
  55                 btVector3 wheelAxleCS(-1,0,0);
  56 #endif
  57
  58 #include "GLDebugDrawer.h"
  59 #include <stdio.h> //printf debugging
  60
  61 #include "GL_ShapeDrawer.h"
  62
  63 #include "GlutStuff.h"
  64 #include "ForkLiftDemo.h"
  65
  66
  67 const int maxProxies = 32766;
  68 const int maxOverlap = 65535;
  69
  70 ///btRaycastVehicle is the interface for the constraint that implements the raycast vehicle
  71 ///notice that for higher-quality slow-moving vehicles, another approach might be better
  72 ///implementing explicit hinged-wheel constraints with cylinder collision, rather then raycasts
  73 float   gEngineForce = 0.f;
  74
  75 float   defaultBreakingForce = 10.f;
  76 float   gBreakingForce = 100.f;
  77
  78 float   maxEngineForce = 1000.f;//this should be engine/velocity dependent
  79 float   maxBreakingForce = 100.f;
  80
  81 float   gVehicleSteering = 0.f;
  82 float   steeringIncrement = 0.04f;
  83 float   steeringClamp = 0.3f;
  84 float   wheelRadius = 0.5f;
  85 float   wheelWidth = 0.4f;
  86 float   wheelFriction = 1000;//BT_LARGE_FLOAT;
  87 float   suspensionStiffness = 20.f;
  88 float   suspensionDamping = 2.3f;
  89 float   suspensionCompression = 4.4f;
  90 float   rollInfluence = 0.1f;//1.0f;
  91
  92
  93 btScalar suspensionRestLength(0.6);
  94
  95 #define CUBE_HALF_EXTENTS 1
  96
  97
  98
  99 ////////////////////////////////////
 100
 101
 102
 103
 104 ForkLiftDemo::ForkLiftDemo()
 105 :
 106 m_carChassis(0),
 107 m_liftBody(0),
 108 m_forkBody(0),
 109 m_loadBody(0),
 110 m_indexVertexArrays(0),
 111 m_vertices(0),
 112 m_cameraHeight(4.f),
 113 m_minCameraDistance(3.f),
 114 m_maxCameraDistance(10.f)
 115 {
 116         m_vehicle = 0;
 117         m_wheelShape = 0;
 118         m_cameraPosition = btVector3(30,30,30);
 119         m_useDefaultCamera = false;
 120         setTexturing(true);
 121         setShadows(true);
 122
 123 }
 124
 125
 126 void ForkLiftDemo::termPhysics()
 127 {
 128                 //cleanup in the reverse order of creation/initialization
 129
 130         //remove the rigidbodies from the dynamics world and delete them
 131         int i;
 132         for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
 133         {
 134                 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
 135                 btRigidBody* body = btRigidBody::upcast(obj);
 136                 if (body && body->getMotionState())
 137                 {
 138
 139                         while (body->getNumConstraintRefs())
 140                         {
 141                                 btTypedConstraint* constraint = body->getConstraintRef(0);
 142                                 m_dynamicsWorld->removeConstraint(constraint);
 143                                 delete constraint;
 144                         }
 145                         delete body->getMotionState();
 146                         m_dynamicsWorld->removeRigidBody(body);
 147                 } else
 148                 {
 149                         m_dynamicsWorld->removeCollisionObject( obj );
 150                 }
 151                 delete obj;
 152         }
 153
 154         //delete collision shapes
 155         for (int j=0;j<m_collisionShapes.size();j++)
 156         {
 157                 btCollisionShape* shape = m_collisionShapes[j];
 158                 delete shape;
 159         }
 160
 161         delete m_indexVertexArrays;
 162         delete m_vertices;
 163
 164         //delete dynamics world
 165         delete m_dynamicsWorld;
 166
 167         delete m_vehicleRayCaster;
 168
 169         delete m_vehicle;
 170
 171         delete m_wheelShape;
 172
 173         //delete solver
 174         delete m_constraintSolver;
 175
 176         //delete broadphase
 177         delete m_overlappingPairCache;
 178
 179         //delete dispatcher
 180         delete m_dispatcher;
 181
 182         delete m_collisionConfiguration;
 183
 184 }
 185
 186 ForkLiftDemo::~ForkLiftDemo()
 187 {
 188         termPhysics();
 189 }
 190
 191 void ForkLiftDemo::initPhysics()
 192 {
 193
 194 #ifdef FORCE_ZAXIS_UP
 195         m_cameraUp = btVector3(0,0,1);
 196         m_forwardAxis = 1;
 197 #endif
 198
 199         btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
 200         m_collisionShapes.push_back(groundShape);
 201         m_collisionConfiguration = new btDefaultCollisionConfiguration();
 202         m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
 203         btVector3 worldMin(-1000,-1000,-1000);
 204         btVector3 worldMax(1000,1000,1000);
 205         m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
 206         m_constraintSolver = new btSequentialImpulseConstraintSolver();
 207         m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
 208 #ifdef FORCE_ZAXIS_UP
 209         m_dynamicsWorld->setGravity(btVector3(0,0,-10));
 210 #endif
 211
 212         //m_dynamicsWorld->setGravity(btVector3(0,0,0));
 213 btTransform tr;
 214 tr.setIdentity();
 215 tr.setOrigin(btVector3(0,-10,0));
 216
 217 //either use heightfield or triangle mesh
 218
 219
 220         //create ground object
 221         localCreateRigidBody(0,tr,groundShape);
 222
 223 #ifdef FORCE_ZAXIS_UP
 224 //   indexRightAxis = 0;
 225 //   indexUpAxis = 2;
 226 //   indexForwardAxis = 1;
 227         btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,2.f, 0.5f));
 228         btCompoundShape* compound = new btCompoundShape();
 229         btTransform localTrans;
 230         localTrans.setIdentity();
 231         //localTrans effectively shifts the center of mass with respect to the chassis
 232         localTrans.setOrigin(btVector3(0,0,1));
 233 #else
 234         btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,0.5f,2.f));
 235         m_collisionShapes.push_back(chassisShape);
 236
 237         btCompoundShape* compound = new btCompoundShape();
 238         m_collisionShapes.push_back(compound);
 239         btTransform localTrans;
 240         localTrans.setIdentity();
 241         //localTrans effectively shifts the center of mass with respect to the chassis
 242         localTrans.setOrigin(btVector3(0,1,0));
 243 #endif
 244
 245         compound->addChildShape(localTrans,chassisShape);
 246
 247         {
 248                 btCollisionShape* suppShape = new btBoxShape(btVector3(0.5f,0.1f,0.5f));
 249                 btTransform suppLocalTrans;
 250                 suppLocalTrans.setIdentity();
 251                 //localTrans effectively shifts the center of mass with respect to the chassis
 252                 suppLocalTrans.setOrigin(btVector3(0,1.0,2.5));
 253                 compound->addChildShape(suppLocalTrans, suppShape);
 254         }
 255
 256         tr.setOrigin(btVector3(0,0.f,0));
 257
 258         m_carChassis = localCreateRigidBody(800,tr,compound);//chassisShape);
 259         //m_carChassis->setDamping(0.2,0.2);
 260
 261         m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth,wheelRadius,wheelRadius));
 262
 263         {
 264                 btCollisionShape* liftShape = new btBoxShape(btVector3(0.5f,2.0f,0.05f));
 265                 m_collisionShapes.push_back(liftShape);
 266                 btTransform liftTrans;
 267                 m_liftStartPos = btVector3(0.0f, 2.5f, 3.05f);
 268                 liftTrans.setIdentity();
 269                 liftTrans.setOrigin(m_liftStartPos);
 270                 m_liftBody = localCreateRigidBody(10,liftTrans, liftShape);
 271
 272                 btTransform localA, localB;
 273                 localA.setIdentity();
 274                 localB.setIdentity();
 275                 localA.getBasis().setEulerZYX(0, M_PI_2, 0);
 276                 localA.setOrigin(btVector3(0.0, 1.0, 3.05));
 277                 localB.getBasis().setEulerZYX(0, M_PI_2, 0);
 278                 localB.setOrigin(btVector3(0.0, -1.5, -0.05));
 279                 m_liftHinge = new btHingeConstraint(*m_carChassis,*m_liftBody, localA, localB);
 280 //              m_liftHinge->setLimit(-LIFT_EPS, LIFT_EPS);
 281                 m_liftHinge->setLimit(0.0f, 0.0f);
 282                 m_dynamicsWorld->addConstraint(m_liftHinge, true);
 283
 284                 btCollisionShape* forkShapeA = new btBoxShape(btVector3(1.0f,0.1f,0.1f));
 285                 m_collisionShapes.push_back(forkShapeA);
 286                 btCompoundShape* forkCompound = new btCompoundShape();
 287                 m_collisionShapes.push_back(forkCompound);
 288                 btTransform forkLocalTrans;
 289                 forkLocalTrans.setIdentity();
 290                 forkCompound->addChildShape(forkLocalTrans, forkShapeA);
 291
 292                 btCollisionShape* forkShapeB = new btBoxShape(btVector3(0.1f,0.02f,0.6f));
 293                 m_collisionShapes.push_back(forkShapeB);
 294                 forkLocalTrans.setIdentity();
 295                 forkLocalTrans.setOrigin(btVector3(-0.9f, -0.08f, 0.7f));
 296                 forkCompound->addChildShape(forkLocalTrans, forkShapeB);
 297
 298                 btCollisionShape* forkShapeC = new btBoxShape(btVector3(0.1f,0.02f,0.6f));
 299                 m_collisionShapes.push_back(forkShapeC);
 300                 forkLocalTrans.setIdentity();
 301                 forkLocalTrans.setOrigin(btVector3(0.9f, -0.08f, 0.7f));
 302                 forkCompound->addChildShape(forkLocalTrans, forkShapeC);
 303
 304                 btTransform forkTrans;
 305                 m_forkStartPos = btVector3(0.0f, 0.6f, 3.2f);
 306                 forkTrans.setIdentity();
 307                 forkTrans.setOrigin(m_forkStartPos);
 308                 m_forkBody = localCreateRigidBody(5, forkTrans, forkCompound);
 309
 310                 localA.setIdentity();
 311                 localB.setIdentity();
 312                 localA.getBasis().setEulerZYX(0, 0, M_PI_2);
 313                 localA.setOrigin(btVector3(0.0f, -1.9f, 0.05f));
 314                 localB.getBasis().setEulerZYX(0, 0, M_PI_2);
 315                 localB.setOrigin(btVector3(0.0, 0.0, -0.1));
 316                 m_forkSlider = new btSliderConstraint(*m_liftBody, *m_forkBody, localA, localB, true);
 317                 m_forkSlider->setLowerLinLimit(0.1f);
 318                 m_forkSlider->setUpperLinLimit(0.1f);
 319 //              m_forkSlider->setLowerAngLimit(-LIFT_EPS);
 320 //              m_forkSlider->setUpperAngLimit(LIFT_EPS);
 321                 m_forkSlider->setLowerAngLimit(0.0f);
 322                 m_forkSlider->setUpperAngLimit(0.0f);
 323                 m_dynamicsWorld->addConstraint(m_forkSlider, true);
 324
 325
 326                 btCompoundShape* loadCompound = new btCompoundShape();
 327                 m_collisionShapes.push_back(loadCompound);
 328                 btCollisionShape* loadShapeA = new btBoxShape(btVector3(2.0f,0.5f,0.5f));
 329                 m_collisionShapes.push_back(loadShapeA);
 330                 btTransform loadTrans;
 331                 loadTrans.setIdentity();
 332                 loadCompound->addChildShape(loadTrans, loadShapeA);
 333                 btCollisionShape* loadShapeB = new btBoxShape(btVector3(0.1f,1.0f,1.0f));
 334                 m_collisionShapes.push_back(loadShapeB);
 335                 loadTrans.setIdentity();
 336                 loadTrans.setOrigin(btVector3(2.1f, 0.0f, 0.0f));
 337                 loadCompound->addChildShape(loadTrans, loadShapeB);
 338                 btCollisionShape* loadShapeC = new btBoxShape(btVector3(0.1f,1.0f,1.0f));
 339                 m_collisionShapes.push_back(loadShapeC);
 340                 loadTrans.setIdentity();
 341                 loadTrans.setOrigin(btVector3(-2.1f, 0.0f, 0.0f));
 342                 loadCompound->addChildShape(loadTrans, loadShapeC);
 343                 loadTrans.setIdentity();
 344                 m_loadStartPos = btVector3(0.0f, -3.5f, 7.0f);
 345                 loadTrans.setOrigin(m_loadStartPos);
 346                 m_loadBody  = localCreateRigidBody(4, loadTrans, loadCompound);
 347         }
 348
 349
 350
 351         clientResetScene();
 352
 353         /// create vehicle
 354         {
 355
 356                 m_vehicleRayCaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
 357                 m_vehicle = new btRaycastVehicle(m_tuning,m_carChassis,m_vehicleRayCaster);
 358
 359                 ///never deactivate the vehicle
 360                 m_carChassis->setActivationState(DISABLE_DEACTIVATION);
 361
 362                 m_dynamicsWorld->addVehicle(m_vehicle);
 363
 364                 float connectionHeight = 1.2f;
 365
 366
 367                 bool isFrontWheel=true;
 368
 369                 //choose coordinate system
 370                 m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);
 371
 372 #ifdef FORCE_ZAXIS_UP
 373                 btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),2*CUBE_HALF_EXTENTS-wheelRadius, connectionHeight);
 374 #else
 375                 btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius);
 376 #endif
 377
 378                 m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
 379 #ifdef FORCE_ZAXIS_UP
 380                 connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),2*CUBE_HALF_EXTENTS-wheelRadius, connectionHeight);
 381 #else
 382                 connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius);
 383 #endif
 384
 385                 m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
 386 #ifdef FORCE_ZAXIS_UP
 387                 connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),-2*CUBE_HALF_EXTENTS+wheelRadius, connectionHeight);
 388 #else
 389                 connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius);
 390 #endif //FORCE_ZAXIS_UP
 391                 isFrontWheel = false;
 392                 m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
 393 #ifdef FORCE_ZAXIS_UP
 394                 connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),-2*CUBE_HALF_EXTENTS+wheelRadius, connectionHeight);
 395 #else
 396                 connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius);
 397 #endif
 398                 m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
 399
 400                 for (int i=0;i<m_vehicle->getNumWheels();i++)
 401                 {
 402                         btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
 403                         wheel.m_suspensionStiffness = suspensionStiffness;
 404                         wheel.m_wheelsDampingRelaxation = suspensionDamping;
 405                         wheel.m_wheelsDampingCompression = suspensionCompression;
 406                         wheel.m_frictionSlip = wheelFriction;
 407                         wheel.m_rollInfluence = rollInfluence;
 408                 }
 409         }
 410
 411
 412         setCameraDistance(26.f);
 413
 414 }
 415
 416
 417 //to be implemented by the demo
 418 void ForkLiftDemo::renderme()
 419 {
 420
 421         updateCamera();
 422
 423         ATTRIBUTE_ALIGNED16(btScalar) m[16];
 424         int i;
 425
 426         btVector3 wheelColor(1,0,0);
 427
 428         btVector3       worldBoundsMin,worldBoundsMax;
 429         getDynamicsWorld()->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);
 430
 431
 432
 433         for (i=0;i<m_vehicle->getNumWheels();i++)
 434         {
 435                 //synchronize the wheels with the (interpolated) chassis worldtransform
 436                 m_vehicle->updateWheelTransform(i,true);
 437                 //draw wheels (cylinders)
 438                 m_vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(m);
 439                 m_shapeDrawer->drawOpenGL(m,m_wheelShape,wheelColor,getDebugMode(),worldBoundsMin,worldBoundsMax);
 440         }
 441
 442
 443         int lineWidth=250;
 444         int xStart = m_glutScreenWidth - lineWidth;
 445         int yStart = 20;
 446
 447         if((getDebugMode() & btIDebugDraw::DBG_NoHelpText)==0)
 448         {
 449                 setOrthographicProjection();
 450                 glDisable(GL_LIGHTING);
 451                 glColor3f(0, 0, 0);
 452                 char buf[124];
 453
 454                 glRasterPos3f(xStart, yStart, 0);
 455                 sprintf(buf,"SHIFT+Cursor Left/Right - rotate lift");
 456                 GLDebugDrawString(xStart,20,buf);
 457                 yStart+=20;
 458                 glRasterPos3f(xStart, yStart, 0);
 459                 sprintf(buf,"SHIFT+Cursor UP/Down - move fork up/down");
 460                 yStart+=20;
 461                 GLDebugDrawString(xStart,yStart,buf);
 462                 glRasterPos3f(xStart, yStart, 0);
 463                 sprintf(buf,"F5 - toggle camera mode");
 464                 yStart+=20;
 465                 GLDebugDrawString(xStart,yStart,buf);
 466                 glRasterPos3f(xStart, yStart, 0);
 467         sprintf(buf,"Click inside this window for keyboard focus");
 468                 yStart+=20;
 469                 GLDebugDrawString(xStart,yStart,buf);
 470
 471
 472                 resetPerspectiveProjection();
 473                 glEnable(GL_LIGHTING);
 474         }
 475         DemoApplication::renderme();
 476 }
 477
 478 void ForkLiftDemo::clientMoveAndDisplay()
 479 {
 480
 481         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 482
 483
 484         {
 485                 int wheelIndex = 2;
 486                 m_vehicle->applyEngineForce(gEngineForce,wheelIndex);
 487                 m_vehicle->setBrake(gBreakingForce,wheelIndex);
 488                 wheelIndex = 3;
 489                 m_vehicle->applyEngineForce(gEngineForce,wheelIndex);
 490                 m_vehicle->setBrake(gBreakingForce,wheelIndex);
 491
 492
 493                 wheelIndex = 0;
 494                 m_vehicle->setSteeringValue(gVehicleSteering,wheelIndex);
 495                 wheelIndex = 1;
 496                 m_vehicle->setSteeringValue(gVehicleSteering,wheelIndex);
 497
 498         }
 499
 500
 501         float dt = getDeltaTimeMicroseconds() * 0.000001f;
 502
 503         if (m_dynamicsWorld)
 504         {
 505                 //during idle mode, just run 1 simulation step maximum
 506                 int maxSimSubSteps = m_idle ? 1 : 2;
 507                 if (m_idle)
 508                         dt = 1.0/420.f;
 509
 510                 int numSimSteps;
 511         numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
 512
 513
 514 //#define VERBOSE_FEEDBACK
 515 #ifdef VERBOSE_FEEDBACK
 516                                 if (!numSimSteps)
 517                         printf("Interpolated transforms\n");
 518                 else
 519                 {
 520                         if (numSimSteps > maxSimSubSteps)
 521                         {
 522                                 //detect dropping frames
 523                                 printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps);
 524                         } else
 525                         {
 526                                 printf("Simulated (%i) steps\n",numSimSteps);
 527                         }
 528                 }
 529 #endif //VERBOSE_FEEDBACK
 530
 531         }
 532
 533
 534
 535
 536
 537
 538
 539 #ifdef USE_QUICKPROF
 540         btProfiler::beginBlock("render");
 541 #endif //USE_QUICKPROF
 542
 543
 544         renderme();
 545
 546         //optional but useful: debug drawing
 547         if (m_dynamicsWorld)
 548                 m_dynamicsWorld->debugDrawWorld();
 549
 550 #ifdef USE_QUICKPROF
 551         btProfiler::endBlock("render");
 552 #endif
 553
 554
 555         glFlush();
 556         glutSwapBuffers();
 557
 558 }
 559
 560
 561
 562 void ForkLiftDemo::displayCallback(void)
 563 {
 564         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 565
 566         renderme();
 567
 568 //optional but useful: debug drawing
 569         if (m_dynamicsWorld)
 570                 m_dynamicsWorld->debugDrawWorld();
 571
 572         glFlush();
 573         glutSwapBuffers();
 574 }
 575
 576
 577
 578 void ForkLiftDemo::clientResetScene()
 579 {
 580         gVehicleSteering = 0.f;
 581         gBreakingForce = defaultBreakingForce;
 582         gEngineForce = 0.f;
 583
 584         m_carChassis->setCenterOfMassTransform(btTransform::getIdentity());
 585         m_carChassis->setLinearVelocity(btVector3(0,0,0));
 586         m_carChassis->setAngularVelocity(btVector3(0,0,0));
 587         m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
 588         if (m_vehicle)
 589         {
 590                 m_vehicle->resetSuspension();
 591                 for (int i=0;i<m_vehicle->getNumWheels();i++)
 592                 {
 593                         //synchronize the wheels with the (interpolated) chassis worldtransform
 594                         m_vehicle->updateWheelTransform(i,true);
 595                 }
 596         }
 597         btTransform liftTrans;
 598         liftTrans.setIdentity();
 599         liftTrans.setOrigin(m_liftStartPos);
 600         m_liftBody->activate();
 601         m_liftBody->setCenterOfMassTransform(liftTrans);
 602         m_liftBody->setLinearVelocity(btVector3(0,0,0));
 603         m_liftBody->setAngularVelocity(btVector3(0,0,0));
 604
 605         btTransform forkTrans;
 606         forkTrans.setIdentity();
 607         forkTrans.setOrigin(m_forkStartPos);
 608         m_forkBody->activate();
 609         m_forkBody->setCenterOfMassTransform(forkTrans);
 610         m_forkBody->setLinearVelocity(btVector3(0,0,0));
 611         m_forkBody->setAngularVelocity(btVector3(0,0,0));
 612
 613 //      m_liftHinge->setLimit(-LIFT_EPS, LIFT_EPS);
 614         m_liftHinge->setLimit(0.0f, 0.0f);
 615         m_liftHinge->enableAngularMotor(false, 0, 0);
 616
 617
 618         m_forkSlider->setLowerLinLimit(0.1f);
 619         m_forkSlider->setUpperLinLimit(0.1f);
 620         m_forkSlider->setPoweredLinMotor(false);
 621
 622         btTransform loadTrans;
 623         loadTrans.setIdentity();
 624         loadTrans.setOrigin(m_loadStartPos);
 625         m_loadBody->activate();
 626         m_loadBody->setCenterOfMassTransform(loadTrans);
 627         m_loadBody->setLinearVelocity(btVector3(0,0,0));
 628         m_loadBody->setAngularVelocity(btVector3(0,0,0));
 629
 630 }
 631
 632
 633
 634 void ForkLiftDemo::specialKeyboardUp(int key, int x, int y)
 635 {
 636    switch (key)
 637         {
 638         case GLUT_KEY_UP :
 639                 {
 640                         lockForkSlider();
 641                         gEngineForce = 0.f;
 642                         gBreakingForce = defaultBreakingForce;
 643                 break;
 644                 }
 645         case GLUT_KEY_DOWN :
 646                 {
 647                         lockForkSlider();
 648                         gEngineForce = 0.f;
 649                         gBreakingForce = defaultBreakingForce;
 650                 break;
 651                 }
 652         case GLUT_KEY_LEFT:
 653         case GLUT_KEY_RIGHT:
 654                 {
 655                         lockLiftHinge();
 656                         break;
 657                 }
 658         default:
 659                 DemoApplication::specialKeyboardUp(key,x,y);
 660                 break;
 661         }
 662 }
 663
 664
 665 void ForkLiftDemo::specialKeyboard(int key, int x, int y)
 666 {
 667
 668         if (key==GLUT_KEY_END)
 669                 return;
 670
 671         //      printf("key = %i x=%i y=%i\n",key,x,y);
 672
 673         int state;
 674         state=glutGetModifiers();
 675         if (state & GLUT_ACTIVE_SHIFT)
 676         {
 677                 switch (key)
 678                         {
 679                         case GLUT_KEY_LEFT :
 680                                 {
 681
 682                                         m_liftHinge->setLimit(-M_PI/16.0f, M_PI/8.0f);
 683                                         m_liftHinge->enableAngularMotor(true, -0.1, 10.0);
 684                                         break;
 685                                 }
 686                         case GLUT_KEY_RIGHT :
 687                                 {
 688
 689                                         m_liftHinge->setLimit(-M_PI/16.0f, M_PI/8.0f);
 690                                         m_liftHinge->enableAngularMotor(true, 0.1, 10.0);
 691                                         break;
 692                                 }
 693                         case GLUT_KEY_UP :
 694                                 {
 695                                         m_forkSlider->setLowerLinLimit(0.1f);
 696                                         m_forkSlider->setUpperLinLimit(3.9f);
 697                                         m_forkSlider->setPoweredLinMotor(true);
 698                                         m_forkSlider->setMaxLinMotorForce(10.0);
 699                                         m_forkSlider->setTargetLinMotorVelocity(1.0);
 700                                         break;
 701                                 }
 702                         case GLUT_KEY_DOWN :
 703                                 {
 704                                         m_forkSlider->setLowerLinLimit(0.1f);
 705                                         m_forkSlider->setUpperLinLimit(3.9f);
 706                                         m_forkSlider->setPoweredLinMotor(true);
 707                                         m_forkSlider->setMaxLinMotorForce(10.0);
 708                                         m_forkSlider->setTargetLinMotorVelocity(-1.0);
 709                                         break;
 710                                 }
 711
 712                         default:
 713                                 DemoApplication::specialKeyboard(key,x,y);
 714                                 break;
 715                         }
 716
 717         } else
 718         {
 719                         switch (key)
 720                         {
 721                         case GLUT_KEY_LEFT :
 722                                 {
 723                                         gVehicleSteering += steeringIncrement;
 724                                         if (    gVehicleSteering > steeringClamp)
 725                                                 gVehicleSteering = steeringClamp;
 726
 727                                         break;
 728                                 }
 729                         case GLUT_KEY_RIGHT :
 730                                 {
 731                                         gVehicleSteering -= steeringIncrement;
 732                                         if (    gVehicleSteering < -steeringClamp)
 733                                                 gVehicleSteering = -steeringClamp;
 734
 735                                         break;
 736                                 }
 737                         case GLUT_KEY_UP :
 738                                 {
 739                                         gEngineForce = maxEngineForce;
 740                                         gBreakingForce = 0.f;
 741                                         break;
 742                                 }
 743                         case GLUT_KEY_DOWN :
 744                                 {
 745                                         gEngineForce = -maxEngineForce;
 746                                         gBreakingForce = 0.f;
 747                                         break;
 748                                 }
 749
 750                         case GLUT_KEY_F5:
 751                                 m_useDefaultCamera = !m_useDefaultCamera;
 752                                 break;
 753                         default:
 754                                 DemoApplication::specialKeyboard(key,x,y);
 755                                 break;
 756                         }
 757
 758         }
 759         //      glutPostRedisplay();
 760
 761
 762 }
 763
 764 void    ForkLiftDemo::updateCamera()
 765 {
 766
 767 //#define DISABLE_CAMERA 1
 768         if(m_useDefaultCamera)
 769         {
 770                 DemoApplication::updateCamera();
 771                 return;
 772         }
 773
 774         glMatrixMode(GL_PROJECTION);
 775         glLoadIdentity();
 776
 777         btTransform chassisWorldTrans;
 778
 779         //look at the vehicle
 780         m_carChassis->getMotionState()->getWorldTransform(chassisWorldTrans);
 781         m_cameraTargetPosition = chassisWorldTrans.getOrigin();
 782
 783         //interpolate the camera height
 784 #ifdef FORCE_ZAXIS_UP
 785         m_cameraPosition[2] = (15.0*m_cameraPosition[2] + m_cameraTargetPosition[2] + m_cameraHeight)/16.0;
 786 #else
 787         m_cameraPosition[1] = (15.0*m_cameraPosition[1] + m_cameraTargetPosition[1] + m_cameraHeight)/16.0;
 788 #endif
 789
 790         btVector3 camToObject = m_cameraTargetPosition - m_cameraPosition;
 791
 792         //keep distance between min and max distance
 793         float cameraDistance = camToObject.length();
 794         float correctionFactor = 0.f;
 795         if (cameraDistance < m_minCameraDistance)
 796         {
 797                 correctionFactor = 0.15*(m_minCameraDistance-cameraDistance)/cameraDistance;
 798         }
 799         if (cameraDistance > m_maxCameraDistance)
 800         {
 801                 correctionFactor = 0.15*(m_maxCameraDistance-cameraDistance)/cameraDistance;
 802         }
 803         m_cameraPosition -= correctionFactor*camToObject;
 804
 805         //update OpenGL camera settings
 806         btScalar aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
 807         glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0);
 808
 809          glMatrixMode(GL_MODELVIEW);
 810          glLoadIdentity();
 811
 812     gluLookAt(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2],
 813                       m_cameraTargetPosition[0],m_cameraTargetPosition[1], m_cameraTargetPosition[2],
 814                           m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
 815
 816
 817
 818 }
 819
 820 void ForkLiftDemo::lockLiftHinge(void)
 821 {
 822         btScalar hingeAngle = m_liftHinge->getHingeAngle();
 823         btScalar lowLim = m_liftHinge->getLowerLimit();
 824         btScalar hiLim = m_liftHinge->getUpperLimit();
 825         m_liftHinge->enableAngularMotor(false, 0, 0);
 826         if(hingeAngle < lowLim)
 827         {
 828 //              m_liftHinge->setLimit(lowLim, lowLim + LIFT_EPS);
 829                 m_liftHinge->setLimit(lowLim, lowLim);
 830         }
 831         else if(hingeAngle > hiLim)
 832         {
 833 //              m_liftHinge->setLimit(hiLim - LIFT_EPS, hiLim);
 834                 m_liftHinge->setLimit(hiLim, hiLim);
 835         }
 836         else
 837         {
 838 //              m_liftHinge->setLimit(hingeAngle - LIFT_EPS, hingeAngle + LIFT_EPS);
 839                 m_liftHinge->setLimit(hingeAngle, hingeAngle);
 840         }
 841         return;
 842 } // ForkLiftDemo::lockLiftHinge()
 843
 844 void ForkLiftDemo::lockForkSlider(void)
 845 {
 846         btScalar linDepth = m_forkSlider->getLinearPos();
 847         btScalar lowLim = m_forkSlider->getLowerLinLimit();
 848         btScalar hiLim = m_forkSlider->getUpperLinLimit();
 849         m_forkSlider->setPoweredLinMotor(false);
 850         if(linDepth <= lowLim)
 851         {
 852                 m_forkSlider->setLowerLinLimit(lowLim);
 853                 m_forkSlider->setUpperLinLimit(lowLim);
 854         }
 855         else if(linDepth > hiLim)
 856         {
 857                 m_forkSlider->setLowerLinLimit(hiLim);
 858                 m_forkSlider->setUpperLinLimit(hiLim);
 859         }
 860         else
 861         {
 862                 m_forkSlider->setLowerLinLimit(linDepth);
 863                 m_forkSlider->setUpperLinLimit(linDepth);
 864         }
 865         return;
 866 } // ForkLiftDemo::lockForkSlider()