1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2015 Erwin Coumans http://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 ///May 2015: implemented the wheels using the Hinge2Constraint
17 ///todo: add controls for the motors etc.
18
19 #include "Hinge2Vehicle.h"
20
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
30 class btCollisionShape;
31
32 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
33 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
34
35 #include "../CommonInterfaces/CommonExampleInterface.h"
36 #include "LinearMath/btAlignedObjectArray.h"
37 #include "btBulletCollisionCommon.h"
38 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
39 #include "../CommonInterfaces/CommonRenderInterface.h"
40 #include "../CommonInterfaces/CommonWindowInterface.h"
41 #include "../CommonInterfaces/CommonGraphicsAppInterface.h"
42
43 #include "../CommonInterfaces/CommonRigidBodyBase.h"
44
45 class Hinge2Vehicle : public CommonRigidBodyBase
46 {
47 public:
48 /* extra stuff*/
49 btVector3 m_cameraPosition;
50
51 btRigidBody* m_carChassis;
52 btRigidBody* localCreateRigidBody(btScalar mass, const btTransform& worldTransform, btCollisionShape* colSape);
53
54 GUIHelperInterface* m_guiHelper;
55 int m_wheelInstances[4];
56
57 bool m_useDefaultCamera;
58 //----------------------------
59
60 class btTriangleIndexVertexArray* m_indexVertexArrays;
61
62 btVector3* m_vertices;
63
64 btCollisionShape* m_wheelShape;
65
66 float m_cameraHeight;
67
68 float m_minCameraDistance;
69 float m_maxCameraDistance;
70
71 Hinge2Vehicle(struct GUIHelperInterface* helper);
72
73 virtual ~Hinge2Vehicle();
74
75 virtual void stepSimulation(float deltaTime);
76
77 virtual void resetForklift();
78
79 virtual void clientResetScene();
80
81 virtual void displayCallback();
82
83 virtual void specialKeyboard(int key, int x, int y);
84
85 virtual void specialKeyboardUp(int key, int x, int y);
86
87 virtual bool keyboardCallback(int key, int state);
88
89 virtual void renderScene();
90
91 virtual void physicsDebugDraw(int debugFlags);
92
93 void initPhysics();
94 void exitPhysics();
95
resetCamera()96 virtual void resetCamera()
97 {
98 float dist = 8;
99 float pitch = -32;
100 float yaw = -45;
101 float targetPos[3] = {0,0,2};
102 m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
103 }
104
105 /*static DemoApplication* Create()
106 {
107 Hinge2Vehicle* demo = new Hinge2Vehicle();
108 demo->myinit();
109 demo->initPhysics();
110 return demo;
111 }
112 */
113 };
114
115 static btScalar maxMotorImpulse = 4000.f;
116
117
118 #ifndef M_PI
119 #define M_PI 3.14159265358979323846
120 #endif
121
122 #ifndef M_PI_2
123 #define M_PI_2 1.57079632679489661923
124 #endif
125
126 #ifndef M_PI_4
127 #define M_PI_4 0.785398163397448309616
128 #endif
129
130 //static int rightIndex = 0;
131 //static int upIndex = 1;
132 //static int forwardIndex = 2;
133 static btVector3 wheelDirectionCS0(0, -1, 0);
134 static btVector3 wheelAxleCS(-1, 0, 0);
135
136 static bool useMCLPSolver = false; //true;
137
138 #include <stdio.h> //printf debugging
139
140 #include "Hinge2Vehicle.h"
141
142 //static const int maxProxies = 32766;
143 //static const int maxOverlap = 65535;
144
145 static float gEngineForce = 0.f;
146
147 static float defaultBreakingForce = 10.f;
148 static float gBreakingForce = 100.f;
149
150 static float maxEngineForce = 1000.f; //this should be engine/velocity dependent
151 //static float maxBreakingForce = 100.f;
152
153 static float gVehicleSteering = 0.f;
154 static float steeringIncrement = 0.04f;
155 static float steeringClamp = 0.3f;
156 static float wheelRadius = 0.5f;
157 static float wheelWidth = 0.4f;
158 //static float wheelFriction = 1000;//BT_LARGE_FLOAT;
159 //static float suspensionStiffness = 20.f;
160 //static float suspensionDamping = 2.3f;
161 //static float suspensionCompression = 4.4f;
162 //static float rollInfluence = 0.1f;//1.0f;
163
164 //static btScalar suspensionRestLength(0.6);
165
166 #define CUBE_HALF_EXTENTS 1
167
168 ////////////////////////////////////
169
Hinge2Vehicle(struct GUIHelperInterface * helper)170 Hinge2Vehicle::Hinge2Vehicle(struct GUIHelperInterface* helper)
171 : CommonRigidBodyBase(helper),
172 m_carChassis(0),
173 m_guiHelper(helper),
174 m_indexVertexArrays(0),
175 m_vertices(0),
176 m_cameraHeight(4.f),
177 m_minCameraDistance(3.f),
178 m_maxCameraDistance(10.f)
179 {
180 helper->setUpAxis(1);
181
182 m_wheelShape = 0;
183 m_cameraPosition = btVector3(30, 30, 30);
184 m_useDefaultCamera = false;
185 }
186
exitPhysics()187 void Hinge2Vehicle::exitPhysics()
188 {
189 //cleanup in the reverse order of creation/initialization
190
191 //remove the rigidbodies from the dynamics world and delete them
192 int i;
193 for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
194 {
195 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
196 btRigidBody* body = btRigidBody::upcast(obj);
197 if (body && body->getMotionState())
198 {
199 while (body->getNumConstraintRefs())
200 {
201 btTypedConstraint* constraint = body->getConstraintRef(0);
202 m_dynamicsWorld->removeConstraint(constraint);
203 delete constraint;
204 }
205 delete body->getMotionState();
206 m_dynamicsWorld->removeRigidBody(body);
207 }
208 else
209 {
210 m_dynamicsWorld->removeCollisionObject(obj);
211 }
212 delete obj;
213 }
214
215 //delete collision shapes
216 for (int j = 0; j < m_collisionShapes.size(); j++)
217 {
218 btCollisionShape* shape = m_collisionShapes[j];
219 delete shape;
220 }
221 m_collisionShapes.clear();
222
223 delete m_indexVertexArrays;
224 delete m_vertices;
225
226 //delete dynamics world
227 delete m_dynamicsWorld;
228 m_dynamicsWorld = 0;
229
230 delete m_wheelShape;
231 m_wheelShape = 0;
232
233 //delete solver
234 delete m_solver;
235 m_solver = 0;
236
237 //delete broadphase
238 delete m_broadphase;
239 m_broadphase = 0;
240
241 //delete dispatcher
242 delete m_dispatcher;
243 m_dispatcher = 0;
244
245 delete m_collisionConfiguration;
246 m_collisionConfiguration = 0;
247 }
248
~Hinge2Vehicle()249 Hinge2Vehicle::~Hinge2Vehicle()
250 {
251 //exitPhysics();
252 }
253
initPhysics()254 void Hinge2Vehicle::initPhysics()
255 {
256 m_guiHelper->setUpAxis(1);
257
258 btCollisionShape* groundShape = new btBoxShape(btVector3(50, 3, 50));
259 m_collisionShapes.push_back(groundShape);
260 m_collisionConfiguration = new btDefaultCollisionConfiguration();
261 m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
262 btVector3 worldMin(-1000, -1000, -1000);
263 btVector3 worldMax(1000, 1000, 1000);
264 m_broadphase = new btAxisSweep3(worldMin, worldMax);
265 if (useMCLPSolver)
266 {
267 btDantzigSolver* mlcp = new btDantzigSolver();
268 //btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel;
269 btMLCPSolver* sol = new btMLCPSolver(mlcp);
270 m_solver = sol;
271 }
272 else
273 {
274 m_solver = new btSequentialImpulseConstraintSolver();
275 }
276 m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
277 if (useMCLPSolver)
278 {
279 m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 1; //for direct solver it is better to have a small A matrix
280 }
281 else
282 {
283 m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 128; //for direct solver, it is better to solve multiple objects together, small batches have high overhead
284 }
285 m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
286 m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
287
288 //m_dynamicsWorld->setGravity(btVector3(0,0,0));
289 btTransform tr;
290 tr.setIdentity();
291 tr.setOrigin(btVector3(0, -3, 0));
292
293 //either use heightfield or triangle mesh
294
295 //create ground object
296 localCreateRigidBody(0, tr, groundShape);
297
298 btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f, 0.5f, 2.f));
299 m_collisionShapes.push_back(chassisShape);
300
301 btCompoundShape* compound = new btCompoundShape();
302 m_collisionShapes.push_back(compound);
303 btTransform localTrans;
304 localTrans.setIdentity();
305 //localTrans effectively shifts the center of mass with respect to the chassis
306 localTrans.setOrigin(btVector3(0, 1, 0));
307
308 compound->addChildShape(localTrans, chassisShape);
309
310 {
311 btCollisionShape* suppShape = new btBoxShape(btVector3(0.5f, 0.1f, 0.5f));
312 btTransform suppLocalTrans;
313 suppLocalTrans.setIdentity();
314 //localTrans effectively shifts the center of mass with respect to the chassis
315 suppLocalTrans.setOrigin(btVector3(0, 1.0, 2.5));
316 compound->addChildShape(suppLocalTrans, suppShape);
317 }
318
319 const btScalar FALLHEIGHT = 5;
320 tr.setOrigin(btVector3(0, FALLHEIGHT, 0));
321
322 const btScalar chassisMass = 2.0f;
323 const btScalar wheelMass = 1.0f;
324 m_carChassis = localCreateRigidBody(chassisMass, tr, compound); //chassisShape);
325 //m_carChassis->setDamping(0.2,0.2);
326
327 //m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth,wheelRadius,wheelRadius));
328 m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth, wheelRadius, wheelRadius));
329
330 btVector3 wheelPos[4] = {
331 btVector3(btScalar(-1.), btScalar(FALLHEIGHT-0.25), btScalar(1.25)),
332 btVector3(btScalar(1.), btScalar(FALLHEIGHT-0.25), btScalar(1.25)),
333 btVector3(btScalar(1.), btScalar(FALLHEIGHT-0.25), btScalar(-1.25)),
334 btVector3(btScalar(-1.), btScalar(FALLHEIGHT-0.25), btScalar(-1.25))};
335
336 for (int i = 0; i < 4; i++)
337 {
338 // create a Hinge2 joint
339 // create two rigid bodies
340 // static bodyA (parent) on top:
341
342 btRigidBody* pBodyA = this->m_carChassis;
343 pBodyA->setActivationState(DISABLE_DEACTIVATION);
344 // dynamic bodyB (child) below it :
345 btTransform tr;
346 tr.setIdentity();
347 tr.setOrigin(wheelPos[i]);
348
349 btRigidBody* pBodyB = createRigidBody(wheelMass, tr, m_wheelShape);
350 pBodyB->setFriction(1110);
351 pBodyB->setActivationState(DISABLE_DEACTIVATION);
352 // add some data to build constraint frames
353 btVector3 parentAxis(0.f, 1.f, 0.f);
354 btVector3 childAxis(1.f, 0.f, 0.f);
355 btVector3 anchor = tr.getOrigin();
356 btHinge2Constraint* pHinge2 = new btHinge2Constraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis);
357
358 //m_guiHelper->get2dCanvasInterface();
359
360 //pHinge2->setLowerLimit(-SIMD_HALF_PI * 0.5f);
361 //pHinge2->setUpperLimit(SIMD_HALF_PI * 0.5f);
362
363 // add constraint to world
364 m_dynamicsWorld->addConstraint(pHinge2, true);
365
366 // Drive engine.
367 pHinge2->enableMotor(3, true);
368 pHinge2->setMaxMotorForce(3, 1000);
369 pHinge2->setTargetVelocity(3, 0);
370
371 // Steering engine.
372 pHinge2->enableMotor(5, true);
373 pHinge2->setMaxMotorForce(5, 1000);
374 pHinge2->setTargetVelocity(5, 0);
375
376 pHinge2->setParam( BT_CONSTRAINT_CFM, 0.15f, 2 );
377 pHinge2->setParam( BT_CONSTRAINT_ERP, 0.35f, 2 );
378
379 pHinge2->setDamping( 2, 2.0 );
380 pHinge2->setStiffness( 2, 40.0 );
381
382 pHinge2->setDbgDrawSize(btScalar(5.f));
383 }
384
385 resetForklift();
386
387 m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
388 }
389
physicsDebugDraw(int debugFlags)390 void Hinge2Vehicle::physicsDebugDraw(int debugFlags)
391 {
392 if (m_dynamicsWorld && m_dynamicsWorld->getDebugDrawer())
393 {
394 m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugFlags);
395 m_dynamicsWorld->debugDrawWorld();
396 }
397 }
398
399 //to be implemented by the demo
renderScene()400 void Hinge2Vehicle::renderScene()
401 {
402 m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
403
404 m_guiHelper->render(m_dynamicsWorld);
405
406 btVector3 wheelColor(1, 0, 0);
407
408 btVector3 worldBoundsMin, worldBoundsMax;
409 getDynamicsWorld()->getBroadphase()->getBroadphaseAabb(worldBoundsMin, worldBoundsMax);
410 }
411
stepSimulation(float deltaTime)412 void Hinge2Vehicle::stepSimulation(float deltaTime)
413 {
414 float dt = deltaTime;
415
416 if (m_dynamicsWorld)
417 {
418 //during idle mode, just run 1 simulation step maximum
419 int maxSimSubSteps = 2;
420
421 int numSimSteps;
422 numSimSteps = m_dynamicsWorld->stepSimulation(dt, maxSimSubSteps);
423
424 if (m_dynamicsWorld->getConstraintSolver()->getSolverType() == BT_MLCP_SOLVER)
425 {
426 btMLCPSolver* sol = (btMLCPSolver*)m_dynamicsWorld->getConstraintSolver();
427 int numFallbacks = sol->getNumFallbacks();
428 if (numFallbacks)
429 {
430 static int totalFailures = 0;
431 totalFailures += numFallbacks;
432 printf("MLCP solver failed %d times, falling back to btSequentialImpulseSolver (SI)\n", totalFailures);
433 }
434 sol->setNumFallbacks(0);
435 }
436
437 //#define VERBOSE_FEEDBACK
438 #ifdef VERBOSE_FEEDBACK
439 if (!numSimSteps)
440 printf("Interpolated transforms\n");
441 else
442 {
443 if (numSimSteps > maxSimSubSteps)
444 {
445 //detect dropping frames
446 printf("Dropped (%i) simulation steps out of %i\n", numSimSteps - maxSimSubSteps, numSimSteps);
447 }
448 else
449 {
450 printf("Simulated (%i) steps\n", numSimSteps);
451 }
452 }
453 #endif //VERBOSE_FEEDBACK
454 }
455 }
456
displayCallback(void)457 void Hinge2Vehicle::displayCallback(void)
458 {
459 // glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
460
461 //renderme();
462
463 //optional but useful: debug drawing
464 if (m_dynamicsWorld)
465 m_dynamicsWorld->debugDrawWorld();
466
467 // glFlush();
468 // glutSwapBuffers();
469 }
470
clientResetScene()471 void Hinge2Vehicle::clientResetScene()
472 {
473 exitPhysics();
474 initPhysics();
475 }
476
resetForklift()477 void Hinge2Vehicle::resetForklift()
478 {
479 gVehicleSteering = 0.f;
480 gBreakingForce = defaultBreakingForce;
481 gEngineForce = 0.f;
482
483 m_carChassis->setCenterOfMassTransform(btTransform::getIdentity());
484 m_carChassis->setLinearVelocity(btVector3(0, 0, 0));
485 m_carChassis->setAngularVelocity(btVector3(0, 0, 0));
486 m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle(), getDynamicsWorld()->getDispatcher());
487 }
488
keyboardCallback(int key,int state)489 bool Hinge2Vehicle::keyboardCallback(int key, int state)
490 {
491 bool handled = false;
492 bool isShiftPressed = m_guiHelper->getAppInterface()->m_window->isModifierKeyPressed(B3G_SHIFT);
493
494 if (state)
495 {
496 if (isShiftPressed)
497 {
498 }
499 else
500 {
501 switch (key)
502 {
503 case B3G_LEFT_ARROW:
504 {
505 handled = true;
506 gVehicleSteering += steeringIncrement;
507 if (gVehicleSteering > steeringClamp)
508 gVehicleSteering = steeringClamp;
509
510 break;
511 }
512 case B3G_RIGHT_ARROW:
513 {
514 handled = true;
515 gVehicleSteering -= steeringIncrement;
516 if (gVehicleSteering < -steeringClamp)
517 gVehicleSteering = -steeringClamp;
518
519 break;
520 }
521 case B3G_UP_ARROW:
522 {
523 handled = true;
524 gEngineForce = maxEngineForce;
525 gBreakingForce = 0.f;
526 break;
527 }
528 case B3G_DOWN_ARROW:
529 {
530 handled = true;
531 gEngineForce = -maxEngineForce;
532 gBreakingForce = 0.f;
533 break;
534 }
535
536 case B3G_F7:
537 {
538 handled = true;
539 btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*)m_dynamicsWorld;
540 world->setLatencyMotionStateInterpolation(!world->getLatencyMotionStateInterpolation());
541 printf("world latencyMotionStateInterpolation = %d\n", world->getLatencyMotionStateInterpolation());
542 break;
543 }
544 case B3G_F6:
545 {
546 handled = true;
547 //switch solver (needs demo restart)
548 useMCLPSolver = !useMCLPSolver;
549 printf("switching to useMLCPSolver = %d\n", useMCLPSolver);
550
551 delete m_solver;
552 if (useMCLPSolver)
553 {
554 btDantzigSolver* mlcp = new btDantzigSolver();
555 //btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel;
556 btMLCPSolver* sol = new btMLCPSolver(mlcp);
557 m_solver = sol;
558 }
559 else
560 {
561 m_solver = new btSequentialImpulseConstraintSolver();
562 }
563
564 m_dynamicsWorld->setConstraintSolver(m_solver);
565
566 //exitPhysics();
567 //initPhysics();
568 break;
569 }
570
571 case B3G_F5:
572 handled = true;
573 m_useDefaultCamera = !m_useDefaultCamera;
574 break;
575 default:
576 break;
577 }
578 }
579 }
580 else
581 {
582 }
583 return handled;
584 }
585
specialKeyboardUp(int key,int x,int y)586 void Hinge2Vehicle::specialKeyboardUp(int key, int x, int y)
587 {
588 }
589
specialKeyboard(int key,int x,int y)590 void Hinge2Vehicle::specialKeyboard(int key, int x, int y)
591 {
592 }
593
localCreateRigidBody(btScalar mass,const btTransform & startTransform,btCollisionShape * shape)594 btRigidBody* Hinge2Vehicle::localCreateRigidBody(btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
595 {
596 btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));
597
598 //rigidbody is dynamic if and only if mass is non zero, otherwise static
599 bool isDynamic = (mass != 0.f);
600
601 btVector3 localInertia(0, 0, 0);
602 if (isDynamic)
603 shape->calculateLocalInertia(mass, localInertia);
604
605 //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
606
607 #define USE_MOTIONSTATE 1
608 #ifdef USE_MOTIONSTATE
609 btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
610
611 btRigidBody::btRigidBodyConstructionInfo cInfo(mass, myMotionState, shape, localInertia);
612
613 btRigidBody* body = new btRigidBody(cInfo);
614 //body->setContactProcessingThreshold(m_defaultContactProcessingThreshold);
615
616 #else
617 btRigidBody* body = new btRigidBody(mass, 0, shape, localInertia);
618 body->setWorldTransform(startTransform);
619 #endif //
620
621 m_dynamicsWorld->addRigidBody(body);
622 return body;
623 }
624
Hinge2VehicleCreateFunc(struct CommonExampleOptions & options)625 CommonExampleInterface* Hinge2VehicleCreateFunc(struct CommonExampleOptions& options)
626 {
627 return new Hinge2Vehicle(options.m_guiHelper);
628 }
629