1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
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
24 #include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
25 #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
26 #include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
27
28 class btVehicleTuning;
29 struct btVehicleRaycaster;
30 class btCollisionShape;
31
32 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
33 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
34 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
35
36 #include "../CommonInterfaces/CommonExampleInterface.h"
37 #include "LinearMath/btAlignedObjectArray.h"
38 #include "btBulletCollisionCommon.h"
39 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
40 #include "../CommonInterfaces/CommonRenderInterface.h"
41 #include "../CommonInterfaces/CommonWindowInterface.h"
42 #include "../CommonInterfaces/CommonGraphicsAppInterface.h"
43
44 ///VehicleDemo shows how to setup and use the built-in raycast vehicle
45 class ForkLiftDemo : public CommonExampleInterface
46 {
47 public:
48 GUIHelperInterface* m_guiHelper;
49
50 /* extra stuff*/
51 btVector3 m_cameraPosition;
52 class btDiscreteDynamicsWorld* m_dynamicsWorld;
getDynamicsWorld()53 btDiscreteDynamicsWorld* getDynamicsWorld()
54 {
55 return m_dynamicsWorld;
56 }
57 btRigidBody* m_carChassis;
58 btRigidBody* localCreateRigidBody(btScalar mass, const btTransform& worldTransform, btCollisionShape* colSape);
59
60 int m_wheelInstances[4];
61
62 //----------------------------
63 btRigidBody* m_liftBody;
64 btVector3 m_liftStartPos;
65 btHingeConstraint* m_liftHinge;
66
67 btRigidBody* m_forkBody;
68 btVector3 m_forkStartPos;
69 btSliderConstraint* m_forkSlider;
70
71 btRigidBody* m_loadBody;
72 btVector3 m_loadStartPos;
73
74 void lockLiftHinge(void);
75 void lockForkSlider(void);
76
77 bool m_useDefaultCamera;
78 //----------------------------
79
80 btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
81
82 class btBroadphaseInterface* m_overlappingPairCache;
83
84 class btCollisionDispatcher* m_dispatcher;
85
86 class btConstraintSolver* m_constraintSolver;
87
88 class btDefaultCollisionConfiguration* m_collisionConfiguration;
89
90 class btTriangleIndexVertexArray* m_indexVertexArrays;
91
92 btVector3* m_vertices;
93
94 btRaycastVehicle::btVehicleTuning m_tuning;
95 btVehicleRaycaster* m_vehicleRayCaster;
96 btRaycastVehicle* m_vehicle;
97 btCollisionShape* m_wheelShape;
98
99 float m_cameraHeight;
100
101 float m_minCameraDistance;
102 float m_maxCameraDistance;
103
104 ForkLiftDemo(struct GUIHelperInterface* helper);
105
106 virtual ~ForkLiftDemo();
107
108 virtual void stepSimulation(float deltaTime);
109
110 virtual void resetForklift();
111
112 virtual void clientResetScene();
113
114 virtual void displayCallback();
115
116 virtual void specialKeyboard(int key, int x, int y);
117
118 virtual void specialKeyboardUp(int key, int x, int y);
119
mouseMoveCallback(float x,float y)120 virtual bool mouseMoveCallback(float x, float y)
121 {
122 return false;
123 }
124
mouseButtonCallback(int button,int state,float x,float y)125 virtual bool mouseButtonCallback(int button, int state, float x, float y)
126 {
127 return false;
128 }
129
130 virtual bool keyboardCallback(int key, int state);
131
132 virtual void renderScene();
133
134 virtual void physicsDebugDraw(int debugFlags);
135
136 void initPhysics();
137 void exitPhysics();
138
resetCamera()139 virtual void resetCamera()
140 {
141 float dist = 8;
142 float pitch = -32;
143 float yaw = -45;
144 float targetPos[3] = {-0.33, -0.72, 4.5};
145 m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
146 }
147
148 /*static DemoApplication* Create()
149 {
150 ForkLiftDemo* demo = new ForkLiftDemo();
151 demo->myinit();
152 demo->initPhysics();
153 return demo;
154 }
155 */
156 };
157
158 btScalar maxMotorImpulse = 4000.f;
159
160 //the sequential impulse solver has difficulties dealing with large mass ratios (differences), between loadMass and the fork parts
161 btScalar loadMass = 350.f; //
162 //btScalar loadMass = 10.f;//this should work fine for the SI solver
163
164 #ifndef M_PI
165 #define M_PI 3.14159265358979323846
166 #endif
167
168 #ifndef M_PI_2
169 #define M_PI_2 1.57079632679489661923
170 #endif
171
172 #ifndef M_PI_4
173 #define M_PI_4 0.785398163397448309616
174 #endif
175
176 int rightIndex = 0;
177 int upIndex = 1;
178 int forwardIndex = 2;
179 btVector3 wheelDirectionCS0(0, -1, 0);
180 btVector3 wheelAxleCS(-1, 0, 0);
181
182 bool useMCLPSolver = true;
183
184 #include <stdio.h> //printf debugging
185
186 #include "ForkLiftDemo.h"
187
188 ///btRaycastVehicle is the interface for the constraint that implements the raycast vehicle
189 ///notice that for higher-quality slow-moving vehicles, another approach might be better
190 ///implementing explicit hinged-wheel constraints with cylinder collision, rather then raycasts
191 float gEngineForce = 0.f;
192
193 float defaultBreakingForce = 10.f;
194 float gBreakingForce = 100.f;
195
196 float maxEngineForce = 1000.f; //this should be engine/velocity dependent
197 float maxBreakingForce = 100.f;
198
199 float gVehicleSteering = 0.f;
200 float steeringIncrement = 0.04f;
201 float steeringClamp = 0.3f;
202 float wheelRadius = 0.5f;
203 float wheelWidth = 0.4f;
204 float wheelFriction = 1000; //BT_LARGE_FLOAT;
205 float suspensionStiffness = 20.f;
206 float suspensionDamping = 2.3f;
207 float suspensionCompression = 4.4f;
208 float rollInfluence = 0.1f; //1.0f;
209
210 btScalar suspensionRestLength(0.6);
211
212 #define CUBE_HALF_EXTENTS 1
213
214 ////////////////////////////////////
215
ForkLiftDemo(struct GUIHelperInterface * helper)216 ForkLiftDemo::ForkLiftDemo(struct GUIHelperInterface* helper)
217 : m_guiHelper(helper),
218 m_carChassis(0),
219 m_liftBody(0),
220 m_forkBody(0),
221 m_loadBody(0),
222 m_indexVertexArrays(0),
223 m_vertices(0),
224 m_cameraHeight(4.f),
225 m_minCameraDistance(3.f),
226 m_maxCameraDistance(10.f)
227 {
228 helper->setUpAxis(1);
229 m_vehicle = 0;
230 m_wheelShape = 0;
231 m_cameraPosition = btVector3(30, 30, 30);
232 m_useDefaultCamera = false;
233 // setTexturing(true);
234 // setShadows(true);
235 }
236
exitPhysics()237 void ForkLiftDemo::exitPhysics()
238 {
239 //cleanup in the reverse order of creation/initialization
240
241 //remove the rigidbodies from the dynamics world and delete them
242 int i;
243 for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
244 {
245 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
246 btRigidBody* body = btRigidBody::upcast(obj);
247 if (body && body->getMotionState())
248 {
249 while (body->getNumConstraintRefs())
250 {
251 btTypedConstraint* constraint = body->getConstraintRef(0);
252 m_dynamicsWorld->removeConstraint(constraint);
253 delete constraint;
254 }
255 delete body->getMotionState();
256 m_dynamicsWorld->removeRigidBody(body);
257 }
258 else
259 {
260 m_dynamicsWorld->removeCollisionObject(obj);
261 }
262 delete obj;
263 }
264
265 //delete collision shapes
266 for (int j = 0; j < m_collisionShapes.size(); j++)
267 {
268 btCollisionShape* shape = m_collisionShapes[j];
269 delete shape;
270 }
271 m_collisionShapes.clear();
272
273 delete m_indexVertexArrays;
274 delete m_vertices;
275
276 //delete dynamics world
277 delete m_dynamicsWorld;
278 m_dynamicsWorld = 0;
279
280 delete m_vehicleRayCaster;
281 m_vehicleRayCaster = 0;
282
283 delete m_vehicle;
284 m_vehicle = 0;
285
286 delete m_wheelShape;
287 m_wheelShape = 0;
288
289 //delete solver
290 delete m_constraintSolver;
291 m_constraintSolver = 0;
292
293 //delete broadphase
294 delete m_overlappingPairCache;
295 m_overlappingPairCache = 0;
296
297 //delete dispatcher
298 delete m_dispatcher;
299 m_dispatcher = 0;
300
301 delete m_collisionConfiguration;
302 m_collisionConfiguration = 0;
303 }
304
~ForkLiftDemo()305 ForkLiftDemo::~ForkLiftDemo()
306 {
307 //exitPhysics();
308 }
309
initPhysics()310 void ForkLiftDemo::initPhysics()
311 {
312 int upAxis = 1;
313
314 m_guiHelper->setUpAxis(upAxis);
315
316 btVector3 groundExtents(50, 50, 50);
317 groundExtents[upAxis] = 3;
318 btCollisionShape* groundShape = new btBoxShape(groundExtents);
319 m_collisionShapes.push_back(groundShape);
320 m_collisionConfiguration = new btDefaultCollisionConfiguration();
321 m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
322 btVector3 worldMin(-1000, -1000, -1000);
323 btVector3 worldMax(1000, 1000, 1000);
324 m_overlappingPairCache = new btAxisSweep3(worldMin, worldMax);
325 if (useMCLPSolver)
326 {
327 btDantzigSolver* mlcp = new btDantzigSolver();
328 //btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel;
329 btMLCPSolver* sol = new btMLCPSolver(mlcp);
330 m_constraintSolver = sol;
331 }
332 else
333 {
334 m_constraintSolver = new btSequentialImpulseConstraintSolver();
335 }
336 m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_overlappingPairCache, m_constraintSolver, m_collisionConfiguration);
337 if (useMCLPSolver)
338 {
339 m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 1; //for direct solver it is better to have a small A matrix
340 }
341 else
342 {
343 m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 128; //for direct solver, it is better to solve multiple objects together, small batches have high overhead
344 }
345 m_dynamicsWorld->getSolverInfo().m_globalCfm = 0.00001;
346
347 m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
348
349 //m_dynamicsWorld->setGravity(btVector3(0,0,0));
350 btTransform tr;
351 tr.setIdentity();
352 tr.setOrigin(btVector3(0, -3, 0));
353
354 //either use heightfield or triangle mesh
355
356 //create ground object
357 localCreateRigidBody(0, tr, groundShape);
358
359 btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f, 0.5f, 2.f));
360 m_collisionShapes.push_back(chassisShape);
361
362 btCompoundShape* compound = new btCompoundShape();
363 m_collisionShapes.push_back(compound);
364 btTransform localTrans;
365 localTrans.setIdentity();
366 //localTrans effectively shifts the center of mass with respect to the chassis
367 localTrans.setOrigin(btVector3(0, 1, 0));
368
369 compound->addChildShape(localTrans, chassisShape);
370
371 {
372 btCollisionShape* suppShape = new btBoxShape(btVector3(0.5f, 0.1f, 0.5f));
373 btTransform suppLocalTrans;
374 suppLocalTrans.setIdentity();
375 //localTrans effectively shifts the center of mass with respect to the chassis
376 suppLocalTrans.setOrigin(btVector3(0, 1.0, 2.5));
377 compound->addChildShape(suppLocalTrans, suppShape);
378 }
379
380 tr.setOrigin(btVector3(0, 0.f, 0));
381
382 m_carChassis = localCreateRigidBody(800, tr, compound); //chassisShape);
383 //m_carChassis->setDamping(0.2,0.2);
384
385 m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth, wheelRadius, wheelRadius));
386
387 m_guiHelper->createCollisionShapeGraphicsObject(m_wheelShape);
388 int wheelGraphicsIndex = m_wheelShape->getUserIndex();
389
390 const float position[4] = {0, 10, 10, 0};
391 const float quaternion[4] = {0, 0, 0, 1};
392 const float color[4] = {0, 1, 0, 1};
393 const float scaling[4] = {1, 1, 1, 1};
394
395 for (int i = 0; i < 4; i++)
396 {
397 m_wheelInstances[i] = m_guiHelper->registerGraphicsInstance(wheelGraphicsIndex, position, quaternion, color, scaling);
398 }
399
400 {
401 btCollisionShape* liftShape = new btBoxShape(btVector3(0.5f, 2.0f, 0.05f));
402 m_collisionShapes.push_back(liftShape);
403 btTransform liftTrans;
404 m_liftStartPos = btVector3(0.0f, 2.5f, 3.05f);
405 liftTrans.setIdentity();
406 liftTrans.setOrigin(m_liftStartPos);
407 m_liftBody = localCreateRigidBody(10, liftTrans, liftShape);
408
409 btTransform localA, localB;
410 localA.setIdentity();
411 localB.setIdentity();
412 localA.getBasis().setEulerZYX(0, M_PI_2, 0);
413 localA.setOrigin(btVector3(0.0, 1.0, 3.05));
414 localB.getBasis().setEulerZYX(0, M_PI_2, 0);
415 localB.setOrigin(btVector3(0.0, -1.5, -0.05));
416 m_liftHinge = new btHingeConstraint(*m_carChassis, *m_liftBody, localA, localB);
417 // m_liftHinge->setLimit(-LIFT_EPS, LIFT_EPS);
418 m_liftHinge->setLimit(0.0f, 0.0f);
419 m_dynamicsWorld->addConstraint(m_liftHinge, true);
420
421 btCollisionShape* forkShapeA = new btBoxShape(btVector3(1.0f, 0.1f, 0.1f));
422 m_collisionShapes.push_back(forkShapeA);
423 btCompoundShape* forkCompound = new btCompoundShape();
424 m_collisionShapes.push_back(forkCompound);
425 btTransform forkLocalTrans;
426 forkLocalTrans.setIdentity();
427 forkCompound->addChildShape(forkLocalTrans, forkShapeA);
428
429 btCollisionShape* forkShapeB = new btBoxShape(btVector3(0.1f, 0.02f, 0.6f));
430 m_collisionShapes.push_back(forkShapeB);
431 forkLocalTrans.setIdentity();
432 forkLocalTrans.setOrigin(btVector3(-0.9f, -0.08f, 0.7f));
433 forkCompound->addChildShape(forkLocalTrans, forkShapeB);
434
435 btCollisionShape* forkShapeC = new btBoxShape(btVector3(0.1f, 0.02f, 0.6f));
436 m_collisionShapes.push_back(forkShapeC);
437 forkLocalTrans.setIdentity();
438 forkLocalTrans.setOrigin(btVector3(0.9f, -0.08f, 0.7f));
439 forkCompound->addChildShape(forkLocalTrans, forkShapeC);
440
441 btTransform forkTrans;
442 m_forkStartPos = btVector3(0.0f, 0.6f, 3.2f);
443 forkTrans.setIdentity();
444 forkTrans.setOrigin(m_forkStartPos);
445 m_forkBody = localCreateRigidBody(5, forkTrans, forkCompound);
446
447 localA.setIdentity();
448 localB.setIdentity();
449 localA.getBasis().setEulerZYX(0, 0, M_PI_2);
450 localA.setOrigin(btVector3(0.0f, -1.9f, 0.05f));
451 localB.getBasis().setEulerZYX(0, 0, M_PI_2);
452 localB.setOrigin(btVector3(0.0, 0.0, -0.1));
453 m_forkSlider = new btSliderConstraint(*m_liftBody, *m_forkBody, localA, localB, true);
454 m_forkSlider->setLowerLinLimit(0.1f);
455 m_forkSlider->setUpperLinLimit(0.1f);
456 // m_forkSlider->setLowerAngLimit(-LIFT_EPS);
457 // m_forkSlider->setUpperAngLimit(LIFT_EPS);
458 m_forkSlider->setLowerAngLimit(0.0f);
459 m_forkSlider->setUpperAngLimit(0.0f);
460 m_dynamicsWorld->addConstraint(m_forkSlider, true);
461
462 btCompoundShape* loadCompound = new btCompoundShape();
463 m_collisionShapes.push_back(loadCompound);
464 btCollisionShape* loadShapeA = new btBoxShape(btVector3(2.0f, 0.5f, 0.5f));
465 m_collisionShapes.push_back(loadShapeA);
466 btTransform loadTrans;
467 loadTrans.setIdentity();
468 loadCompound->addChildShape(loadTrans, loadShapeA);
469 btCollisionShape* loadShapeB = new btBoxShape(btVector3(0.1f, 1.0f, 1.0f));
470 m_collisionShapes.push_back(loadShapeB);
471 loadTrans.setIdentity();
472 loadTrans.setOrigin(btVector3(2.1f, 0.0f, 0.0f));
473 loadCompound->addChildShape(loadTrans, loadShapeB);
474 btCollisionShape* loadShapeC = new btBoxShape(btVector3(0.1f, 1.0f, 1.0f));
475 m_collisionShapes.push_back(loadShapeC);
476 loadTrans.setIdentity();
477 loadTrans.setOrigin(btVector3(-2.1f, 0.0f, 0.0f));
478 loadCompound->addChildShape(loadTrans, loadShapeC);
479 loadTrans.setIdentity();
480 m_loadStartPos = btVector3(0.0f, 3.5f, 7.0f);
481 loadTrans.setOrigin(m_loadStartPos);
482 m_loadBody = localCreateRigidBody(loadMass, loadTrans, loadCompound);
483 }
484
485 /// create vehicle
486 {
487 m_vehicleRayCaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
488 m_vehicle = new btRaycastVehicle(m_tuning, m_carChassis, m_vehicleRayCaster);
489
490 ///never deactivate the vehicle
491 m_carChassis->setActivationState(DISABLE_DEACTIVATION);
492
493 m_dynamicsWorld->addVehicle(m_vehicle);
494
495 float connectionHeight = 1.2f;
496
497 bool isFrontWheel = true;
498
499 //choose coordinate system
500 m_vehicle->setCoordinateSystem(rightIndex, upIndex, forwardIndex);
501
502 btVector3 connectionPointCS0(CUBE_HALF_EXTENTS - (0.3 * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
503
504 m_vehicle->addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
505 connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS + (0.3 * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
506
507 m_vehicle->addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
508 connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS + (0.3 * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
509 isFrontWheel = false;
510 m_vehicle->addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
511 connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS - (0.3 * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
512 m_vehicle->addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
513
514 for (int i = 0; i < m_vehicle->getNumWheels(); i++)
515 {
516 btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
517 wheel.m_suspensionStiffness = suspensionStiffness;
518 wheel.m_wheelsDampingRelaxation = suspensionDamping;
519 wheel.m_wheelsDampingCompression = suspensionCompression;
520 wheel.m_frictionSlip = wheelFriction;
521 wheel.m_rollInfluence = rollInfluence;
522 }
523 }
524
525 resetForklift();
526
527 // setCameraDistance(26.f);
528
529 m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
530 }
531
physicsDebugDraw(int debugFlags)532 void ForkLiftDemo::physicsDebugDraw(int debugFlags)
533 {
534 if (m_dynamicsWorld && m_dynamicsWorld->getDebugDrawer())
535 {
536 m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugFlags);
537 m_dynamicsWorld->debugDrawWorld();
538 }
539 }
540
541 //to be implemented by the demo
renderScene()542 void ForkLiftDemo::renderScene()
543 {
544 m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
545
546 for (int i = 0; i < m_vehicle->getNumWheels(); i++)
547 {
548 //synchronize the wheels with the (interpolated) chassis worldtransform
549 m_vehicle->updateWheelTransform(i, true);
550
551 CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
552 if (renderer)
553 {
554 btTransform tr = m_vehicle->getWheelInfo(i).m_worldTransform;
555 btVector3 pos = tr.getOrigin();
556 btQuaternion orn = tr.getRotation();
557 renderer->writeSingleInstanceTransformToCPU(pos, orn, m_wheelInstances[i]);
558 }
559 }
560
561 m_guiHelper->render(m_dynamicsWorld);
562
563 ATTRIBUTE_ALIGNED16(btScalar)
564 m[16];
565 int i;
566
567 btVector3 wheelColor(1, 0, 0);
568
569 btVector3 worldBoundsMin, worldBoundsMax;
570 getDynamicsWorld()->getBroadphase()->getBroadphaseAabb(worldBoundsMin, worldBoundsMax);
571
572 for (i = 0; i < m_vehicle->getNumWheels(); i++)
573 {
574 //synchronize the wheels with the (interpolated) chassis worldtransform
575 m_vehicle->updateWheelTransform(i, true);
576 //draw wheels (cylinders)
577 m_vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(m);
578 // m_shapeDrawer->drawOpenGL(m,m_wheelShape,wheelColor,getDebugMode(),worldBoundsMin,worldBoundsMax);
579 }
580
581 #if 0
582 int lineWidth=400;
583 int xStart = m_glutScreenWidth - lineWidth;
584 int yStart = 20;
585
586 if((getDebugMode() & btIDebugDraw::DBG_NoHelpText)==0)
587 {
588 setOrthographicProjection();
589 glDisable(GL_LIGHTING);
590 glColor3f(0, 0, 0);
591 char buf[124];
592
593 sprintf(buf,"SHIFT+Cursor Left/Right - rotate lift");
594 GLDebugDrawString(xStart,20,buf);
595 yStart+=20;
596 sprintf(buf,"SHIFT+Cursor UP/Down - fork up/down");
597 yStart+=20;
598 GLDebugDrawString(xStart,yStart,buf);
599
600 if (m_useDefaultCamera)
601 {
602 sprintf(buf,"F5 - camera mode (free)");
603 } else
604 {
605 sprintf(buf,"F5 - camera mode (follow)");
606 }
607 yStart+=20;
608 GLDebugDrawString(xStart,yStart,buf);
609
610 yStart+=20;
611 if (m_dynamicsWorld->getConstraintSolver()->getSolverType()==BT_MLCP_SOLVER)
612 {
613 sprintf(buf,"F6 - solver (direct MLCP)");
614 } else
615 {
616 sprintf(buf,"F6 - solver (sequential impulse)");
617 }
618 GLDebugDrawString(xStart,yStart,buf);
619 btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) m_dynamicsWorld;
620 if (world->getLatencyMotionStateInterpolation())
621 {
622 sprintf(buf,"F7 - motionstate interpolation (on)");
623 } else
624 {
625 sprintf(buf,"F7 - motionstate interpolation (off)");
626 }
627 yStart+=20;
628 GLDebugDrawString(xStart,yStart,buf);
629
630 sprintf(buf,"Click window for keyboard focus");
631 yStart+=20;
632 GLDebugDrawString(xStart,yStart,buf);
633
634
635 resetPerspectiveProjection();
636 glEnable(GL_LIGHTING);
637 }
638 #endif
639 }
640
stepSimulation(float deltaTime)641 void ForkLiftDemo::stepSimulation(float deltaTime)
642 {
643 //glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
644
645 {
646 int wheelIndex = 2;
647 m_vehicle->applyEngineForce(gEngineForce, wheelIndex);
648 m_vehicle->setBrake(gBreakingForce, wheelIndex);
649 wheelIndex = 3;
650 m_vehicle->applyEngineForce(gEngineForce, wheelIndex);
651 m_vehicle->setBrake(gBreakingForce, wheelIndex);
652
653 wheelIndex = 0;
654 m_vehicle->setSteeringValue(gVehicleSteering, wheelIndex);
655 wheelIndex = 1;
656 m_vehicle->setSteeringValue(gVehicleSteering, wheelIndex);
657 }
658
659 float dt = deltaTime;
660
661 if (m_dynamicsWorld)
662 {
663 //during idle mode, just run 1 simulation step maximum
664 int maxSimSubSteps = 2;
665
666 int numSimSteps;
667 numSimSteps = m_dynamicsWorld->stepSimulation(dt, maxSimSubSteps);
668
669 if (m_dynamicsWorld->getConstraintSolver()->getSolverType() == BT_MLCP_SOLVER)
670 {
671 btMLCPSolver* sol = (btMLCPSolver*)m_dynamicsWorld->getConstraintSolver();
672 int numFallbacks = sol->getNumFallbacks();
673 if (numFallbacks)
674 {
675 static int totalFailures = 0;
676 totalFailures += numFallbacks;
677 printf("MLCP solver failed %d times, falling back to btSequentialImpulseSolver (SI)\n", totalFailures);
678 }
679 sol->setNumFallbacks(0);
680 }
681
682 //#define VERBOSE_FEEDBACK
683 #ifdef VERBOSE_FEEDBACK
684 if (!numSimSteps)
685 printf("Interpolated transforms\n");
686 else
687 {
688 if (numSimSteps > maxSimSubSteps)
689 {
690 //detect dropping frames
691 printf("Dropped (%i) simulation steps out of %i\n", numSimSteps - maxSimSubSteps, numSimSteps);
692 }
693 else
694 {
695 printf("Simulated (%i) steps\n", numSimSteps);
696 }
697 }
698 #endif //VERBOSE_FEEDBACK
699 }
700 }
701
displayCallback(void)702 void ForkLiftDemo::displayCallback(void)
703 {
704 // glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
705
706 //renderme();
707
708 //optional but useful: debug drawing
709 if (m_dynamicsWorld)
710 m_dynamicsWorld->debugDrawWorld();
711
712 // glFlush();
713 // glutSwapBuffers();
714 }
715
clientResetScene()716 void ForkLiftDemo::clientResetScene()
717 {
718 exitPhysics();
719 initPhysics();
720 }
721
resetForklift()722 void ForkLiftDemo::resetForklift()
723 {
724 gVehicleSteering = 0.f;
725 gBreakingForce = defaultBreakingForce;
726 gEngineForce = 0.f;
727
728 m_carChassis->setCenterOfMassTransform(btTransform::getIdentity());
729 m_carChassis->setLinearVelocity(btVector3(0, 0, 0));
730 m_carChassis->setAngularVelocity(btVector3(0, 0, 0));
731 m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle(), getDynamicsWorld()->getDispatcher());
732 if (m_vehicle)
733 {
734 m_vehicle->resetSuspension();
735 for (int i = 0; i < m_vehicle->getNumWheels(); i++)
736 {
737 //synchronize the wheels with the (interpolated) chassis worldtransform
738 m_vehicle->updateWheelTransform(i, true);
739 }
740 }
741 btTransform liftTrans;
742 liftTrans.setIdentity();
743 liftTrans.setOrigin(m_liftStartPos);
744 m_liftBody->activate();
745 m_liftBody->setCenterOfMassTransform(liftTrans);
746 m_liftBody->setLinearVelocity(btVector3(0, 0, 0));
747 m_liftBody->setAngularVelocity(btVector3(0, 0, 0));
748
749 btTransform forkTrans;
750 forkTrans.setIdentity();
751 forkTrans.setOrigin(m_forkStartPos);
752 m_forkBody->activate();
753 m_forkBody->setCenterOfMassTransform(forkTrans);
754 m_forkBody->setLinearVelocity(btVector3(0, 0, 0));
755 m_forkBody->setAngularVelocity(btVector3(0, 0, 0));
756
757 // m_liftHinge->setLimit(-LIFT_EPS, LIFT_EPS);
758 m_liftHinge->setLimit(0.0f, 0.0f);
759 m_liftHinge->enableAngularMotor(false, 0, 0);
760
761 m_forkSlider->setLowerLinLimit(0.1f);
762 m_forkSlider->setUpperLinLimit(0.1f);
763 m_forkSlider->setPoweredLinMotor(false);
764
765 btTransform loadTrans;
766 loadTrans.setIdentity();
767 loadTrans.setOrigin(m_loadStartPos);
768 m_loadBody->activate();
769 m_loadBody->setCenterOfMassTransform(loadTrans);
770 m_loadBody->setLinearVelocity(btVector3(0, 0, 0));
771 m_loadBody->setAngularVelocity(btVector3(0, 0, 0));
772 }
773
keyboardCallback(int key,int state)774 bool ForkLiftDemo::keyboardCallback(int key, int state)
775 {
776 bool handled = false;
777 bool isShiftPressed = m_guiHelper->getAppInterface()->m_window->isModifierKeyPressed(B3G_SHIFT);
778
779 if (state)
780 {
781 if (isShiftPressed)
782 {
783 switch (key)
784 {
785 case B3G_LEFT_ARROW:
786 {
787 m_liftHinge->setLimit(-M_PI / 16.0f, M_PI / 8.0f);
788 m_liftHinge->enableAngularMotor(true, -0.1, maxMotorImpulse);
789 handled = true;
790 break;
791 }
792 case B3G_RIGHT_ARROW:
793 {
794 m_liftHinge->setLimit(-M_PI / 16.0f, M_PI / 8.0f);
795 m_liftHinge->enableAngularMotor(true, 0.1, maxMotorImpulse);
796 handled = true;
797 break;
798 }
799 case B3G_UP_ARROW:
800 {
801 m_forkSlider->setLowerLinLimit(0.1f);
802 m_forkSlider->setUpperLinLimit(3.9f);
803 m_forkSlider->setPoweredLinMotor(true);
804 m_forkSlider->setMaxLinMotorForce(maxMotorImpulse);
805 m_forkSlider->setTargetLinMotorVelocity(1.0);
806 handled = true;
807 break;
808 }
809 case B3G_DOWN_ARROW:
810 {
811 m_forkSlider->setLowerLinLimit(0.1f);
812 m_forkSlider->setUpperLinLimit(3.9f);
813 m_forkSlider->setPoweredLinMotor(true);
814 m_forkSlider->setMaxLinMotorForce(maxMotorImpulse);
815 m_forkSlider->setTargetLinMotorVelocity(-1.0);
816 handled = true;
817 break;
818 }
819 }
820 }
821 else
822 {
823 switch (key)
824 {
825 case B3G_LEFT_ARROW:
826 {
827 handled = true;
828 gVehicleSteering += steeringIncrement;
829 if (gVehicleSteering > steeringClamp)
830 gVehicleSteering = steeringClamp;
831
832 break;
833 }
834 case B3G_RIGHT_ARROW:
835 {
836 handled = true;
837 gVehicleSteering -= steeringIncrement;
838 if (gVehicleSteering < -steeringClamp)
839 gVehicleSteering = -steeringClamp;
840
841 break;
842 }
843 case B3G_UP_ARROW:
844 {
845 handled = true;
846 gEngineForce = maxEngineForce;
847 gBreakingForce = 0.f;
848 break;
849 }
850 case B3G_DOWN_ARROW:
851 {
852 handled = true;
853 gEngineForce = -maxEngineForce;
854 gBreakingForce = 0.f;
855 break;
856 }
857
858 case B3G_F7:
859 {
860 handled = true;
861 btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*)m_dynamicsWorld;
862 world->setLatencyMotionStateInterpolation(!world->getLatencyMotionStateInterpolation());
863 printf("world latencyMotionStateInterpolation = %d\n", world->getLatencyMotionStateInterpolation());
864 break;
865 }
866 case B3G_F6:
867 {
868 handled = true;
869 //switch solver (needs demo restart)
870 useMCLPSolver = !useMCLPSolver;
871 printf("switching to useMLCPSolver = %d\n", useMCLPSolver);
872
873 delete m_constraintSolver;
874 if (useMCLPSolver)
875 {
876 btDantzigSolver* mlcp = new btDantzigSolver();
877 //btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel;
878 btMLCPSolver* sol = new btMLCPSolver(mlcp);
879 m_constraintSolver = sol;
880 }
881 else
882 {
883 m_constraintSolver = new btSequentialImpulseConstraintSolver();
884 }
885
886 m_dynamicsWorld->setConstraintSolver(m_constraintSolver);
887
888 //exitPhysics();
889 //initPhysics();
890 break;
891 }
892
893 case B3G_F5:
894 handled = true;
895 m_useDefaultCamera = !m_useDefaultCamera;
896 break;
897 default:
898 break;
899 }
900 }
901 }
902 else
903 {
904 switch (key)
905 {
906 case B3G_UP_ARROW:
907 {
908 lockForkSlider();
909 gEngineForce = 0.f;
910 gBreakingForce = defaultBreakingForce;
911 handled = true;
912 break;
913 }
914 case B3G_DOWN_ARROW:
915 {
916 lockForkSlider();
917 gEngineForce = 0.f;
918 gBreakingForce = defaultBreakingForce;
919 handled = true;
920 break;
921 }
922 case B3G_LEFT_ARROW:
923 case B3G_RIGHT_ARROW:
924 {
925 lockLiftHinge();
926 handled = true;
927 break;
928 }
929 default:
930
931 break;
932 }
933 }
934 return handled;
935 }
936
specialKeyboardUp(int key,int x,int y)937 void ForkLiftDemo::specialKeyboardUp(int key, int x, int y)
938 {
939 #if 0
940
941 #endif
942 }
943
specialKeyboard(int key,int x,int y)944 void ForkLiftDemo::specialKeyboard(int key, int x, int y)
945 {
946 #if 0
947 if (key==GLUT_KEY_END)
948 return;
949
950 // printf("key = %i x=%i y=%i\n",key,x,y);
951
952 int state;
953 state=glutGetModifiers();
954 if (state & GLUT_ACTIVE_SHIFT)
955 {
956 switch (key)
957 {
958 case GLUT_KEY_LEFT :
959 {
960
961 m_liftHinge->setLimit(-M_PI/16.0f, M_PI/8.0f);
962 m_liftHinge->enableAngularMotor(true, -0.1, maxMotorImpulse);
963 break;
964 }
965 case GLUT_KEY_RIGHT :
966 {
967
968 m_liftHinge->setLimit(-M_PI/16.0f, M_PI/8.0f);
969 m_liftHinge->enableAngularMotor(true, 0.1, maxMotorImpulse);
970 break;
971 }
972 case GLUT_KEY_UP :
973 {
974 m_forkSlider->setLowerLinLimit(0.1f);
975 m_forkSlider->setUpperLinLimit(3.9f);
976 m_forkSlider->setPoweredLinMotor(true);
977 m_forkSlider->setMaxLinMotorForce(maxMotorImpulse);
978 m_forkSlider->setTargetLinMotorVelocity(1.0);
979 break;
980 }
981 case GLUT_KEY_DOWN :
982 {
983 m_forkSlider->setLowerLinLimit(0.1f);
984 m_forkSlider->setUpperLinLimit(3.9f);
985 m_forkSlider->setPoweredLinMotor(true);
986 m_forkSlider->setMaxLinMotorForce(maxMotorImpulse);
987 m_forkSlider->setTargetLinMotorVelocity(-1.0);
988 break;
989 }
990
991 default:
992 DemoApplication::specialKeyboard(key,x,y);
993 break;
994 }
995
996 } else
997 {
998 switch (key)
999 {
1000 case GLUT_KEY_LEFT :
1001 {
1002 gVehicleSteering += steeringIncrement;
1003 if ( gVehicleSteering > steeringClamp)
1004 gVehicleSteering = steeringClamp;
1005
1006 break;
1007 }
1008 case GLUT_KEY_RIGHT :
1009 {
1010 gVehicleSteering -= steeringIncrement;
1011 if ( gVehicleSteering < -steeringClamp)
1012 gVehicleSteering = -steeringClamp;
1013
1014 break;
1015 }
1016 case GLUT_KEY_UP :
1017 {
1018 gEngineForce = maxEngineForce;
1019 gBreakingForce = 0.f;
1020 break;
1021 }
1022 case GLUT_KEY_DOWN :
1023 {
1024 gEngineForce = -maxEngineForce;
1025 gBreakingForce = 0.f;
1026 break;
1027 }
1028
1029 case GLUT_KEY_F7:
1030 {
1031 btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*)m_dynamicsWorld;
1032 world->setLatencyMotionStateInterpolation(!world->getLatencyMotionStateInterpolation());
1033 printf("world latencyMotionStateInterpolation = %d\n", world->getLatencyMotionStateInterpolation());
1034 break;
1035 }
1036 case GLUT_KEY_F6:
1037 {
1038 //switch solver (needs demo restart)
1039 useMCLPSolver = !useMCLPSolver;
1040 printf("switching to useMLCPSolver = %d\n", useMCLPSolver);
1041
1042 delete m_constraintSolver;
1043 if (useMCLPSolver)
1044 {
1045 btDantzigSolver* mlcp = new btDantzigSolver();
1046 //btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel;
1047 btMLCPSolver* sol = new btMLCPSolver(mlcp);
1048 m_constraintSolver = sol;
1049 } else
1050 {
1051 m_constraintSolver = new btSequentialImpulseConstraintSolver();
1052 }
1053
1054 m_dynamicsWorld->setConstraintSolver(m_constraintSolver);
1055
1056
1057 //exitPhysics();
1058 //initPhysics();
1059 break;
1060 }
1061
1062 case GLUT_KEY_F5:
1063 m_useDefaultCamera = !m_useDefaultCamera;
1064 break;
1065 default:
1066 DemoApplication::specialKeyboard(key,x,y);
1067 break;
1068 }
1069
1070 }
1071 // glutPostRedisplay();
1072
1073 #endif
1074 }
1075
lockLiftHinge(void)1076 void ForkLiftDemo::lockLiftHinge(void)
1077 {
1078 btScalar hingeAngle = m_liftHinge->getHingeAngle();
1079 btScalar lowLim = m_liftHinge->getLowerLimit();
1080 btScalar hiLim = m_liftHinge->getUpperLimit();
1081 m_liftHinge->enableAngularMotor(false, 0, 0);
1082 if (hingeAngle < lowLim)
1083 {
1084 // m_liftHinge->setLimit(lowLim, lowLim + LIFT_EPS);
1085 m_liftHinge->setLimit(lowLim, lowLim);
1086 }
1087 else if (hingeAngle > hiLim)
1088 {
1089 // m_liftHinge->setLimit(hiLim - LIFT_EPS, hiLim);
1090 m_liftHinge->setLimit(hiLim, hiLim);
1091 }
1092 else
1093 {
1094 // m_liftHinge->setLimit(hingeAngle - LIFT_EPS, hingeAngle + LIFT_EPS);
1095 m_liftHinge->setLimit(hingeAngle, hingeAngle);
1096 }
1097 return;
1098 } // ForkLiftDemo::lockLiftHinge()
1099
lockForkSlider(void)1100 void ForkLiftDemo::lockForkSlider(void)
1101 {
1102 btScalar linDepth = m_forkSlider->getLinearPos();
1103 btScalar lowLim = m_forkSlider->getLowerLinLimit();
1104 btScalar hiLim = m_forkSlider->getUpperLinLimit();
1105 m_forkSlider->setPoweredLinMotor(false);
1106 if (linDepth <= lowLim)
1107 {
1108 m_forkSlider->setLowerLinLimit(lowLim);
1109 m_forkSlider->setUpperLinLimit(lowLim);
1110 }
1111 else if (linDepth > hiLim)
1112 {
1113 m_forkSlider->setLowerLinLimit(hiLim);
1114 m_forkSlider->setUpperLinLimit(hiLim);
1115 }
1116 else
1117 {
1118 m_forkSlider->setLowerLinLimit(linDepth);
1119 m_forkSlider->setUpperLinLimit(linDepth);
1120 }
1121 return;
1122 } // ForkLiftDemo::lockForkSlider()
1123
localCreateRigidBody(btScalar mass,const btTransform & startTransform,btCollisionShape * shape)1124 btRigidBody* ForkLiftDemo::localCreateRigidBody(btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
1125 {
1126 btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));
1127
1128 //rigidbody is dynamic if and only if mass is non zero, otherwise static
1129 bool isDynamic = (mass != 0.f);
1130
1131 btVector3 localInertia(0, 0, 0);
1132 if (isDynamic)
1133 shape->calculateLocalInertia(mass, localInertia);
1134
1135 //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
1136
1137 #define USE_MOTIONSTATE 1
1138 #ifdef USE_MOTIONSTATE
1139 btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
1140
1141 btRigidBody::btRigidBodyConstructionInfo cInfo(mass, myMotionState, shape, localInertia);
1142
1143 btRigidBody* body = new btRigidBody(cInfo);
1144 //body->setContactProcessingThreshold(m_defaultContactProcessingThreshold);
1145
1146 #else
1147 btRigidBody* body = new btRigidBody(mass, 0, shape, localInertia);
1148 body->setWorldTransform(startTransform);
1149 #endif //
1150
1151 m_dynamicsWorld->addRigidBody(body);
1152 return body;
1153 }
1154
ForkLiftCreateFunc(struct CommonExampleOptions & options)1155 CommonExampleInterface* ForkLiftCreateFunc(struct CommonExampleOptions& options)
1156 {
1157 return new ForkLiftDemo(options.m_guiHelper);
1158 }
1159