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