1 /* 2 This source file is part of GIMPACT Library. 3 4 For the latest info, see http://gimpact.sourceforge.net/ 5 6 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. 7 email: projectileman (at) yahoo.com 8 9 10 This software is provided 'as-is', without any express or implied warranty. 11 In no event will the authors be held liable for any damages arising from the use of this software. 12 Permission is granted to anyone to use this software for any purpose, 13 including commercial applications, and to alter it and redistribute it freely, 14 subject to the following restrictions: 15 16 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. 17 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 18 3. This notice may not be removed or altered from any source distribution. 19 */ 20 /* 21 Author: Francisco Len Njera 22 Concave-Concave Collision 23 24 */ 25 26 #include "BulletCollision/CollisionDispatch/btManifoldResult.h" 27 #include "LinearMath/btIDebugDraw.h" 28 #include "BulletCollision/CollisionDispatch/btCollisionObject.h" 29 #include "BulletCollision/CollisionShapes/btBoxShape.h" 30 #include "btGImpactCollisionAlgorithm.h" 31 #include "btContactProcessing.h" 32 #include "LinearMath/btQuickprof.h" 33 34 35 //! Class for accessing the plane equation 36 class btPlaneShape : public btStaticPlaneShape 37 { 38 public: 39 40 btPlaneShape(const btVector3& v, float f) 41 :btStaticPlaneShape(v,f) 42 { 43 } 44 45 void get_plane_equation(btVector4 &equation) 46 { 47 equation[0] = m_planeNormal[0]; 48 equation[1] = m_planeNormal[1]; 49 equation[2] = m_planeNormal[2]; 50 equation[3] = m_planeConstant; 51 } 52 53 54 void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation) const 55 { 56 equation[0] = trans.getBasis().getRow(0).dot(m_planeNormal); 57 equation[1] = trans.getBasis().getRow(1).dot(m_planeNormal); 58 equation[2] = trans.getBasis().getRow(2).dot(m_planeNormal); 59 equation[3] = trans.getOrigin().dot(m_planeNormal) + m_planeConstant; 60 } 61 }; 62 63 64 65 ////////////////////////////////////////////////////////////////////////////////////////////// 66 #ifdef TRI_COLLISION_PROFILING 67 68 btClock g_triangle_clock; 69 70 float g_accum_triangle_collision_time = 0; 71 int g_count_triangle_collision = 0; 72 73 void bt_begin_gim02_tri_time() 74 { 75 g_triangle_clock.reset(); 76 } 77 78 void bt_end_gim02_tri_time() 79 { 80 g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds(); 81 g_count_triangle_collision++; 82 } 83 #endif //TRI_COLLISION_PROFILING 84 //! Retrieving shapes shapes 85 /*! 86 Declared here due of insuficent space on Pool allocators 87 */ 88 //!@{ 89 class GIM_ShapeRetriever 90 { 91 public: 92 const btGImpactShapeInterface * m_gim_shape; 93 btTriangleShapeEx m_trishape; 94 btTetrahedronShapeEx m_tetrashape; 95 96 public: 97 class ChildShapeRetriever 98 { 99 public: 100 GIM_ShapeRetriever * m_parent; 101 virtual const btCollisionShape * getChildShape(int index) 102 { 103 return m_parent->m_gim_shape->getChildShape(index); 104 } 105 virtual ~ChildShapeRetriever() {} 106 }; 107 108 class TriangleShapeRetriever:public ChildShapeRetriever 109 { 110 public: 111 112 virtual btCollisionShape * getChildShape(int index) 113 { 114 m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape); 115 return &m_parent->m_trishape; 116 } 117 virtual ~TriangleShapeRetriever() {} 118 }; 119 120 class TetraShapeRetriever:public ChildShapeRetriever 121 { 122 public: 123 124 virtual btCollisionShape * getChildShape(int index) 125 { 126 m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape); 127 return &m_parent->m_tetrashape; 128 } 129 }; 130 public: 131 ChildShapeRetriever m_child_retriever; 132 TriangleShapeRetriever m_tri_retriever; 133 TetraShapeRetriever m_tetra_retriever; 134 ChildShapeRetriever * m_current_retriever; 135 136 GIM_ShapeRetriever(const btGImpactShapeInterface * gim_shape) 137 { 138 m_gim_shape = gim_shape; 139 //select retriever 140 if(m_gim_shape->needsRetrieveTriangles()) 141 { 142 m_current_retriever = &m_tri_retriever; 143 } 144 else if(m_gim_shape->needsRetrieveTetrahedrons()) 145 { 146 m_current_retriever = &m_tetra_retriever; 147 } 148 else 149 { 150 m_current_retriever = &m_child_retriever; 151 } 152 153 m_current_retriever->m_parent = this; 154 } 155 156 const btCollisionShape * getChildShape(int index) 157 { 158 return m_current_retriever->getChildShape(index); 159 } 160 161 162 }; 163 164 165 166 //!@} 167 168 169 #ifdef TRI_COLLISION_PROFILING 170 171 //! Gets the average time in miliseconds of tree collisions 172 float btGImpactCollisionAlgorithm::getAverageTreeCollisionTime() 173 { 174 return btGImpactBoxSet::getAverageTreeCollisionTime(); 175 176 } 177 178 //! Gets the average time in miliseconds of triangle collisions 179 float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime() 180 { 181 if(g_count_triangle_collision == 0) return 0; 182 183 float avgtime = g_accum_triangle_collision_time; 184 avgtime /= (float)g_count_triangle_collision; 185 186 g_accum_triangle_collision_time = 0; 187 g_count_triangle_collision = 0; 188 189 return avgtime; 190 } 191 192 #endif //TRI_COLLISION_PROFILING 193 194 195 196 btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) 197 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap) 198 { 199 m_manifoldPtr = NULL; 200 m_convex_algorithm = NULL; 201 } 202 203 btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm() 204 { 205 clearCache(); 206 } 207 208 209 210 211 212 void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper * body0Wrap, 213 const btCollisionObjectWrapper * body1Wrap, 214 const btVector3 & point, 215 const btVector3 & normal, 216 btScalar distance) 217 { 218 m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); 219 m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); 220 checkManifold(body0Wrap,body1Wrap); 221 m_resultOut->addContactPoint(normal,point,distance); 222 } 223 224 225 void btGImpactCollisionAlgorithm::shape_vs_shape_collision( 226 const btCollisionObjectWrapper * body0Wrap, 227 const btCollisionObjectWrapper* body1Wrap, 228 const btCollisionShape * shape0, 229 const btCollisionShape * shape1) 230 { 231 232 233 { 234 235 btCollisionAlgorithm* algor = newAlgorithm(body0Wrap,body1Wrap); 236 // post : checkManifold is called 237 238 m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); 239 m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); 240 241 algor->processCollision(body0Wrap,body1Wrap,*m_dispatchInfo,m_resultOut); 242 243 algor->~btCollisionAlgorithm(); 244 m_dispatcher->freeCollisionAlgorithm(algor); 245 } 246 247 } 248 249 void btGImpactCollisionAlgorithm::convex_vs_convex_collision( 250 const btCollisionObjectWrapper* body0Wrap, 251 const btCollisionObjectWrapper* body1Wrap, 252 const btCollisionShape* shape0, 253 const btCollisionShape* shape1) 254 { 255 256 m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); 257 m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); 258 259 btCollisionObjectWrapper ob0(body0Wrap,shape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0); 260 btCollisionObjectWrapper ob1(body1Wrap,shape1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),m_part1,m_triface1); 261 checkConvexAlgorithm(&ob0,&ob1); 262 m_convex_algorithm->processCollision(&ob0,&ob1,*m_dispatchInfo,m_resultOut); 263 264 265 } 266 267 268 269 270 void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs( 271 const btTransform & trans0, 272 const btTransform & trans1, 273 const btGImpactShapeInterface * shape0, 274 const btGImpactShapeInterface * shape1,btPairSet & pairset) 275 { 276 if(shape0->hasBoxSet() && shape1->hasBoxSet()) 277 { 278 btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset); 279 } 280 else 281 { 282 btAABB boxshape0; 283 btAABB boxshape1; 284 int i = shape0->getNumChildShapes(); 285 286 while(i--) 287 { 288 shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); 289 290 int j = shape1->getNumChildShapes(); 291 while(j--) 292 { 293 shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max); 294 295 if(boxshape1.has_collision(boxshape0)) 296 { 297 pairset.push_pair(i,j); 298 } 299 } 300 } 301 } 302 303 304 } 305 306 307 void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs( 308 const btTransform & trans0, 309 const btTransform & trans1, 310 const btGImpactShapeInterface * shape0, 311 const btCollisionShape * shape1, 312 btAlignedObjectArray<int> & collided_primitives) 313 { 314 315 btAABB boxshape; 316 317 318 if(shape0->hasBoxSet()) 319 { 320 btTransform trans1to0 = trans0.inverse(); 321 trans1to0 *= trans1; 322 323 shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max); 324 325 shape0->getBoxSet()->boxQuery(boxshape, collided_primitives); 326 } 327 else 328 { 329 shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max); 330 331 btAABB boxshape0; 332 int i = shape0->getNumChildShapes(); 333 334 while(i--) 335 { 336 shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); 337 338 if(boxshape.has_collision(boxshape0)) 339 { 340 collided_primitives.push_back(i); 341 } 342 } 343 344 } 345 346 } 347 348 349 void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper * body0Wrap, 350 const btCollisionObjectWrapper * body1Wrap, 351 const btGImpactMeshShapePart * shape0, 352 const btGImpactMeshShapePart * shape1, 353 const int * pairs, int pair_count) 354 { 355 btTriangleShapeEx tri0; 356 btTriangleShapeEx tri1; 357 358 shape0->lockChildShapes(); 359 shape1->lockChildShapes(); 360 361 const int * pair_pointer = pairs; 362 363 while(pair_count--) 364 { 365 366 m_triface0 = *(pair_pointer); 367 m_triface1 = *(pair_pointer+1); 368 pair_pointer+=2; 369 370 371 372 shape0->getBulletTriangle(m_triface0,tri0); 373 shape1->getBulletTriangle(m_triface1,tri1); 374 375 376 //collide two convex shapes 377 if(tri0.overlap_test_conservative(tri1)) 378 { 379 convex_vs_convex_collision(body0Wrap,body1Wrap,&tri0,&tri1); 380 } 381 382 } 383 384 shape0->unlockChildShapes(); 385 shape1->unlockChildShapes(); 386 } 387 388 void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap, 389 const btCollisionObjectWrapper* body1Wrap, 390 const btGImpactMeshShapePart * shape0, 391 const btGImpactMeshShapePart * shape1, 392 const int * pairs, int pair_count) 393 { 394 btTransform orgtrans0 = body0Wrap->getWorldTransform(); 395 btTransform orgtrans1 = body1Wrap->getWorldTransform(); 396 397 btPrimitiveTriangle ptri0; 398 btPrimitiveTriangle ptri1; 399 GIM_TRIANGLE_CONTACT contact_data; 400 401 shape0->lockChildShapes(); 402 shape1->lockChildShapes(); 403 404 const int * pair_pointer = pairs; 405 406 while(pair_count--) 407 { 408 409 m_triface0 = *(pair_pointer); 410 m_triface1 = *(pair_pointer+1); 411 pair_pointer+=2; 412 413 414 shape0->getPrimitiveTriangle(m_triface0,ptri0); 415 shape1->getPrimitiveTriangle(m_triface1,ptri1); 416 417 #ifdef TRI_COLLISION_PROFILING 418 bt_begin_gim02_tri_time(); 419 #endif 420 421 ptri0.applyTransform(orgtrans0); 422 ptri1.applyTransform(orgtrans1); 423 424 425 //build planes 426 ptri0.buildTriPlane(); 427 ptri1.buildTriPlane(); 428 // test conservative 429 430 431 432 if(ptri0.overlap_test_conservative(ptri1)) 433 { 434 if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data)) 435 { 436 437 int j = contact_data.m_point_count; 438 while(j--) 439 { 440 441 addContactPoint(body0Wrap, body1Wrap, 442 contact_data.m_points[j], 443 contact_data.m_separating_normal, 444 -contact_data.m_penetration_depth); 445 } 446 } 447 } 448 449 #ifdef TRI_COLLISION_PROFILING 450 bt_end_gim02_tri_time(); 451 #endif 452 453 } 454 455 shape0->unlockChildShapes(); 456 shape1->unlockChildShapes(); 457 458 } 459 460 461 void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( 462 const btCollisionObjectWrapper* body0Wrap, 463 const btCollisionObjectWrapper * body1Wrap, 464 const btGImpactShapeInterface * shape0, 465 const btGImpactShapeInterface * shape1) 466 { 467 468 if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) 469 { 470 const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0); 471 m_part0 = meshshape0->getMeshPartCount(); 472 473 while(m_part0--) 474 { 475 gimpact_vs_gimpact(body0Wrap,body1Wrap,meshshape0->getMeshPart(m_part0),shape1); 476 } 477 478 return; 479 } 480 481 if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) 482 { 483 const btGImpactMeshShape * meshshape1 = static_cast<const btGImpactMeshShape *>(shape1); 484 m_part1 = meshshape1->getMeshPartCount(); 485 486 while(m_part1--) 487 { 488 489 gimpact_vs_gimpact(body0Wrap,body1Wrap,shape0,meshshape1->getMeshPart(m_part1)); 490 491 } 492 493 return; 494 } 495 496 497 btTransform orgtrans0 = body0Wrap->getWorldTransform(); 498 btTransform orgtrans1 = body1Wrap->getWorldTransform(); 499 500 btPairSet pairset; 501 502 gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset); 503 504 if(pairset.size()== 0) return; 505 506 if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && 507 shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART) 508 { 509 const btGImpactMeshShapePart * shapepart0 = static_cast<const btGImpactMeshShapePart * >(shape0); 510 const btGImpactMeshShapePart * shapepart1 = static_cast<const btGImpactMeshShapePart * >(shape1); 511 //specialized function 512 #ifdef BULLET_TRIANGLE_COLLISION 513 collide_gjk_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); 514 #else 515 collide_sat_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); 516 #endif 517 518 return; 519 } 520 521 //general function 522 523 shape0->lockChildShapes(); 524 shape1->lockChildShapes(); 525 526 GIM_ShapeRetriever retriever0(shape0); 527 GIM_ShapeRetriever retriever1(shape1); 528 529 bool child_has_transform0 = shape0->childrenHasTransform(); 530 bool child_has_transform1 = shape1->childrenHasTransform(); 531 532 int i = pairset.size(); 533 while(i--) 534 { 535 GIM_PAIR * pair = &pairset[i]; 536 m_triface0 = pair->m_index1; 537 m_triface1 = pair->m_index2; 538 const btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0); 539 const btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1); 540 541 btTransform tr0 = body0Wrap->getWorldTransform(); 542 btTransform tr1 = body1Wrap->getWorldTransform(); 543 544 if(child_has_transform0) 545 { 546 tr0 = orgtrans0*shape0->getChildTransform(m_triface0); 547 } 548 549 if(child_has_transform1) 550 { 551 tr1 = orgtrans1*shape1->getChildTransform(m_triface1); 552 } 553 554 btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),tr0,m_part0,m_triface0); 555 btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),tr1,m_part1,m_triface1); 556 557 //collide two convex shapes 558 convex_vs_convex_collision(&ob0,&ob1,colshape0,colshape1); 559 } 560 561 shape0->unlockChildShapes(); 562 shape1->unlockChildShapes(); 563 } 564 565 void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap, 566 const btCollisionObjectWrapper * body1Wrap, 567 const btGImpactShapeInterface * shape0, 568 const btCollisionShape * shape1,bool swapped) 569 { 570 if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) 571 { 572 const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0); 573 int& part = swapped ? m_part1 : m_part0; 574 part = meshshape0->getMeshPartCount(); 575 576 while(part--) 577 { 578 579 gimpact_vs_shape(body0Wrap, 580 body1Wrap, 581 meshshape0->getMeshPart(part), 582 shape1,swapped); 583 584 } 585 586 return; 587 } 588 589 #ifdef GIMPACT_VS_PLANE_COLLISION 590 if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && 591 shape1->getShapeType() == STATIC_PLANE_PROXYTYPE) 592 { 593 const btGImpactMeshShapePart * shapepart = static_cast<const btGImpactMeshShapePart *>(shape0); 594 const btStaticPlaneShape * planeshape = static_cast<const btStaticPlaneShape * >(shape1); 595 gimpacttrimeshpart_vs_plane_collision(body0Wrap,body1Wrap,shapepart,planeshape,swapped); 596 return; 597 } 598 599 #endif 600 601 602 603 if(shape1->isCompound()) 604 { 605 const btCompoundShape * compoundshape = static_cast<const btCompoundShape *>(shape1); 606 gimpact_vs_compoundshape(body0Wrap,body1Wrap,shape0,compoundshape,swapped); 607 return; 608 } 609 else if(shape1->isConcave()) 610 { 611 const btConcaveShape * concaveshape = static_cast<const btConcaveShape *>(shape1); 612 gimpact_vs_concave(body0Wrap,body1Wrap,shape0,concaveshape,swapped); 613 return; 614 } 615 616 617 btTransform orgtrans0 = body0Wrap->getWorldTransform(); 618 619 btTransform orgtrans1 = body1Wrap->getWorldTransform(); 620 621 btAlignedObjectArray<int> collided_results; 622 623 gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results); 624 625 if(collided_results.size() == 0) return; 626 627 628 shape0->lockChildShapes(); 629 630 GIM_ShapeRetriever retriever0(shape0); 631 632 633 bool child_has_transform0 = shape0->childrenHasTransform(); 634 635 636 int i = collided_results.size(); 637 638 while(i--) 639 { 640 int child_index = collided_results[i]; 641 if(swapped) 642 m_triface1 = child_index; 643 else 644 m_triface0 = child_index; 645 646 const btCollisionShape * colshape0 = retriever0.getChildShape(child_index); 647 648 btTransform tr0 = body0Wrap->getWorldTransform(); 649 650 if(child_has_transform0) 651 { 652 tr0 = orgtrans0*shape0->getChildTransform(child_index); 653 } 654 655 btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0); 656 const btCollisionObjectWrapper* prevObj0 = m_resultOut->getBody0Wrap(); 657 658 if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob0.getCollisionObject()) 659 { 660 m_resultOut->setBody0Wrap(&ob0); 661 } else 662 { 663 m_resultOut->setBody1Wrap(&ob0); 664 } 665 666 //collide two shapes 667 if(swapped) 668 { 669 670 shape_vs_shape_collision(body1Wrap,&ob0,shape1,colshape0); 671 } 672 else 673 { 674 675 shape_vs_shape_collision(&ob0,body1Wrap,colshape0,shape1); 676 } 677 m_resultOut->setBody0Wrap(prevObj0); 678 679 } 680 681 shape0->unlockChildShapes(); 682 683 } 684 685 void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap, 686 const btCollisionObjectWrapper* body1Wrap, 687 const btGImpactShapeInterface * shape0, 688 const btCompoundShape * shape1,bool swapped) 689 { 690 btTransform orgtrans1 = body1Wrap->getWorldTransform(); 691 692 int i = shape1->getNumChildShapes(); 693 while(i--) 694 { 695 696 const btCollisionShape * colshape1 = shape1->getChildShape(i); 697 btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i); 698 699 btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),childtrans1,-1,i); 700 701 const btCollisionObjectWrapper* tmp = 0; 702 if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject()) 703 { 704 tmp = m_resultOut->getBody0Wrap(); 705 m_resultOut->setBody0Wrap(&ob1); 706 } else 707 { 708 tmp = m_resultOut->getBody1Wrap(); 709 m_resultOut->setBody1Wrap(&ob1); 710 } 711 //collide child shape 712 gimpact_vs_shape(body0Wrap, &ob1, 713 shape0,colshape1,swapped); 714 715 if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject()) 716 { 717 m_resultOut->setBody0Wrap(tmp); 718 } else 719 { 720 m_resultOut->setBody1Wrap(tmp); 721 } 722 } 723 } 724 725 void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( 726 const btCollisionObjectWrapper * body0Wrap, 727 const btCollisionObjectWrapper * body1Wrap, 728 const btGImpactMeshShapePart * shape0, 729 const btStaticPlaneShape * shape1,bool swapped) 730 { 731 732 733 btTransform orgtrans0 = body0Wrap->getWorldTransform(); 734 btTransform orgtrans1 = body1Wrap->getWorldTransform(); 735 736 const btPlaneShape * planeshape = static_cast<const btPlaneShape *>(shape1); 737 btVector4 plane; 738 planeshape->get_plane_equation_transformed(orgtrans1,plane); 739 740 //test box against plane 741 742 btAABB tribox; 743 shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max); 744 tribox.increment_margin(planeshape->getMargin()); 745 746 if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return; 747 748 shape0->lockChildShapes(); 749 750 btScalar margin = shape0->getMargin() + planeshape->getMargin(); 751 752 btVector3 vertex; 753 int vi = shape0->getVertexCount(); 754 while(vi--) 755 { 756 shape0->getVertex(vi,vertex); 757 vertex = orgtrans0(vertex); 758 759 btScalar distance = vertex.dot(plane) - plane[3] - margin; 760 761 if(distance<0.0)//add contact 762 { 763 if(swapped) 764 { 765 addContactPoint(body1Wrap, body0Wrap, 766 vertex, 767 -plane, 768 distance); 769 } 770 else 771 { 772 addContactPoint(body0Wrap, body1Wrap, 773 vertex, 774 plane, 775 distance); 776 } 777 } 778 } 779 780 shape0->unlockChildShapes(); 781 } 782 783 784 785 786 class btGImpactTriangleCallback: public btTriangleCallback 787 { 788 public: 789 btGImpactCollisionAlgorithm * algorithm; 790 const btCollisionObjectWrapper * body0Wrap; 791 const btCollisionObjectWrapper * body1Wrap; 792 const btGImpactShapeInterface * gimpactshape0; 793 bool swapped; 794 btScalar margin; 795 796 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) 797 { 798 btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]); 799 tri1.setMargin(margin); 800 if(swapped) 801 { 802 algorithm->setPart0(partId); 803 algorithm->setFace0(triangleIndex); 804 } 805 else 806 { 807 algorithm->setPart1(partId); 808 algorithm->setFace1(triangleIndex); 809 } 810 811 btCollisionObjectWrapper ob1Wrap(body1Wrap,&tri1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),partId,triangleIndex); 812 const btCollisionObjectWrapper * tmp = 0; 813 814 if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject()) 815 { 816 tmp = algorithm->internalGetResultOut()->getBody0Wrap(); 817 algorithm->internalGetResultOut()->setBody0Wrap(&ob1Wrap); 818 } else 819 { 820 tmp = algorithm->internalGetResultOut()->getBody1Wrap(); 821 algorithm->internalGetResultOut()->setBody1Wrap(&ob1Wrap); 822 } 823 824 algorithm->gimpact_vs_shape( 825 body0Wrap,&ob1Wrap,gimpactshape0,&tri1,swapped); 826 827 if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject()) 828 { 829 algorithm->internalGetResultOut()->setBody0Wrap(tmp); 830 } else 831 { 832 algorithm->internalGetResultOut()->setBody1Wrap(tmp); 833 } 834 835 } 836 }; 837 838 839 840 841 void btGImpactCollisionAlgorithm::gimpact_vs_concave( 842 const btCollisionObjectWrapper* body0Wrap, 843 const btCollisionObjectWrapper * body1Wrap, 844 const btGImpactShapeInterface * shape0, 845 const btConcaveShape * shape1,bool swapped) 846 { 847 //create the callback 848 btGImpactTriangleCallback tricallback; 849 tricallback.algorithm = this; 850 tricallback.body0Wrap = body0Wrap; 851 tricallback.body1Wrap = body1Wrap; 852 tricallback.gimpactshape0 = shape0; 853 tricallback.swapped = swapped; 854 tricallback.margin = shape1->getMargin(); 855 856 //getting the trimesh AABB 857 btTransform gimpactInConcaveSpace; 858 859 gimpactInConcaveSpace = body1Wrap->getWorldTransform().inverse() * body0Wrap->getWorldTransform(); 860 861 btVector3 minAABB,maxAABB; 862 shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB); 863 864 shape1->processAllTriangles(&tricallback,minAABB,maxAABB); 865 866 } 867 868 869 870 void btGImpactCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) 871 { 872 clearCache(); 873 874 m_resultOut = resultOut; 875 m_dispatchInfo = &dispatchInfo; 876 const btGImpactShapeInterface * gimpactshape0; 877 const btGImpactShapeInterface * gimpactshape1; 878 879 if (body0Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE) 880 { 881 gimpactshape0 = static_cast<const btGImpactShapeInterface *>(body0Wrap->getCollisionShape()); 882 883 if( body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) 884 { 885 gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape()); 886 887 gimpact_vs_gimpact(body0Wrap,body1Wrap,gimpactshape0,gimpactshape1); 888 } 889 else 890 { 891 gimpact_vs_shape(body0Wrap,body1Wrap,gimpactshape0,body1Wrap->getCollisionShape(),false); 892 } 893 894 } 895 else if (body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) 896 { 897 gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape()); 898 899 gimpact_vs_shape(body1Wrap,body0Wrap,gimpactshape1,body0Wrap->getCollisionShape(),true); 900 } 901 } 902 903 904 btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) 905 { 906 return 1.f; 907 908 } 909 910 ///////////////////////////////////// REGISTERING ALGORITHM ////////////////////////////////////////////// 911 912 913 914 //! Use this function for register the algorithm externally 915 void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher) 916 { 917 918 static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf; 919 920 int i; 921 922 for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) 923 { 924 dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf); 925 } 926 927 for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) 928 { 929 dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf); 930 } 931 932 } 933
