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_cameraHeight(4.f),
 111 m_minCameraDistance(3.f),
 112 m_maxCameraDistance(10.f),
 113 m_indexVertexArrays(0),
 114 m_vertices(0)
 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         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 = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
 511
 512
 513 //#define VERBOSE_FEEDBACK
 514 #ifdef VERBOSE_FEEDBACK
 515                 if (!numSimSteps)
 516                         printf("Interpolated transforms\n");
 517                 else
 518                 {
 519                         if (numSimSteps > maxSimSubSteps)
 520                         {
 521                                 //detect dropping frames
 522                                 printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps);
 523                         } else
 524                         {
 525                                 printf("Simulated (%i) steps\n",numSimSteps);
 526                         }
 527                 }
 528 #endif //VERBOSE_FEEDBACK
 529
 530         }
 531
 532
 533
 534
 535
 536
 537
 538 #ifdef USE_QUICKPROF
 539         btProfiler::beginBlock("render");
 540 #endif //USE_QUICKPROF
 541
 542
 543         renderme();
 544
 545         //optional but useful: debug drawing
 546         if (m_dynamicsWorld)
 547                 m_dynamicsWorld->debugDrawWorld();
 548
 549 #ifdef USE_QUICKPROF
 550         btProfiler::endBlock("render");
 551 #endif
 552
 553
 554         glFlush();
 555         glutSwapBuffers();
 556
 557 }
 558
 559
 560
 561 void ForkLiftDemo::displayCallback(void)
 562 {
 563         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 564
 565         renderme();
 566
 567 //optional but useful: debug drawing
 568         if (m_dynamicsWorld)
 569                 m_dynamicsWorld->debugDrawWorld();
 570
 571         glFlush();
 572         glutSwapBuffers();
 573 }
 574
 575
 576
 577 void ForkLiftDemo::clientResetScene()
 578 {
 579         gVehicleSteering = 0.f;
 580         gBreakingForce = defaultBreakingForce;
 581         gEngineForce = 0.f;
 582
 583         m_carChassis->setCenterOfMassTransform(btTransform::getIdentity());
 584         m_carChassis->setLinearVelocity(btVector3(0,0,0));
 585         m_carChassis->setAngularVelocity(btVector3(0,0,0));
 586         m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
 587         if (m_vehicle)
 588         {
 589                 m_vehicle->resetSuspension();
 590                 for (int i=0;i<m_vehicle->getNumWheels();i++)
 591                 {
 592                         //synchronize the wheels with the (interpolated) chassis worldtransform
 593                         m_vehicle->updateWheelTransform(i,true);
 594                 }
 595         }
 596         btTransform liftTrans;
 597         liftTrans.setIdentity();
 598         liftTrans.setOrigin(m_liftStartPos);
 599         m_liftBody->activate();
 600         m_liftBody->setCenterOfMassTransform(liftTrans);
 601         m_liftBody->setLinearVelocity(btVector3(0,0,0));
 602         m_liftBody->setAngularVelocity(btVector3(0,0,0));
 603
 604         btTransform forkTrans;
 605         forkTrans.setIdentity();
 606         forkTrans.setOrigin(m_forkStartPos);
 607         m_forkBody->activate();
 608         m_forkBody->setCenterOfMassTransform(forkTrans);
 609         m_forkBody->setLinearVelocity(btVector3(0,0,0));
 610         m_forkBody->setAngularVelocity(btVector3(0,0,0));
 611
 612 //      m_liftHinge->setLimit(-LIFT_EPS, LIFT_EPS);
 613         m_liftHinge->setLimit(0.0f, 0.0f);
 614         m_liftHinge->enableAngularMotor(false, 0, 0);
 615
 616
 617         m_forkSlider->setLowerLinLimit(0.1f);
 618         m_forkSlider->setUpperLinLimit(0.1f);
 619         m_forkSlider->setPoweredLinMotor(false);
 620
 621         btTransform loadTrans;
 622         loadTrans.setIdentity();
 623         loadTrans.setOrigin(m_loadStartPos);
 624         m_loadBody->activate();
 625         m_loadBody->setCenterOfMassTransform(loadTrans);
 626         m_loadBody->setLinearVelocity(btVector3(0,0,0));
 627         m_loadBody->setAngularVelocity(btVector3(0,0,0));
 628
 629 }
 630
 631
 632
 633 void ForkLiftDemo::specialKeyboardUp(int key, int x, int y)
 634 {
 635    switch (key)
 636         {
 637         case GLUT_KEY_UP :
 638                 {
 639                         lockForkSlider();
 640                         gEngineForce = 0.f;
 641                         gBreakingForce = defaultBreakingForce;
 642                 break;
 643                 }
 644         case GLUT_KEY_DOWN :
 645                 {
 646                         lockForkSlider();
 647                         gEngineForce = 0.f;
 648                         gBreakingForce = defaultBreakingForce;
 649                 break;
 650                 }
 651         case GLUT_KEY_LEFT:
 652         case GLUT_KEY_RIGHT:
 653                 {
 654                         lockLiftHinge();
 655                         break;
 656                 }
 657         default:
 658                 DemoApplication::specialKeyboardUp(key,x,y);
 659                 break;
 660         }
 661 }
 662
 663
 664 void ForkLiftDemo::specialKeyboard(int key, int x, int y)
 665 {
 666
 667         if (key==GLUT_KEY_END)
 668                 return;
 669
 670         //      printf("key = %i x=%i y=%i\n",key,x,y);
 671
 672         int state;
 673         state=glutGetModifiers();
 674         if (state & GLUT_ACTIVE_SHIFT)
 675         {
 676                 switch (key)
 677                         {
 678                         case GLUT_KEY_LEFT :
 679                                 {
 680
 681                                         m_liftHinge->setLimit(-M_PI/16.0f, M_PI/8.0f);
 682                                         m_liftHinge->enableAngularMotor(true, -0.1, 10.0);
 683                                         break;
 684                                 }
 685                         case GLUT_KEY_RIGHT :
 686                                 {
 687
 688                                         m_liftHinge->setLimit(-M_PI/16.0f, M_PI/8.0f);
 689                                         m_liftHinge->enableAngularMotor(true, 0.1, 10.0);
 690                                         break;
 691                                 }
 692                         case GLUT_KEY_UP :
 693                                 {
 694                                         m_forkSlider->setLowerLinLimit(0.1f);
 695                                         m_forkSlider->setUpperLinLimit(3.9f);
 696                                         m_forkSlider->setPoweredLinMotor(true);
 697                                         m_forkSlider->setMaxLinMotorForce(10.0);
 698                                         m_forkSlider->setTargetLinMotorVelocity(1.0);
 699                                         break;
 700                                 }
 701                         case GLUT_KEY_DOWN :
 702                                 {
 703                                         m_forkSlider->setLowerLinLimit(0.1f);
 704                                         m_forkSlider->setUpperLinLimit(3.9f);
 705                                         m_forkSlider->setPoweredLinMotor(true);
 706                                         m_forkSlider->setMaxLinMotorForce(10.0);
 707                                         m_forkSlider->setTargetLinMotorVelocity(-1.0);
 708                                         break;
 709                                 }
 710
 711                         default:
 712                                 DemoApplication::specialKeyboard(key,x,y);
 713                                 break;
 714                         }
 715
 716         } else
 717         {
 718                         switch (key)
 719                         {
 720                         case GLUT_KEY_LEFT :
 721                                 {
 722                                         gVehicleSteering += steeringIncrement;
 723                                         if (    gVehicleSteering > steeringClamp)
 724                                                 gVehicleSteering = steeringClamp;
 725
 726                                         break;
 727                                 }
 728                         case GLUT_KEY_RIGHT :
 729                                 {
 730                                         gVehicleSteering -= steeringIncrement;
 731                                         if (    gVehicleSteering < -steeringClamp)
 732                                                 gVehicleSteering = -steeringClamp;
 733
 734                                         break;
 735                                 }
 736                         case GLUT_KEY_UP :
 737                                 {
 738                                         gEngineForce = maxEngineForce;
 739                                         gBreakingForce = 0.f;
 740                                         break;
 741                                 }
 742                         case GLUT_KEY_DOWN :
 743                                 {
 744                                         gEngineForce = -maxEngineForce;
 745                                         gBreakingForce = 0.f;
 746                                         break;
 747                                 }
 748
 749                         case GLUT_KEY_F5:
 750                                 m_useDefaultCamera = !m_useDefaultCamera;
 751                                 break;
 752                         default:
 753                                 DemoApplication::specialKeyboard(key,x,y);
 754                                 break;
 755                         }
 756
 757         }
 758         //      glutPostRedisplay();
 759
 760
 761 }
 762
 763 void    ForkLiftDemo::updateCamera()
 764 {
 765
 766 //#define DISABLE_CAMERA 1
 767         if(m_useDefaultCamera)
 768         {
 769                 DemoApplication::updateCamera();
 770                 return;
 771         }
 772
 773         glMatrixMode(GL_PROJECTION);
 774         glLoadIdentity();
 775
 776         btTransform chassisWorldTrans;
 777
 778         //look at the vehicle
 779         m_carChassis->getMotionState()->getWorldTransform(chassisWorldTrans);
 780         m_cameraTargetPosition = chassisWorldTrans.getOrigin();
 781
 782         //interpolate the camera height
 783 #ifdef FORCE_ZAXIS_UP
 784         m_cameraPosition[2] = (15.0*m_cameraPosition[2] + m_cameraTargetPosition[2] + m_cameraHeight)/16.0;
 785 #else
 786         m_cameraPosition[1] = (15.0*m_cameraPosition[1] + m_cameraTargetPosition[1] + m_cameraHeight)/16.0;
 787 #endif
 788
 789         btVector3 camToObject = m_cameraTargetPosition - m_cameraPosition;
 790
 791         //keep distance between min and max distance
 792         float cameraDistance = camToObject.length();
 793         float correctionFactor = 0.f;
 794         if (cameraDistance < m_minCameraDistance)
 795         {
 796                 correctionFactor = 0.15*(m_minCameraDistance-cameraDistance)/cameraDistance;
 797         }
 798         if (cameraDistance > m_maxCameraDistance)
 799         {
 800                 correctionFactor = 0.15*(m_maxCameraDistance-cameraDistance)/cameraDistance;
 801         }
 802         m_cameraPosition -= correctionFactor*camToObject;
 803
 804         //update OpenGL camera settings
 805         btScalar aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
 806         glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0);
 807
 808          glMatrixMode(GL_MODELVIEW);
 809          glLoadIdentity();
 810
 811     gluLookAt(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2],
 812                       m_cameraTargetPosition[0],m_cameraTargetPosition[1], m_cameraTargetPosition[2],
 813                           m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
 814
 815
 816
 817 }
 818
 819 void ForkLiftDemo::lockLiftHinge(void)
 820 {
 821         btScalar hingeAngle = m_liftHinge->getHingeAngle();
 822         btScalar lowLim = m_liftHinge->getLowerLimit();
 823         btScalar hiLim = m_liftHinge->getUpperLimit();
 824         m_liftHinge->enableAngularMotor(false, 0, 0);
 825         if(hingeAngle < lowLim)
 826         {
 827 //              m_liftHinge->setLimit(lowLim, lowLim + LIFT_EPS);
 828                 m_liftHinge->setLimit(lowLim, lowLim);
 829         }
 830         else if(hingeAngle > hiLim)
 831         {
 832 //              m_liftHinge->setLimit(hiLim - LIFT_EPS, hiLim);
 833                 m_liftHinge->setLimit(hiLim, hiLim);
 834         }
 835         else
 836         {
 837 //              m_liftHinge->setLimit(hingeAngle - LIFT_EPS, hingeAngle + LIFT_EPS);
 838                 m_liftHinge->setLimit(hingeAngle, hingeAngle);
 839         }
 840         return;
 841 } // ForkLiftDemo::lockLiftHinge()
 842
 843 void ForkLiftDemo::lockForkSlider(void)
 844 {
 845         btScalar linDepth = m_forkSlider->getLinearPos();
 846         btScalar lowLim = m_forkSlider->getLowerLinLimit();
 847         btScalar hiLim = m_forkSlider->getUpperLinLimit();
 848         m_forkSlider->setPoweredLinMotor(false);
 849         if(linDepth <= lowLim)
 850         {
 851                 m_forkSlider->setLowerLinLimit(lowLim);
 852                 m_forkSlider->setUpperLinLimit(lowLim);
 853         }
 854         else if(linDepth > hiLim)
 855         {
 856                 m_forkSlider->setLowerLinLimit(hiLim);
 857                 m_forkSlider->setUpperLinLimit(hiLim);
 858         }
 859         else
 860         {
 861                 m_forkSlider->setLowerLinLimit(linDepth);
 862                 m_forkSlider->setUpperLinLimit(linDepth);
 863         }
 864         return;
 865 } // ForkLiftDemo::lockForkSlider()