1 
2 #ifndef COMMON_MULTI_BODY_SETUP_H
3 #define COMMON_MULTI_BODY_SETUP_H
4 
5 #include "btBulletDynamicsCommon.h"
6 
7 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
8 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
9 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
10 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
11 
12 #include "btBulletDynamicsCommon.h"
13 #include "CommonExampleInterface.h"
14 #include "CommonGUIHelperInterface.h"
15 #include "CommonRenderInterface.h"
16 #include "CommonGraphicsAppInterface.h"
17 #include "CommonWindowInterface.h"
18 #include "CommonCameraInterface.h"
19 
20 enum MyFilterModes
21 {
22 	FILTER_GROUPAMASKB_AND_GROUPBMASKA2 = 0,
23 	FILTER_GROUPAMASKB_OR_GROUPBMASKA2
24 };
25 
26 struct MyOverlapFilterCallback2 : public btOverlapFilterCallback
27 {
28 	int m_filterMode;
29 
MyOverlapFilterCallback2MyOverlapFilterCallback230 	MyOverlapFilterCallback2()
31 		: m_filterMode(FILTER_GROUPAMASKB_AND_GROUPBMASKA2)
32 	{
33 	}
34 
~MyOverlapFilterCallback2MyOverlapFilterCallback235 	virtual ~MyOverlapFilterCallback2()
36 	{
37 	}
38 	// return true when pairs need collision
needBroadphaseCollisionMyOverlapFilterCallback239 	virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
40 	{
41 		if (m_filterMode == FILTER_GROUPAMASKB_AND_GROUPBMASKA2)
42 		{
43 			bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
44 			collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
45 			return collides;
46 		}
47 
48 		if (m_filterMode == FILTER_GROUPAMASKB_OR_GROUPBMASKA2)
49 		{
50 			bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
51 			collides = collides || (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
52 			return collides;
53 		}
54 		return false;
55 	}
56 };
57 
58 struct CommonMultiBodyBase : public CommonExampleInterface
59 {
60 	//keep the collision shapes, for deletion/cleanup
61 	btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
62 	MyOverlapFilterCallback2* m_filterCallback;
63 	btOverlappingPairCache* m_pairCache;
64 	btBroadphaseInterface* m_broadphase;
65 	btCollisionDispatcher* m_dispatcher;
66 	btMultiBodyConstraintSolver* m_solver;
67 	btDefaultCollisionConfiguration* m_collisionConfiguration;
68 	btMultiBodyDynamicsWorld* m_dynamicsWorld;
69 
70 	//data for picking objects
71 	class btRigidBody* m_pickedBody;
72 	class btTypedConstraint* m_pickedConstraint;
73 	class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
74 
75 	btVector3 m_oldPickingPos;
76 	btVector3 m_hitPos;
77 	btScalar m_oldPickingDist;
78 	bool m_prevCanSleep;
79 
80 	struct GUIHelperInterface* m_guiHelper;
81 
CommonMultiBodyBaseCommonMultiBodyBase82 	CommonMultiBodyBase(GUIHelperInterface* helper)
83 		: m_filterCallback(0),
84 		  m_pairCache(0),
85 		  m_broadphase(0),
86 		  m_dispatcher(0),
87 		  m_solver(0),
88 		  m_collisionConfiguration(0),
89 		  m_dynamicsWorld(0),
90 		  m_pickedBody(0),
91 		  m_pickedConstraint(0),
92 		  m_pickingMultiBodyPoint2Point(0),
93 		  m_prevCanSleep(false),
94 		  m_guiHelper(helper)
95 	{
96 	}
97 
createEmptyDynamicsWorldCommonMultiBodyBase98 	virtual void createEmptyDynamicsWorld()
99 	{
100 		///collision configuration contains default setup for memory, collision setup
101 		m_collisionConfiguration = new btDefaultCollisionConfiguration();
102 		//m_collisionConfiguration->setConvexConvexMultipointIterations();
103 		m_filterCallback = new MyOverlapFilterCallback2();
104 
105 		///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
106 		m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
107 
108 		m_pairCache = new btHashedOverlappingPairCache();
109 
110 		m_pairCache->setOverlapFilterCallback(m_filterCallback);
111 
112 		m_broadphase = new btDbvtBroadphase(m_pairCache);  //btSimpleBroadphase();
113 
114 		m_solver = new btMultiBodyConstraintSolver;
115 
116 		m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
117 
118 		m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
119 	}
120 
stepSimulationCommonMultiBodyBase121 	virtual void stepSimulation(float deltaTime)
122 	{
123 		if (m_dynamicsWorld)
124 		{
125 			m_dynamicsWorld->stepSimulation(deltaTime);
126 		}
127 	}
128 
exitPhysicsCommonMultiBodyBase129 	virtual void exitPhysics()
130 	{
131 		removePickingConstraint();
132 		//cleanup in the reverse order of creation/initialization
133 
134 		//remove the rigidbodies from the dynamics world and delete them
135 
136 		if (m_dynamicsWorld)
137 		{
138 			int i;
139 			for (i = m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
140 			{
141 				m_dynamicsWorld->removeConstraint(m_dynamicsWorld->getConstraint(i));
142 			}
143 
144 			for (i = m_dynamicsWorld->getNumMultiBodyConstraints() - 1; i >= 0; i--)
145 			{
146 				btMultiBodyConstraint* mbc = m_dynamicsWorld->getMultiBodyConstraint(i);
147 				m_dynamicsWorld->removeMultiBodyConstraint(mbc);
148 				delete mbc;
149 			}
150 
151 			for (i = m_dynamicsWorld->getNumMultibodies() - 1; i >= 0; i--)
152 			{
153 				btMultiBody* mb = m_dynamicsWorld->getMultiBody(i);
154 				m_dynamicsWorld->removeMultiBody(mb);
155 				delete mb;
156 			}
157 			for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
158 			{
159 				btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
160 				btRigidBody* body = btRigidBody::upcast(obj);
161 				if (body && body->getMotionState())
162 				{
163 					delete body->getMotionState();
164 				}
165 				m_dynamicsWorld->removeCollisionObject(obj);
166 				delete obj;
167 			}
168 		}
169 		//delete collision shapes
170 		for (int j = 0; j < m_collisionShapes.size(); j++)
171 		{
172 			btCollisionShape* shape = m_collisionShapes[j];
173 			delete shape;
174 		}
175 		m_collisionShapes.clear();
176 
177 		delete m_dynamicsWorld;
178 		m_dynamicsWorld = 0;
179 
180 		delete m_solver;
181 		m_solver = 0;
182 
183 		delete m_broadphase;
184 		m_broadphase = 0;
185 
186 		delete m_dispatcher;
187 		m_dispatcher = 0;
188 
189 		delete m_pairCache;
190 		m_pairCache = 0;
191 
192 		delete m_filterCallback;
193 		m_filterCallback = 0;
194 
195 		delete m_collisionConfiguration;
196 		m_collisionConfiguration = 0;
197 	}
198 
syncPhysicsToGraphicsCommonMultiBodyBase199 	virtual void syncPhysicsToGraphics()
200 	{
201 		if (m_dynamicsWorld)
202 		{
203 			m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
204 		}
205 	}
206 
renderSceneCommonMultiBodyBase207 	virtual void renderScene()
208 	{
209 		if (m_dynamicsWorld)
210 		{
211 			m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
212 
213 			m_guiHelper->render(m_dynamicsWorld);
214 		}
215 	}
216 
physicsDebugDrawCommonMultiBodyBase217 	virtual void physicsDebugDraw(int debugDrawFlags)
218 	{
219 		if (m_dynamicsWorld)
220 		{
221 			if (m_dynamicsWorld->getDebugDrawer())
222 			{
223 				m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
224 			}
225 			m_dynamicsWorld->debugDrawWorld();
226 		}
227 	}
228 
keyboardCallbackCommonMultiBodyBase229 	virtual bool keyboardCallback(int key, int state)
230 	{
231 		if ((key == B3G_F3) && state && m_dynamicsWorld)
232 		{
233 			btDefaultSerializer* serializer = new btDefaultSerializer();
234 			m_dynamicsWorld->serialize(serializer);
235 
236 			FILE* file = fopen("testFile.bullet", "wb");
237 			fwrite(serializer->getBufferPointer(), serializer->getCurrentBufferSize(), 1, file);
238 			fclose(file);
239 			//b3Printf("btDefaultSerializer wrote testFile.bullet");
240 			delete serializer;
241 			return true;
242 		}
243 		return false;  //don't handle this key
244 	}
245 
getRayToCommonMultiBodyBase246 	btVector3 getRayTo(int x, int y)
247 	{
248 		CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
249 
250 		if (!renderer)
251 		{
252 			btAssert(0);
253 			return btVector3(0, 0, 0);
254 		}
255 
256 		float top = 1.f;
257 		float bottom = -1.f;
258 		float nearPlane = 1.f;
259 		float tanFov = (top - bottom) * 0.5f / nearPlane;
260 		float fov = btScalar(2.0) * btAtan(tanFov);
261 
262 		btVector3 camPos, camTarget;
263 		renderer->getActiveCamera()->getCameraPosition(camPos);
264 		renderer->getActiveCamera()->getCameraTargetPosition(camTarget);
265 
266 		btVector3 rayFrom = camPos;
267 		btVector3 rayForward = (camTarget - camPos);
268 		rayForward.normalize();
269 		float farPlane = 10000.f;
270 		rayForward *= farPlane;
271 
272 		btVector3 rightOffset;
273 		btVector3 cameraUp = btVector3(0, 0, 0);
274 		cameraUp[m_guiHelper->getAppInterface()->getUpAxis()] = 1;
275 
276 		btVector3 vertical = cameraUp;
277 
278 		btVector3 hor;
279 		hor = rayForward.cross(vertical);
280 		hor.normalize();
281 		vertical = hor.cross(rayForward);
282 		vertical.normalize();
283 
284 		float tanfov = tanf(0.5f * fov);
285 
286 		hor *= 2.f * farPlane * tanfov;
287 		vertical *= 2.f * farPlane * tanfov;
288 
289 		btScalar aspect;
290 		float width = float(renderer->getScreenWidth());
291 		float height = float(renderer->getScreenHeight());
292 
293 		aspect = width / height;
294 
295 		hor *= aspect;
296 
297 		btVector3 rayToCenter = rayFrom + rayForward;
298 		btVector3 dHor = hor * 1.f / width;
299 		btVector3 dVert = vertical * 1.f / height;
300 
301 		btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
302 		rayTo += btScalar(x) * dHor;
303 		rayTo -= btScalar(y) * dVert;
304 		return rayTo;
305 	}
306 
mouseMoveCallbackCommonMultiBodyBase307 	virtual bool mouseMoveCallback(float x, float y)
308 	{
309 		CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
310 
311 		if (!renderer)
312 		{
313 			btAssert(0);
314 			return false;
315 		}
316 
317 		btVector3 rayTo = getRayTo(int(x), int(y));
318 		btVector3 rayFrom;
319 		renderer->getActiveCamera()->getCameraPosition(rayFrom);
320 		movePickedBody(rayFrom, rayTo);
321 
322 		return false;
323 	}
324 
mouseButtonCallbackCommonMultiBodyBase325 	virtual bool mouseButtonCallback(int button, int state, float x, float y)
326 	{
327 		CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
328 
329 		if (!renderer)
330 		{
331 			btAssert(0);
332 			return false;
333 		}
334 
335 		CommonWindowInterface* window = m_guiHelper->getAppInterface()->m_window;
336 
337 		if (state == 1)
338 		{
339 			if (button == 0 && (!window->isModifierKeyPressed(B3G_ALT) && !window->isModifierKeyPressed(B3G_CONTROL)))
340 			{
341 				btVector3 camPos;
342 				renderer->getActiveCamera()->getCameraPosition(camPos);
343 
344 				btVector3 rayFrom = camPos;
345 				btVector3 rayTo = getRayTo(int(x), int(y));
346 
347 				pickBody(rayFrom, rayTo);
348 			}
349 		}
350 		else
351 		{
352 			if (button == 0)
353 			{
354 				removePickingConstraint();
355 				//remove p2p
356 			}
357 		}
358 
359 		//printf("button=%d, state=%d\n",button,state);
360 		return false;
361 	}
362 
pickBodyCommonMultiBodyBase363 	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
364 	{
365 		if (m_dynamicsWorld == 0)
366 			return false;
367 
368 		btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld);
369 
370 		m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback);
371 		if (rayCallback.hasHit())
372 		{
373 			btVector3 pickPos = rayCallback.m_hitPointWorld;
374 			btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
375 			if (body)
376 			{
377 				//other exclusions?
378 				if (!(body->isStaticObject() || body->isKinematicObject()))
379 				{
380 					m_pickedBody = body;
381 					m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
382 					//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
383 					btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
384 					btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot);
385 					m_dynamicsWorld->addConstraint(p2p, true);
386 					m_pickedConstraint = p2p;
387 					btScalar mousePickClamping = 30.f;
388 					p2p->m_setting.m_impulseClamp = mousePickClamping;
389 					//very weak constraint for picking
390 					p2p->m_setting.m_tau = 0.001f;
391 				}
392 			}
393 			else
394 			{
395 				btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
396 				if (multiCol && multiCol->m_multiBody)
397 				{
398 					m_prevCanSleep = multiCol->m_multiBody->getCanSleep();
399 					multiCol->m_multiBody->setCanSleep(false);
400 
401 					btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos);
402 
403 					btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody, multiCol->m_link, 0, pivotInA, pickPos);
404 					//if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
405 					//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
406 					//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
407 					//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
408 					btScalar scaling = 1;
409 					p2p->setMaxAppliedImpulse(2 * scaling);
410 
411 					btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_dynamicsWorld;
412 					world->addMultiBodyConstraint(p2p);
413 					m_pickingMultiBodyPoint2Point = p2p;
414 				}
415 			}
416 
417 			//					pickObject(pickPos, rayCallback.m_collisionObject);
418 			m_oldPickingPos = rayToWorld;
419 			m_hitPos = pickPos;
420 			m_oldPickingDist = (pickPos - rayFromWorld).length();
421 			//					printf("hit !\n");
422 			//add p2p
423 		}
424 		return false;
425 	}
movePickedBodyCommonMultiBodyBase426 	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
427 	{
428 		if (m_pickedBody && m_pickedConstraint)
429 		{
430 			btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_pickedConstraint);
431 			if (pickCon)
432 			{
433 				//keep it at the same picking distance
434 
435 				btVector3 dir = rayToWorld - rayFromWorld;
436 				dir.normalize();
437 				dir *= m_oldPickingDist;
438 
439 				btVector3 newPivotB = rayFromWorld + dir;
440 				pickCon->setPivotB(newPivotB);
441 			}
442 		}
443 
444 		if (m_pickingMultiBodyPoint2Point)
445 		{
446 			//keep it at the same picking distance
447 
448 			btVector3 dir = rayToWorld - rayFromWorld;
449 			dir.normalize();
450 			dir *= m_oldPickingDist;
451 
452 			btVector3 newPivotB = rayFromWorld + dir;
453 
454 			m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
455 		}
456 
457 		return false;
458 	}
459 
removePickingConstraintCommonMultiBodyBase460 	virtual void removePickingConstraint()
461 	{
462 		if (m_pickedConstraint)
463 		{
464 			m_dynamicsWorld->removeConstraint(m_pickedConstraint);
465 
466 			if (m_pickedBody)
467 			{
468 				m_pickedBody->forceActivationState(ACTIVE_TAG);
469 				m_pickedBody->activate(true);
470 			}
471 			delete m_pickedConstraint;
472 			m_pickedConstraint = 0;
473 			m_pickedBody = 0;
474 		}
475 		if (m_pickingMultiBodyPoint2Point)
476 		{
477 			m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_prevCanSleep);
478 			btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_dynamicsWorld;
479 			world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point);
480 			delete m_pickingMultiBodyPoint2Point;
481 			m_pickingMultiBodyPoint2Point = 0;
482 		}
483 	}
484 
createBoxShapeCommonMultiBodyBase485 	btBoxShape* createBoxShape(const btVector3& halfExtents)
486 	{
487 		btBoxShape* box = new btBoxShape(halfExtents);
488 		return box;
489 	}
490 
491 	btRigidBody* createRigidBody(float mass, const btTransform& startTransform, btCollisionShape* shape, const btVector4& color = btVector4(1, 0, 0, 1))
492 	{
493 		btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));
494 
495 		//rigidbody is dynamic if and only if mass is non zero, otherwise static
496 		bool isDynamic = (mass != 0.f);
497 
498 		btVector3 localInertia(0, 0, 0);
499 		if (isDynamic)
500 			shape->calculateLocalInertia(mass, localInertia);
501 
502 			//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
503 
504 #define USE_MOTIONSTATE 1
505 #ifdef USE_MOTIONSTATE
506 		btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
507 
508 		btRigidBody::btRigidBodyConstructionInfo cInfo(mass, myMotionState, shape, localInertia);
509 
510 		btRigidBody* body = new btRigidBody(cInfo);
511 		//body->setContactProcessingThreshold(m_defaultContactProcessingThreshold);
512 
513 #else
514 		btRigidBody* body = new btRigidBody(mass, 0, shape, localInertia);
515 		body->setWorldTransform(startTransform);
516 #endif  //
517 
518 		body->setUserIndex(-1);
519 		m_dynamicsWorld->addRigidBody(body);
520 		return body;
521 	}
522 };
523 
524 #endif  //COMMON_MULTI_BODY_SETUP_H
525