00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
00027 #include "LinearMath/btIDebugDraw.h"
00028 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00029 #include "BulletCollision/CollisionShapes/btBoxShape.h"
00030 #include "btGImpactCollisionAlgorithm.h"
00031 #include "btContactProcessing.h"
00032 #include "LinearMath/btQuickprof.h"
00033
00034
00036 class btPlaneShape : public btStaticPlaneShape
00037 {
00038 public:
00039
00040 btPlaneShape(const btVector3& v, float f)
00041 :btStaticPlaneShape(v,f)
00042 {
00043 }
00044
00045 void get_plane_equation(btVector4 &equation)
00046 {
00047 equation[0] = m_planeNormal[0];
00048 equation[1] = m_planeNormal[1];
00049 equation[2] = m_planeNormal[2];
00050 equation[3] = m_planeConstant;
00051 }
00052
00053
00054 void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation) const
00055 {
00056 equation[0] = trans.getBasis().getRow(0).dot(m_planeNormal);
00057 equation[1] = trans.getBasis().getRow(1).dot(m_planeNormal);
00058 equation[2] = trans.getBasis().getRow(2).dot(m_planeNormal);
00059 equation[3] = trans.getOrigin().dot(m_planeNormal) + m_planeConstant;
00060 }
00061 };
00062
00063
00064
00066 #ifdef TRI_COLLISION_PROFILING
00067
00068 btClock g_triangle_clock;
00069
00070 float g_accum_triangle_collision_time = 0;
00071 int g_count_triangle_collision = 0;
00072
00073 void bt_begin_gim02_tri_time()
00074 {
00075 g_triangle_clock.reset();
00076 }
00077
00078 void bt_end_gim02_tri_time()
00079 {
00080 g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds();
00081 g_count_triangle_collision++;
00082 }
00083 #endif //TRI_COLLISION_PROFILING
00084
00085
00088
00089 class GIM_ShapeRetriever
00090 {
00091 public:
00092 const btGImpactShapeInterface * m_gim_shape;
00093 btTriangleShapeEx m_trishape;
00094 btTetrahedronShapeEx m_tetrashape;
00095
00096 public:
00097 class ChildShapeRetriever
00098 {
00099 public:
00100 GIM_ShapeRetriever * m_parent;
00101 virtual const btCollisionShape * getChildShape(int index)
00102 {
00103 return m_parent->m_gim_shape->getChildShape(index);
00104 }
00105 virtual ~ChildShapeRetriever() {}
00106 };
00107
00108 class TriangleShapeRetriever:public ChildShapeRetriever
00109 {
00110 public:
00111
00112 virtual btCollisionShape * getChildShape(int index)
00113 {
00114 m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape);
00115 return &m_parent->m_trishape;
00116 }
00117 virtual ~TriangleShapeRetriever() {}
00118 };
00119
00120 class TetraShapeRetriever:public ChildShapeRetriever
00121 {
00122 public:
00123
00124 virtual btCollisionShape * getChildShape(int index)
00125 {
00126 m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape);
00127 return &m_parent->m_tetrashape;
00128 }
00129 };
00130 public:
00131 ChildShapeRetriever m_child_retriever;
00132 TriangleShapeRetriever m_tri_retriever;
00133 TetraShapeRetriever m_tetra_retriever;
00134 ChildShapeRetriever * m_current_retriever;
00135
00136 GIM_ShapeRetriever(const btGImpactShapeInterface * gim_shape)
00137 {
00138 m_gim_shape = gim_shape;
00139
00140 if(m_gim_shape->needsRetrieveTriangles())
00141 {
00142 m_current_retriever = &m_tri_retriever;
00143 }
00144 else if(m_gim_shape->needsRetrieveTetrahedrons())
00145 {
00146 m_current_retriever = &m_tetra_retriever;
00147 }
00148 else
00149 {
00150 m_current_retriever = &m_child_retriever;
00151 }
00152
00153 m_current_retriever->m_parent = this;
00154 }
00155
00156 const btCollisionShape * getChildShape(int index)
00157 {
00158 return m_current_retriever->getChildShape(index);
00159 }
00160
00161
00162 };
00163
00164
00165
00167
00168
00169 #ifdef TRI_COLLISION_PROFILING
00170
00172 float btGImpactCollisionAlgorithm::getAverageTreeCollisionTime()
00173 {
00174 return btGImpactBoxSet::getAverageTreeCollisionTime();
00175
00176 }
00177
00179 float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime()
00180 {
00181 if(g_count_triangle_collision == 0) return 0;
00182
00183 float avgtime = g_accum_triangle_collision_time;
00184 avgtime /= (float)g_count_triangle_collision;
00185
00186 g_accum_triangle_collision_time = 0;
00187 g_count_triangle_collision = 0;
00188
00189 return avgtime;
00190 }
00191
00192 #endif //TRI_COLLISION_PROFILING
00193
00194
00195
00196 btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
00197 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap)
00198 {
00199 m_manifoldPtr = NULL;
00200 m_convex_algorithm = NULL;
00201 }
00202
00203 btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm()
00204 {
00205 clearCache();
00206 }
00207
00208
00209
00210
00211
00212 void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper * body0Wrap,
00213 const btCollisionObjectWrapper * body1Wrap,
00214 const btVector3 & point,
00215 const btVector3 & normal,
00216 btScalar distance)
00217 {
00218 m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
00219 m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
00220 checkManifold(body0Wrap,body1Wrap);
00221 m_resultOut->addContactPoint(normal,point,distance);
00222 }
00223
00224
00225 void btGImpactCollisionAlgorithm::shape_vs_shape_collision(
00226 const btCollisionObjectWrapper * body0Wrap,
00227 const btCollisionObjectWrapper* body1Wrap,
00228 const btCollisionShape * shape0,
00229 const btCollisionShape * shape1)
00230 {
00231
00232
00233 {
00234
00235 btCollisionAlgorithm* algor = newAlgorithm(body0Wrap,body1Wrap);
00236
00237
00238 m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
00239 m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
00240
00241 algor->processCollision(body0Wrap,body1Wrap,*m_dispatchInfo,m_resultOut);
00242
00243 algor->~btCollisionAlgorithm();
00244 m_dispatcher->freeCollisionAlgorithm(algor);
00245 }
00246
00247 }
00248
00249 void btGImpactCollisionAlgorithm::convex_vs_convex_collision(
00250 const btCollisionObjectWrapper* body0Wrap,
00251 const btCollisionObjectWrapper* body1Wrap,
00252 const btCollisionShape* shape0,
00253 const btCollisionShape* shape1)
00254 {
00255
00256 m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
00257 m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
00258
00259 btCollisionObjectWrapper ob0(body0Wrap,shape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0);
00260 btCollisionObjectWrapper ob1(body1Wrap,shape1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),m_part1,m_triface1);
00261 checkConvexAlgorithm(&ob0,&ob1);
00262 m_convex_algorithm->processCollision(&ob0,&ob1,*m_dispatchInfo,m_resultOut);
00263
00264
00265 }
00266
00267
00268
00269
00270 void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs(
00271 const btTransform & trans0,
00272 const btTransform & trans1,
00273 const btGImpactShapeInterface * shape0,
00274 const btGImpactShapeInterface * shape1,btPairSet & pairset)
00275 {
00276 if(shape0->hasBoxSet() && shape1->hasBoxSet())
00277 {
00278 btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset);
00279 }
00280 else
00281 {
00282 btAABB boxshape0;
00283 btAABB boxshape1;
00284 int i = shape0->getNumChildShapes();
00285
00286 while(i--)
00287 {
00288 shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
00289
00290 int j = shape1->getNumChildShapes();
00291 while(j--)
00292 {
00293 shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max);
00294
00295 if(boxshape1.has_collision(boxshape0))
00296 {
00297 pairset.push_pair(i,j);
00298 }
00299 }
00300 }
00301 }
00302
00303
00304 }
00305
00306
00307 void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs(
00308 const btTransform & trans0,
00309 const btTransform & trans1,
00310 const btGImpactShapeInterface * shape0,
00311 const btCollisionShape * shape1,
00312 btAlignedObjectArray<int> & collided_primitives)
00313 {
00314
00315 btAABB boxshape;
00316
00317
00318 if(shape0->hasBoxSet())
00319 {
00320 btTransform trans1to0 = trans0.inverse();
00321 trans1to0 *= trans1;
00322
00323 shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);
00324
00325 shape0->getBoxSet()->boxQuery(boxshape, collided_primitives);
00326 }
00327 else
00328 {
00329 shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max);
00330
00331 btAABB boxshape0;
00332 int i = shape0->getNumChildShapes();
00333
00334 while(i--)
00335 {
00336 shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
00337
00338 if(boxshape.has_collision(boxshape0))
00339 {
00340 collided_primitives.push_back(i);
00341 }
00342 }
00343
00344 }
00345
00346 }
00347
00348
00349 void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper * body0Wrap,
00350 const btCollisionObjectWrapper * body1Wrap,
00351 const btGImpactMeshShapePart * shape0,
00352 const btGImpactMeshShapePart * shape1,
00353 const int * pairs, int pair_count)
00354 {
00355 btTriangleShapeEx tri0;
00356 btTriangleShapeEx tri1;
00357
00358 shape0->lockChildShapes();
00359 shape1->lockChildShapes();
00360
00361 const int * pair_pointer = pairs;
00362
00363 while(pair_count--)
00364 {
00365
00366 m_triface0 = *(pair_pointer);
00367 m_triface1 = *(pair_pointer+1);
00368 pair_pointer+=2;
00369
00370
00371
00372 shape0->getBulletTriangle(m_triface0,tri0);
00373 shape1->getBulletTriangle(m_triface1,tri1);
00374
00375
00376
00377 if(tri0.overlap_test_conservative(tri1))
00378 {
00379 convex_vs_convex_collision(body0Wrap,body1Wrap,&tri0,&tri1);
00380 }
00381
00382 }
00383
00384 shape0->unlockChildShapes();
00385 shape1->unlockChildShapes();
00386 }
00387
00388 void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap,
00389 const btCollisionObjectWrapper* body1Wrap,
00390 const btGImpactMeshShapePart * shape0,
00391 const btGImpactMeshShapePart * shape1,
00392 const int * pairs, int pair_count)
00393 {
00394 btTransform orgtrans0 = body0Wrap->getWorldTransform();
00395 btTransform orgtrans1 = body1Wrap->getWorldTransform();
00396
00397 btPrimitiveTriangle ptri0;
00398 btPrimitiveTriangle ptri1;
00399 GIM_TRIANGLE_CONTACT contact_data;
00400
00401 shape0->lockChildShapes();
00402 shape1->lockChildShapes();
00403
00404 const int * pair_pointer = pairs;
00405
00406 while(pair_count--)
00407 {
00408
00409 m_triface0 = *(pair_pointer);
00410 m_triface1 = *(pair_pointer+1);
00411 pair_pointer+=2;
00412
00413
00414 shape0->getPrimitiveTriangle(m_triface0,ptri0);
00415 shape1->getPrimitiveTriangle(m_triface1,ptri1);
00416
00417 #ifdef TRI_COLLISION_PROFILING
00418 bt_begin_gim02_tri_time();
00419 #endif
00420
00421 ptri0.applyTransform(orgtrans0);
00422 ptri1.applyTransform(orgtrans1);
00423
00424
00425
00426 ptri0.buildTriPlane();
00427 ptri1.buildTriPlane();
00428
00429
00430
00431
00432 if(ptri0.overlap_test_conservative(ptri1))
00433 {
00434 if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data))
00435 {
00436
00437 int j = contact_data.m_point_count;
00438 while(j--)
00439 {
00440
00441 addContactPoint(body0Wrap, body1Wrap,
00442 contact_data.m_points[j],
00443 contact_data.m_separating_normal,
00444 -contact_data.m_penetration_depth);
00445 }
00446 }
00447 }
00448
00449 #ifdef TRI_COLLISION_PROFILING
00450 bt_end_gim02_tri_time();
00451 #endif
00452
00453 }
00454
00455 shape0->unlockChildShapes();
00456 shape1->unlockChildShapes();
00457
00458 }
00459
00460
00461 void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(
00462 const btCollisionObjectWrapper* body0Wrap,
00463 const btCollisionObjectWrapper * body1Wrap,
00464 const btGImpactShapeInterface * shape0,
00465 const btGImpactShapeInterface * shape1)
00466 {
00467
00468 if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
00469 {
00470 const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0);
00471 m_part0 = meshshape0->getMeshPartCount();
00472
00473 while(m_part0--)
00474 {
00475 gimpact_vs_gimpact(body0Wrap,body1Wrap,meshshape0->getMeshPart(m_part0),shape1);
00476 }
00477
00478 return;
00479 }
00480
00481 if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
00482 {
00483 const btGImpactMeshShape * meshshape1 = static_cast<const btGImpactMeshShape *>(shape1);
00484 m_part1 = meshshape1->getMeshPartCount();
00485
00486 while(m_part1--)
00487 {
00488
00489 gimpact_vs_gimpact(body0Wrap,body1Wrap,shape0,meshshape1->getMeshPart(m_part1));
00490
00491 }
00492
00493 return;
00494 }
00495
00496
00497 btTransform orgtrans0 = body0Wrap->getWorldTransform();
00498 btTransform orgtrans1 = body1Wrap->getWorldTransform();
00499
00500 btPairSet pairset;
00501
00502 gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset);
00503
00504 if(pairset.size()== 0) return;
00505
00506 if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
00507 shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART)
00508 {
00509 const btGImpactMeshShapePart * shapepart0 = static_cast<const btGImpactMeshShapePart * >(shape0);
00510 const btGImpactMeshShapePart * shapepart1 = static_cast<const btGImpactMeshShapePart * >(shape1);
00511
00512 #ifdef BULLET_TRIANGLE_COLLISION
00513 collide_gjk_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
00514 #else
00515 collide_sat_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
00516 #endif
00517
00518 return;
00519 }
00520
00521
00522
00523 shape0->lockChildShapes();
00524 shape1->lockChildShapes();
00525
00526 GIM_ShapeRetriever retriever0(shape0);
00527 GIM_ShapeRetriever retriever1(shape1);
00528
00529 bool child_has_transform0 = shape0->childrenHasTransform();
00530 bool child_has_transform1 = shape1->childrenHasTransform();
00531
00532 int i = pairset.size();
00533 while(i--)
00534 {
00535 GIM_PAIR * pair = &pairset[i];
00536 m_triface0 = pair->m_index1;
00537 m_triface1 = pair->m_index2;
00538 const btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0);
00539 const btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1);
00540
00541 btTransform tr0 = body0Wrap->getWorldTransform();
00542 btTransform tr1 = body1Wrap->getWorldTransform();
00543
00544 if(child_has_transform0)
00545 {
00546 tr0 = orgtrans0*shape0->getChildTransform(m_triface0);
00547 }
00548
00549 if(child_has_transform1)
00550 {
00551 tr1 = orgtrans1*shape1->getChildTransform(m_triface1);
00552 }
00553
00554 btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),tr0,m_part0,m_triface0);
00555 btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),tr1,m_part1,m_triface1);
00556
00557
00558 convex_vs_convex_collision(&ob0,&ob1,colshape0,colshape1);
00559 }
00560
00561 shape0->unlockChildShapes();
00562 shape1->unlockChildShapes();
00563 }
00564
00565 void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap,
00566 const btCollisionObjectWrapper * body1Wrap,
00567 const btGImpactShapeInterface * shape0,
00568 const btCollisionShape * shape1,bool swapped)
00569 {
00570 if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
00571 {
00572 const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0);
00573 int& part = swapped ? m_part1 : m_part0;
00574 part = meshshape0->getMeshPartCount();
00575
00576 while(part--)
00577 {
00578
00579 gimpact_vs_shape(body0Wrap,
00580 body1Wrap,
00581 meshshape0->getMeshPart(part),
00582 shape1,swapped);
00583
00584 }
00585
00586 return;
00587 }
00588
00589 #ifdef GIMPACT_VS_PLANE_COLLISION
00590 if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
00591 shape1->getShapeType() == STATIC_PLANE_PROXYTYPE)
00592 {
00593 const btGImpactMeshShapePart * shapepart = static_cast<const btGImpactMeshShapePart *>(shape0);
00594 const btStaticPlaneShape * planeshape = static_cast<const btStaticPlaneShape * >(shape1);
00595 gimpacttrimeshpart_vs_plane_collision(body0Wrap,body1Wrap,shapepart,planeshape,swapped);
00596 return;
00597 }
00598
00599 #endif
00600
00601
00602
00603 if(shape1->isCompound())
00604 {
00605 const btCompoundShape * compoundshape = static_cast<const btCompoundShape *>(shape1);
00606 gimpact_vs_compoundshape(body0Wrap,body1Wrap,shape0,compoundshape,swapped);
00607 return;
00608 }
00609 else if(shape1->isConcave())
00610 {
00611 const btConcaveShape * concaveshape = static_cast<const btConcaveShape *>(shape1);
00612 gimpact_vs_concave(body0Wrap,body1Wrap,shape0,concaveshape,swapped);
00613 return;
00614 }
00615
00616
00617 btTransform orgtrans0 = body0Wrap->getWorldTransform();
00618
00619 btTransform orgtrans1 = body1Wrap->getWorldTransform();
00620
00621 btAlignedObjectArray<int> collided_results;
00622
00623 gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results);
00624
00625 if(collided_results.size() == 0) return;
00626
00627
00628 shape0->lockChildShapes();
00629
00630 GIM_ShapeRetriever retriever0(shape0);
00631
00632
00633 bool child_has_transform0 = shape0->childrenHasTransform();
00634
00635
00636 int i = collided_results.size();
00637
00638 while(i--)
00639 {
00640 int child_index = collided_results[i];
00641 if(swapped)
00642 m_triface1 = child_index;
00643 else
00644 m_triface0 = child_index;
00645
00646 const btCollisionShape * colshape0 = retriever0.getChildShape(child_index);
00647
00648 btTransform tr0 = body0Wrap->getWorldTransform();
00649
00650 if(child_has_transform0)
00651 {
00652 tr0 = orgtrans0*shape0->getChildTransform(child_index);
00653 }
00654
00655 btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0);
00656 const btCollisionObjectWrapper* prevObj0 = m_resultOut->getBody0Wrap();
00657
00658 if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob0.getCollisionObject())
00659 {
00660 m_resultOut->setBody0Wrap(&ob0);
00661 } else
00662 {
00663 m_resultOut->setBody1Wrap(&ob0);
00664 }
00665
00666
00667 if(swapped)
00668 {
00669
00670 shape_vs_shape_collision(body1Wrap,&ob0,shape1,colshape0);
00671 }
00672 else
00673 {
00674
00675 shape_vs_shape_collision(&ob0,body1Wrap,colshape0,shape1);
00676 }
00677 m_resultOut->setBody0Wrap(prevObj0);
00678
00679 }
00680
00681 shape0->unlockChildShapes();
00682
00683 }
00684
00685 void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap,
00686 const btCollisionObjectWrapper* body1Wrap,
00687 const btGImpactShapeInterface * shape0,
00688 const btCompoundShape * shape1,bool swapped)
00689 {
00690 btTransform orgtrans1 = body1Wrap->getWorldTransform();
00691
00692 int i = shape1->getNumChildShapes();
00693 while(i--)
00694 {
00695
00696 const btCollisionShape * colshape1 = shape1->getChildShape(i);
00697 btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i);
00698
00699 btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),childtrans1,-1,i);
00700
00701 const btCollisionObjectWrapper* tmp = 0;
00702 if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject())
00703 {
00704 tmp = m_resultOut->getBody0Wrap();
00705 m_resultOut->setBody0Wrap(&ob1);
00706 } else
00707 {
00708 tmp = m_resultOut->getBody1Wrap();
00709 m_resultOut->setBody1Wrap(&ob1);
00710 }
00711
00712 gimpact_vs_shape(body0Wrap, &ob1,
00713 shape0,colshape1,swapped);
00714
00715 if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject())
00716 {
00717 m_resultOut->setBody0Wrap(tmp);
00718 } else
00719 {
00720 m_resultOut->setBody1Wrap(tmp);
00721 }
00722 }
00723 }
00724
00725 void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
00726 const btCollisionObjectWrapper * body0Wrap,
00727 const btCollisionObjectWrapper * body1Wrap,
00728 const btGImpactMeshShapePart * shape0,
00729 const btStaticPlaneShape * shape1,bool swapped)
00730 {
00731
00732
00733 btTransform orgtrans0 = body0Wrap->getWorldTransform();
00734 btTransform orgtrans1 = body1Wrap->getWorldTransform();
00735
00736 const btPlaneShape * planeshape = static_cast<const btPlaneShape *>(shape1);
00737 btVector4 plane;
00738 planeshape->get_plane_equation_transformed(orgtrans1,plane);
00739
00740
00741
00742 btAABB tribox;
00743 shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max);
00744 tribox.increment_margin(planeshape->getMargin());
00745
00746 if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return;
00747
00748 shape0->lockChildShapes();
00749
00750 btScalar margin = shape0->getMargin() + planeshape->getMargin();
00751
00752 btVector3 vertex;
00753 int vi = shape0->getVertexCount();
00754 while(vi--)
00755 {
00756 shape0->getVertex(vi,vertex);
00757 vertex = orgtrans0(vertex);
00758
00759 btScalar distance = vertex.dot(plane) - plane[3] - margin;
00760
00761 if(distance<0.0)
00762 {
00763 if(swapped)
00764 {
00765 addContactPoint(body1Wrap, body0Wrap,
00766 vertex,
00767 -plane,
00768 distance);
00769 }
00770 else
00771 {
00772 addContactPoint(body0Wrap, body1Wrap,
00773 vertex,
00774 plane,
00775 distance);
00776 }
00777 }
00778 }
00779
00780 shape0->unlockChildShapes();
00781 }
00782
00783
00784
00785
00786 class btGImpactTriangleCallback: public btTriangleCallback
00787 {
00788 public:
00789 btGImpactCollisionAlgorithm * algorithm;
00790 const btCollisionObjectWrapper * body0Wrap;
00791 const btCollisionObjectWrapper * body1Wrap;
00792 const btGImpactShapeInterface * gimpactshape0;
00793 bool swapped;
00794 btScalar margin;
00795
00796 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
00797 {
00798 btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]);
00799 tri1.setMargin(margin);
00800 if(swapped)
00801 {
00802 algorithm->setPart0(partId);
00803 algorithm->setFace0(triangleIndex);
00804 }
00805 else
00806 {
00807 algorithm->setPart1(partId);
00808 algorithm->setFace1(triangleIndex);
00809 }
00810
00811 btCollisionObjectWrapper ob1Wrap(body1Wrap,&tri1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),partId,triangleIndex);
00812 const btCollisionObjectWrapper * tmp = 0;
00813
00814 if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject())
00815 {
00816 tmp = algorithm->internalGetResultOut()->getBody0Wrap();
00817 algorithm->internalGetResultOut()->setBody0Wrap(&ob1Wrap);
00818 } else
00819 {
00820 tmp = algorithm->internalGetResultOut()->getBody1Wrap();
00821 algorithm->internalGetResultOut()->setBody1Wrap(&ob1Wrap);
00822 }
00823
00824 algorithm->gimpact_vs_shape(
00825 body0Wrap,&ob1Wrap,gimpactshape0,&tri1,swapped);
00826
00827 if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject())
00828 {
00829 algorithm->internalGetResultOut()->setBody0Wrap(tmp);
00830 } else
00831 {
00832 algorithm->internalGetResultOut()->setBody1Wrap(tmp);
00833 }
00834
00835 }
00836 };
00837
00838
00839
00840
00841 void btGImpactCollisionAlgorithm::gimpact_vs_concave(
00842 const btCollisionObjectWrapper* body0Wrap,
00843 const btCollisionObjectWrapper * body1Wrap,
00844 const btGImpactShapeInterface * shape0,
00845 const btConcaveShape * shape1,bool swapped)
00846 {
00847
00848 btGImpactTriangleCallback tricallback;
00849 tricallback.algorithm = this;
00850 tricallback.body0Wrap = body0Wrap;
00851 tricallback.body1Wrap = body1Wrap;
00852 tricallback.gimpactshape0 = shape0;
00853 tricallback.swapped = swapped;
00854 tricallback.margin = shape1->getMargin();
00855
00856
00857 btTransform gimpactInConcaveSpace;
00858
00859 gimpactInConcaveSpace = body1Wrap->getWorldTransform().inverse() * body0Wrap->getWorldTransform();
00860
00861 btVector3 minAABB,maxAABB;
00862 shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB);
00863
00864 shape1->processAllTriangles(&tricallback,minAABB,maxAABB);
00865
00866 }
00867
00868
00869
00870 void btGImpactCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00871 {
00872 clearCache();
00873
00874 m_resultOut = resultOut;
00875 m_dispatchInfo = &dispatchInfo;
00876 const btGImpactShapeInterface * gimpactshape0;
00877 const btGImpactShapeInterface * gimpactshape1;
00878
00879 if (body0Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
00880 {
00881 gimpactshape0 = static_cast<const btGImpactShapeInterface *>(body0Wrap->getCollisionShape());
00882
00883 if( body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
00884 {
00885 gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape());
00886
00887 gimpact_vs_gimpact(body0Wrap,body1Wrap,gimpactshape0,gimpactshape1);
00888 }
00889 else
00890 {
00891 gimpact_vs_shape(body0Wrap,body1Wrap,gimpactshape0,body1Wrap->getCollisionShape(),false);
00892 }
00893
00894 }
00895 else if (body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
00896 {
00897 gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape());
00898
00899 gimpact_vs_shape(body1Wrap,body0Wrap,gimpactshape1,body0Wrap->getCollisionShape(),true);
00900 }
00901 }
00902
00903
00904 btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00905 {
00906 return 1.f;
00907
00908 }
00909
00911
00912
00913
00915 void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher)
00916 {
00917
00918 static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf;
00919
00920 int i;
00921
00922 for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
00923 {
00924 dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf);
00925 }
00926
00927 for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
00928 {
00929 dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf);
00930 }
00931
00932 }