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