src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp

   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@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 Le\7fn Nßjera
  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                 btCollisionObjectWrapper ob0(body0Wrap,shape0,body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform());
 235                 btCollisionObjectWrapper ob1(body1Wrap,shape1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform());
 236
 237                 btCollisionAlgorithm* algor = newAlgorithm(&ob0,&ob1);
 238                 // post :       checkManifold is called
 239
 240                 m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
 241                 m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
 242
 243                 algor->processCollision(&ob0,&ob1,*m_dispatchInfo,m_resultOut);
 244
 245                 algor->~btCollisionAlgorithm();
 246                 m_dispatcher->freeCollisionAlgorithm(algor);
 247         }
 248
 249 }
 250
 251 void btGImpactCollisionAlgorithm::convex_vs_convex_collision(
 252                                           const btCollisionObjectWrapper* body0Wrap,
 253                                           const btCollisionObjectWrapper* body1Wrap,
 254                                           const btCollisionShape* shape0,
 255                                           const btCollisionShape* shape1)
 256 {
 257
 258         m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
 259         m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
 260
 261         btCollisionObjectWrapper ob0(body0Wrap,shape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform());
 262         btCollisionObjectWrapper ob1(body1Wrap,shape1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform());
 263         checkConvexAlgorithm(&ob0,&ob1);
 264         m_convex_algorithm->processCollision(&ob0,&ob1,*m_dispatchInfo,m_resultOut);
 265
 266
 267 }
 268
 269
 270
 271
 272 void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs(
 273                                           const btTransform & trans0,
 274                                           const btTransform & trans1,
 275                                           const btGImpactShapeInterface * shape0,
 276                                           const btGImpactShapeInterface * shape1,btPairSet & pairset)
 277 {
 278         if(shape0->hasBoxSet() && shape1->hasBoxSet())
 279         {
 280                 btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset);
 281         }
 282         else
 283         {
 284                 btAABB boxshape0;
 285                 btAABB boxshape1;
 286                 int i = shape0->getNumChildShapes();
 287
 288                 while(i--)
 289                 {
 290                         shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
 291
 292                         int j = shape1->getNumChildShapes();
 293                         while(j--)
 294                         {
 295                                 shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max);
 296
 297                                 if(boxshape1.has_collision(boxshape0))
 298                                 {
 299                                         pairset.push_pair(i,j);
 300                                 }
 301                         }
 302                 }
 303         }
 304
 305
 306 }
 307
 308
 309 void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs(
 310                                           const btTransform & trans0,
 311                                           const btTransform & trans1,
 312                                           const btGImpactShapeInterface * shape0,
 313                                           const btCollisionShape * shape1,
 314                                           btAlignedObjectArray<int> & collided_primitives)
 315 {
 316
 317         btAABB boxshape;
 318
 319
 320         if(shape0->hasBoxSet())
 321         {
 322                 btTransform trans1to0 = trans0.inverse();
 323                 trans1to0 *= trans1;
 324
 325                 shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);
 326
 327                 shape0->getBoxSet()->boxQuery(boxshape, collided_primitives);
 328         }
 329         else
 330         {
 331                 shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max);
 332
 333                 btAABB boxshape0;
 334                 int i = shape0->getNumChildShapes();
 335
 336                 while(i--)
 337                 {
 338                         shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
 339
 340                         if(boxshape.has_collision(boxshape0))
 341                         {
 342                                 collided_primitives.push_back(i);
 343                         }
 344                 }
 345
 346         }
 347
 348 }
 349
 350
 351 void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper * body0Wrap,
 352                                   const btCollisionObjectWrapper * body1Wrap,
 353                                   const btGImpactMeshShapePart * shape0,
 354                                   const btGImpactMeshShapePart * shape1,
 355                                   const int * pairs, int pair_count)
 356 {
 357         btTriangleShapeEx tri0;
 358         btTriangleShapeEx tri1;
 359
 360         shape0->lockChildShapes();
 361         shape1->lockChildShapes();
 362
 363         const int * pair_pointer = pairs;
 364
 365         while(pair_count--)
 366         {
 367
 368                 m_triface0 = *(pair_pointer);
 369                 m_triface1 = *(pair_pointer+1);
 370                 pair_pointer+=2;
 371
 372
 373
 374                 shape0->getBulletTriangle(m_triface0,tri0);
 375                 shape1->getBulletTriangle(m_triface1,tri1);
 376
 377
 378                 //collide two convex shapes
 379                 if(tri0.overlap_test_conservative(tri1))
 380                 {
 381                         convex_vs_convex_collision(body0Wrap,body1Wrap,&tri0,&tri1);
 382                 }
 383
 384         }
 385
 386         shape0->unlockChildShapes();
 387         shape1->unlockChildShapes();
 388 }
 389
 390 void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap,
 391                                           const btCollisionObjectWrapper* body1Wrap,
 392                                           const btGImpactMeshShapePart * shape0,
 393                                           const btGImpactMeshShapePart * shape1,
 394                                           const int * pairs, int pair_count)
 395 {
 396         btTransform orgtrans0 = body0Wrap->getWorldTransform();
 397         btTransform orgtrans1 = body1Wrap->getWorldTransform();
 398
 399         btPrimitiveTriangle ptri0;
 400         btPrimitiveTriangle ptri1;
 401         GIM_TRIANGLE_CONTACT contact_data;
 402
 403         shape0->lockChildShapes();
 404         shape1->lockChildShapes();
 405
 406         const int * pair_pointer = pairs;
 407
 408         while(pair_count--)
 409         {
 410
 411                 m_triface0 = *(pair_pointer);
 412                 m_triface1 = *(pair_pointer+1);
 413                 pair_pointer+=2;
 414
 415
 416                 shape0->getPrimitiveTriangle(m_triface0,ptri0);
 417                 shape1->getPrimitiveTriangle(m_triface1,ptri1);
 418
 419                 #ifdef TRI_COLLISION_PROFILING
 420                 bt_begin_gim02_tri_time();
 421                 #endif
 422
 423                 ptri0.applyTransform(orgtrans0);
 424                 ptri1.applyTransform(orgtrans1);
 425
 426
 427                 //build planes
 428                 ptri0.buildTriPlane();
 429                 ptri1.buildTriPlane();
 430                 // test conservative
 431
 432
 433
 434                 if(ptri0.overlap_test_conservative(ptri1))
 435                 {
 436                         if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data))
 437                         {
 438
 439                                 int j = contact_data.m_point_count;
 440                                 while(j--)
 441                                 {
 442
 443                                         addContactPoint(body0Wrap, body1Wrap,
 444                                                                 contact_data.m_points[j],
 445                                                                 contact_data.m_separating_normal,
 446                                                                 -contact_data.m_penetration_depth);
 447                                 }
 448                         }
 449                 }
 450
 451                 #ifdef TRI_COLLISION_PROFILING
 452                 bt_end_gim02_tri_time();
 453                 #endif
 454
 455         }
 456
 457         shape0->unlockChildShapes();
 458         shape1->unlockChildShapes();
 459
 460 }
 461
 462
 463 void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(
 464                                                 const btCollisionObjectWrapper* body0Wrap,
 465                                                 const btCollisionObjectWrapper * body1Wrap,
 466                                                 const btGImpactShapeInterface * shape0,
 467                                                 const btGImpactShapeInterface * shape1)
 468 {
 469
 470         if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
 471         {
 472                 const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0);
 473                 m_part0 = meshshape0->getMeshPartCount();
 474
 475                 while(m_part0--)
 476                 {
 477                         gimpact_vs_gimpact(body0Wrap,body1Wrap,meshshape0->getMeshPart(m_part0),shape1);
 478                 }
 479
 480                 return;
 481         }
 482
 483         if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
 484         {
 485                 const btGImpactMeshShape * meshshape1 = static_cast<const btGImpactMeshShape *>(shape1);
 486                 m_part1 = meshshape1->getMeshPartCount();
 487
 488                 while(m_part1--)
 489                 {
 490
 491                         gimpact_vs_gimpact(body0Wrap,body1Wrap,shape0,meshshape1->getMeshPart(m_part1));
 492
 493                 }
 494
 495                 return;
 496         }
 497
 498
 499         btTransform orgtrans0 = body0Wrap->getWorldTransform();
 500         btTransform orgtrans1 = body1Wrap->getWorldTransform();
 501
 502         btPairSet pairset;
 503
 504         gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset);
 505
 506         if(pairset.size()== 0) return;
 507
 508         if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
 509                 shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART)
 510         {
 511                 const btGImpactMeshShapePart * shapepart0 = static_cast<const btGImpactMeshShapePart * >(shape0);
 512                 const btGImpactMeshShapePart * shapepart1 = static_cast<const btGImpactMeshShapePart * >(shape1);
 513                 //specialized function
 514                 #ifdef BULLET_TRIANGLE_COLLISION
 515                 collide_gjk_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
 516                 #else
 517                 collide_sat_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
 518                 #endif
 519
 520                 return;
 521         }
 522
 523         //general function
 524
 525         shape0->lockChildShapes();
 526         shape1->lockChildShapes();
 527
 528         GIM_ShapeRetriever retriever0(shape0);
 529         GIM_ShapeRetriever retriever1(shape1);
 530
 531         bool child_has_transform0 = shape0->childrenHasTransform();
 532         bool child_has_transform1 = shape1->childrenHasTransform();
 533
 534         int i = pairset.size();
 535         while(i--)
 536         {
 537                 GIM_PAIR * pair = &pairset[i];
 538                 m_triface0 = pair->m_index1;
 539                 m_triface1 = pair->m_index2;
 540                 const btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0);
 541                 const btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1);
 542
 543                 btTransform tr0 = body0Wrap->getWorldTransform();
 544                 btTransform tr1 = body1Wrap->getWorldTransform();
 545
 546                 if(child_has_transform0)
 547                 {
 548                         tr0 = orgtrans0*shape0->getChildTransform(m_triface0);
 549                 }
 550
 551                 if(child_has_transform1)
 552                 {
 553                         tr1 = orgtrans1*shape1->getChildTransform(m_triface1);
 554                 }
 555
 556                 btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),tr0);
 557                 btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),tr1);
 558
 559                 //collide two convex shapes
 560                 convex_vs_convex_collision(&ob0,&ob1,colshape0,colshape1);
 561         }
 562
 563         shape0->unlockChildShapes();
 564         shape1->unlockChildShapes();
 565 }
 566
 567 void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap,
 568                                   const btCollisionObjectWrapper * body1Wrap,
 569                                   const btGImpactShapeInterface * shape0,
 570                                   const btCollisionShape * shape1,bool swapped)
 571 {
 572         if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
 573         {
 574                 const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0);
 575                 int& part = swapped ? m_part1 : m_part0;
 576                 part = meshshape0->getMeshPartCount();
 577
 578                 while(part--)
 579                 {
 580
 581                         gimpact_vs_shape(body0Wrap,
 582                                   body1Wrap,
 583                                   meshshape0->getMeshPart(part),
 584                                   shape1,swapped);
 585
 586                 }
 587
 588                 return;
 589         }
 590
 591         #ifdef GIMPACT_VS_PLANE_COLLISION
 592         if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
 593                 shape1->getShapeType() == STATIC_PLANE_PROXYTYPE)
 594         {
 595                 const btGImpactMeshShapePart * shapepart = static_cast<const btGImpactMeshShapePart *>(shape0);
 596                 const btStaticPlaneShape * planeshape = static_cast<const btStaticPlaneShape * >(shape1);
 597                 gimpacttrimeshpart_vs_plane_collision(body0Wrap,body1Wrap,shapepart,planeshape,swapped);
 598                 return;
 599         }
 600
 601         #endif
 602
 603
 604
 605         if(shape1->isCompound())
 606         {
 607                 const btCompoundShape * compoundshape = static_cast<const btCompoundShape *>(shape1);
 608                 gimpact_vs_compoundshape(body0Wrap,body1Wrap,shape0,compoundshape,swapped);
 609                 return;
 610         }
 611         else if(shape1->isConcave())
 612         {
 613                 const btConcaveShape * concaveshape = static_cast<const btConcaveShape *>(shape1);
 614                 gimpact_vs_concave(body0Wrap,body1Wrap,shape0,concaveshape,swapped);
 615                 return;
 616         }
 617
 618
 619         btTransform orgtrans0 = body0Wrap->getWorldTransform();
 620
 621         btTransform orgtrans1 = body1Wrap->getWorldTransform();
 622
 623         btAlignedObjectArray<int> collided_results;
 624
 625         gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results);
 626
 627         if(collided_results.size() == 0) return;
 628
 629
 630         shape0->lockChildShapes();
 631
 632         GIM_ShapeRetriever retriever0(shape0);
 633
 634
 635         bool child_has_transform0 = shape0->childrenHasTransform();
 636
 637
 638         int i = collided_results.size();
 639
 640         while(i--)
 641         {
 642                 int child_index = collided_results[i];
 643         if(swapped)
 644                 m_triface1 = child_index;
 645         else
 646             m_triface0 = child_index;
 647
 648                 const btCollisionShape * colshape0 = retriever0.getChildShape(child_index);
 649
 650                 btTransform tr0 = body0Wrap->getWorldTransform();
 651
 652                 if(child_has_transform0)
 653                 {
 654                         tr0 = orgtrans0*shape0->getChildTransform(child_index);
 655                 }
 656
 657                 btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform());
 658
 659                 //collide two shapes
 660                 if(swapped)
 661                 {
 662                         shape_vs_shape_collision(body1Wrap,&ob0,shape1,colshape0);
 663                 }
 664                 else
 665                 {
 666                         shape_vs_shape_collision(&ob0,body1Wrap,colshape0,shape1);
 667                 }
 668
 669         }
 670
 671         shape0->unlockChildShapes();
 672
 673 }
 674
 675 void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap,
 676                                   const btCollisionObjectWrapper* body1Wrap,
 677                                   const btGImpactShapeInterface * shape0,
 678                                   const btCompoundShape * shape1,bool swapped)
 679 {
 680         btTransform orgtrans1 = body1Wrap->getWorldTransform();
 681
 682         int i = shape1->getNumChildShapes();
 683         while(i--)
 684         {
 685
 686                 const btCollisionShape * colshape1 = shape1->getChildShape(i);
 687                 btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i);
 688
 689                 btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),childtrans1);
 690                 //collide child shape
 691                 gimpact_vs_shape(body0Wrap, &ob1,
 692                                           shape0,colshape1,swapped);
 693         }
 694 }
 695
 696 void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
 697                                           const btCollisionObjectWrapper * body0Wrap,
 698                                           const btCollisionObjectWrapper * body1Wrap,
 699                                           const btGImpactMeshShapePart * shape0,
 700                                           const btStaticPlaneShape * shape1,bool swapped)
 701 {
 702
 703
 704         btTransform orgtrans0 = body0Wrap->getWorldTransform();
 705         btTransform orgtrans1 = body1Wrap->getWorldTransform();
 706
 707         const btPlaneShape * planeshape = static_cast<const btPlaneShape *>(shape1);
 708         btVector4 plane;
 709         planeshape->get_plane_equation_transformed(orgtrans1,plane);
 710
 711         //test box against plane
 712
 713         btAABB tribox;
 714         shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max);
 715         tribox.increment_margin(planeshape->getMargin());
 716
 717         if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return;
 718
 719         shape0->lockChildShapes();
 720
 721         btScalar margin = shape0->getMargin() + planeshape->getMargin();
 722
 723         btVector3 vertex;
 724         int vi = shape0->getVertexCount();
 725         while(vi--)
 726         {
 727                 shape0->getVertex(vi,vertex);
 728                 vertex = orgtrans0(vertex);
 729
 730                 btScalar distance = vertex.dot(plane) - plane[3] - margin;
 731
 732                 if(distance<0.0)//add contact
 733                 {
 734                         if(swapped)
 735                         {
 736                                 addContactPoint(body1Wrap, body0Wrap,
 737                                         vertex,
 738                                         -plane,
 739                                         distance);
 740                         }
 741                         else
 742                         {
 743                                 addContactPoint(body0Wrap, body1Wrap,
 744                                         vertex,
 745                                         plane,
 746                                         distance);
 747                         }
 748                 }
 749         }
 750
 751         shape0->unlockChildShapes();
 752 }
 753
 754
 755
 756
 757 class btGImpactTriangleCallback: public btTriangleCallback
 758 {
 759 public:
 760         btGImpactCollisionAlgorithm * algorithm;
 761         const btCollisionObjectWrapper * body0Wrap;
 762         const btCollisionObjectWrapper * body1Wrap;
 763         const btGImpactShapeInterface * gimpactshape0;
 764         bool swapped;
 765         btScalar margin;
 766
 767         virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 768         {
 769                 btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]);
 770                 tri1.setMargin(margin);
 771         if(swapped)
 772         {
 773             algorithm->setPart0(partId);
 774             algorithm->setFace0(triangleIndex);
 775         }
 776         else
 777         {
 778             algorithm->setPart1(partId);
 779             algorithm->setFace1(triangleIndex);
 780         }
 781                 algorithm->gimpact_vs_shape(
 782                                                         body0Wrap,body1Wrap,gimpactshape0,&tri1,swapped);
 783         }
 784 };
 785
 786
 787
 788
 789 void btGImpactCollisionAlgorithm::gimpact_vs_concave(
 790                                   const btCollisionObjectWrapper* body0Wrap,
 791                                   const btCollisionObjectWrapper * body1Wrap,
 792                                   const btGImpactShapeInterface * shape0,
 793                                   const btConcaveShape * shape1,bool swapped)
 794 {
 795         //create the callback
 796         btGImpactTriangleCallback tricallback;
 797         tricallback.algorithm = this;
 798         tricallback.body0Wrap = body0Wrap;
 799         tricallback.body1Wrap = body1Wrap;
 800         tricallback.gimpactshape0 = shape0;
 801         tricallback.swapped = swapped;
 802         tricallback.margin = shape1->getMargin();
 803
 804         //getting the trimesh AABB
 805         btTransform gimpactInConcaveSpace;
 806
 807         gimpactInConcaveSpace = body1Wrap->getWorldTransform().inverse() * body0Wrap->getWorldTransform();
 808
 809         btVector3 minAABB,maxAABB;
 810         shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB);
 811
 812         shape1->processAllTriangles(&tricallback,minAABB,maxAABB);
 813
 814 }
 815
 816
 817
 818 void btGImpactCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 819 {
 820     clearCache();
 821
 822     m_resultOut = resultOut;
 823         m_dispatchInfo = &dispatchInfo;
 824     const btGImpactShapeInterface * gimpactshape0;
 825     const btGImpactShapeInterface * gimpactshape1;
 826
 827         if (body0Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
 828         {
 829                 gimpactshape0 = static_cast<const btGImpactShapeInterface *>(body0Wrap->getCollisionShape());
 830
 831                 if( body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
 832                 {
 833                         gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape());
 834
 835                         gimpact_vs_gimpact(body0Wrap,body1Wrap,gimpactshape0,gimpactshape1);
 836                 }
 837                 else
 838                 {
 839                         gimpact_vs_shape(body0Wrap,body1Wrap,gimpactshape0,body1Wrap->getCollisionShape(),false);
 840                 }
 841
 842         }
 843         else if (body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
 844         {
 845                 gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape());
 846
 847                 gimpact_vs_shape(body1Wrap,body0Wrap,gimpactshape1,body0Wrap->getCollisionShape(),true);
 848         }
 849 }
 850
 851
 852 btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 853 {
 854         return 1.f;
 855
 856 }
 857
 858 ///////////////////////////////////// REGISTERING ALGORITHM //////////////////////////////////////////////
 859
 860
 861
 862 //! Use this function for register the algorithm externally
 863 void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher)
 864 {
 865
 866         static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf;
 867
 868         int i;
 869
 870         for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
 871         {
 872                 dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf);
 873         }
 874
 875         for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
 876         {
 877                 dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf);
 878         }
 879
 880 }