btGImpactCollisionAlgorithm.cpp

Go to the documentation of this file.
00001 /*
00002 This source file is part of GIMPACT Library.
00003 
00004 For the latest info, see http://gimpact.sourceforge.net/
00005 
00006 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
00007 email: projectileman@yahoo.com
00008 
00009 
00010 This software is provided 'as-is', without any express or implied warranty.
00011 In no event will the authors be held liable for any damages arising from the use of this software.
00012 Permission is granted to anyone to use this software for any purpose,
00013 including commercial applications, and to alter it and redistribute it freely,
00014 subject to the following restrictions:
00015 
00016 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.
00017 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00018 3. This notice may not be removed or altered from any source distribution.
00019 */
00020 /*
00021 Author: Francisco Len Nßjera
00022 Concave-Concave Collision
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                 //select retriever
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                 // post :       checkManifold is called
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                 //collide two convex shapes
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                 //build planes
00426                 ptri0.buildTriPlane();
00427                 ptri1.buildTriPlane();
00428                 // test conservative
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                 //specialized function
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         //general function
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                 //collide two convex shapes
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                 //collide two shapes
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                 //collide child shape
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         //test box against plane
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)//add contact
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         //create the callback
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         //getting the trimesh AABB
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 }