Blender V2.61 - r43446

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)
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     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 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(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     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, btCollisionObject* body0,btCollisionObject* body1)
00197 : btActivatingCollisionAlgorithm(ci,body0,body1)
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(btCollisionObject * body0,
00213                 btCollisionObject * body1,
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(body0,body1);
00221     m_resultOut->addContactPoint(normal,point,distance);
00222 }
00223 
00224 
00225 void btGImpactCollisionAlgorithm::shape_vs_shape_collision(
00226                       btCollisionObject * body0,
00227                       btCollisionObject * body1,
00228                       btCollisionShape * shape0,
00229                       btCollisionShape * shape1)
00230 {
00231 
00232     btCollisionShape* tmpShape0 = body0->getCollisionShape();
00233     btCollisionShape* tmpShape1 = body1->getCollisionShape();
00234     
00235     body0->internalSetTemporaryCollisionShape(shape0);
00236     body1->internalSetTemporaryCollisionShape(shape1);
00237 
00238     {
00239         btCollisionAlgorithm* algor = newAlgorithm(body0,body1);
00240         // post :   checkManifold is called
00241 
00242         m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
00243         m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
00244 
00245         algor->processCollision(body0,body1,*m_dispatchInfo,m_resultOut);
00246 
00247         algor->~btCollisionAlgorithm();
00248         m_dispatcher->freeCollisionAlgorithm(algor);
00249     }
00250 
00251     body0->internalSetTemporaryCollisionShape(tmpShape0);
00252     body1->internalSetTemporaryCollisionShape(tmpShape1);
00253 }
00254 
00255 void btGImpactCollisionAlgorithm::convex_vs_convex_collision(
00256                       btCollisionObject * body0,
00257                       btCollisionObject * body1,
00258                       btCollisionShape * shape0,
00259                       btCollisionShape * shape1)
00260 {
00261 
00262     btCollisionShape* tmpShape0 = body0->getCollisionShape();
00263     btCollisionShape* tmpShape1 = body1->getCollisionShape();
00264     
00265     body0->internalSetTemporaryCollisionShape(shape0);
00266     body1->internalSetTemporaryCollisionShape(shape1);
00267 
00268 
00269     m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
00270     m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
00271 
00272     checkConvexAlgorithm(body0,body1);
00273     m_convex_algorithm->processCollision(body0,body1,*m_dispatchInfo,m_resultOut);
00274 
00275     body0->internalSetTemporaryCollisionShape(tmpShape0);
00276     body1->internalSetTemporaryCollisionShape(tmpShape1);
00277 
00278 }
00279 
00280 
00281 
00282 
00283 void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs(
00284                       const btTransform & trans0,
00285                       const btTransform & trans1,
00286                       btGImpactShapeInterface * shape0,
00287                       btGImpactShapeInterface * shape1,btPairSet & pairset)
00288 {
00289     if(shape0->hasBoxSet() && shape1->hasBoxSet())
00290     {
00291         btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset);
00292     }
00293     else
00294     {
00295         btAABB boxshape0;
00296         btAABB boxshape1;
00297         int i = shape0->getNumChildShapes();
00298 
00299         while(i--)
00300         {
00301             shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
00302 
00303             int j = shape1->getNumChildShapes();
00304             while(j--)
00305             {
00306                 shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max);
00307 
00308                 if(boxshape1.has_collision(boxshape0))
00309                 {
00310                     pairset.push_pair(i,j);
00311                 }
00312             }
00313         }
00314     }
00315 
00316 
00317 }
00318 
00319 
00320 void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs(
00321                       const btTransform & trans0,
00322                       const btTransform & trans1,
00323                       btGImpactShapeInterface * shape0,
00324                       btCollisionShape * shape1,
00325                       btAlignedObjectArray<int> & collided_primitives)
00326 {
00327 
00328     btAABB boxshape;
00329 
00330 
00331     if(shape0->hasBoxSet())
00332     {
00333         btTransform trans1to0 = trans0.inverse();
00334         trans1to0 *= trans1;
00335 
00336         shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);
00337 
00338         shape0->getBoxSet()->boxQuery(boxshape, collided_primitives);
00339     }
00340     else
00341     {
00342         shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max);
00343 
00344         btAABB boxshape0;
00345         int i = shape0->getNumChildShapes();
00346 
00347         while(i--)
00348         {
00349             shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
00350 
00351             if(boxshape.has_collision(boxshape0))
00352             {
00353                 collided_primitives.push_back(i);
00354             }
00355         }
00356 
00357     }
00358 
00359 }
00360 
00361 
00362 void btGImpactCollisionAlgorithm::collide_gjk_triangles(btCollisionObject * body0,
00363                   btCollisionObject * body1,
00364                   btGImpactMeshShapePart * shape0,
00365                   btGImpactMeshShapePart * shape1,
00366                   const int * pairs, int pair_count)
00367 {
00368     btTriangleShapeEx tri0;
00369     btTriangleShapeEx tri1;
00370 
00371     shape0->lockChildShapes();
00372     shape1->lockChildShapes();
00373 
00374     const int * pair_pointer = pairs;
00375 
00376     while(pair_count--)
00377     {
00378 
00379         m_triface0 = *(pair_pointer);
00380         m_triface1 = *(pair_pointer+1);
00381         pair_pointer+=2;
00382 
00383 
00384 
00385         shape0->getBulletTriangle(m_triface0,tri0);
00386         shape1->getBulletTriangle(m_triface1,tri1);
00387 
00388 
00389         //collide two convex shapes
00390         if(tri0.overlap_test_conservative(tri1))
00391         {
00392             convex_vs_convex_collision(body0,body1,&tri0,&tri1);
00393         }
00394 
00395     }
00396 
00397     shape0->unlockChildShapes();
00398     shape1->unlockChildShapes();
00399 }
00400 
00401 void btGImpactCollisionAlgorithm::collide_sat_triangles(btCollisionObject * body0,
00402                       btCollisionObject * body1,
00403                       btGImpactMeshShapePart * shape0,
00404                       btGImpactMeshShapePart * shape1,
00405                       const int * pairs, int pair_count)
00406 {
00407     btTransform orgtrans0 = body0->getWorldTransform();
00408     btTransform orgtrans1 = body1->getWorldTransform();
00409 
00410     btPrimitiveTriangle ptri0;
00411     btPrimitiveTriangle ptri1;
00412     GIM_TRIANGLE_CONTACT contact_data;
00413 
00414     shape0->lockChildShapes();
00415     shape1->lockChildShapes();
00416 
00417     const int * pair_pointer = pairs;
00418 
00419     while(pair_count--)
00420     {
00421 
00422         m_triface0 = *(pair_pointer);
00423         m_triface1 = *(pair_pointer+1);
00424         pair_pointer+=2;
00425 
00426 
00427         shape0->getPrimitiveTriangle(m_triface0,ptri0);
00428         shape1->getPrimitiveTriangle(m_triface1,ptri1);
00429 
00430         #ifdef TRI_COLLISION_PROFILING
00431         bt_begin_gim02_tri_time();
00432         #endif
00433 
00434         ptri0.applyTransform(orgtrans0);
00435         ptri1.applyTransform(orgtrans1);
00436 
00437 
00438         //build planes
00439         ptri0.buildTriPlane();
00440         ptri1.buildTriPlane();
00441         // test conservative
00442 
00443 
00444 
00445         if(ptri0.overlap_test_conservative(ptri1))
00446         {
00447             if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data))
00448             {
00449 
00450                 int j = contact_data.m_point_count;
00451                 while(j--)
00452                 {
00453 
00454                     addContactPoint(body0, body1,
00455                                 contact_data.m_points[j],
00456                                 contact_data.m_separating_normal,
00457                                 -contact_data.m_penetration_depth);
00458                 }
00459             }
00460         }
00461 
00462         #ifdef TRI_COLLISION_PROFILING
00463         bt_end_gim02_tri_time();
00464         #endif
00465 
00466     }
00467 
00468     shape0->unlockChildShapes();
00469     shape1->unlockChildShapes();
00470 
00471 }
00472 
00473 
00474 void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(
00475                         btCollisionObject * body0,
00476                         btCollisionObject * body1,
00477                         btGImpactShapeInterface * shape0,
00478                         btGImpactShapeInterface * shape1)
00479 {
00480 
00481     if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
00482     {
00483         btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0);
00484         m_part0 = meshshape0->getMeshPartCount();
00485 
00486         while(m_part0--)
00487         {
00488             gimpact_vs_gimpact(body0,body1,meshshape0->getMeshPart(m_part0),shape1);
00489         }
00490 
00491         return;
00492     }
00493 
00494     if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
00495     {
00496         btGImpactMeshShape * meshshape1 = static_cast<btGImpactMeshShape *>(shape1);
00497         m_part1 = meshshape1->getMeshPartCount();
00498 
00499         while(m_part1--)
00500         {
00501 
00502             gimpact_vs_gimpact(body0,body1,shape0,meshshape1->getMeshPart(m_part1));
00503 
00504         }
00505 
00506         return;
00507     }
00508 
00509 
00510     btTransform orgtrans0 = body0->getWorldTransform();
00511     btTransform orgtrans1 = body1->getWorldTransform();
00512 
00513     btPairSet pairset;
00514 
00515     gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset);
00516 
00517     if(pairset.size()== 0) return;
00518 
00519     if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
00520         shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART)
00521     {
00522         btGImpactMeshShapePart * shapepart0 = static_cast<btGImpactMeshShapePart * >(shape0);
00523         btGImpactMeshShapePart * shapepart1 = static_cast<btGImpactMeshShapePart * >(shape1);
00524         //specialized function
00525         #ifdef BULLET_TRIANGLE_COLLISION
00526         collide_gjk_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
00527         #else
00528         collide_sat_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
00529         #endif
00530 
00531         return;
00532     }
00533 
00534     //general function
00535 
00536     shape0->lockChildShapes();
00537     shape1->lockChildShapes();
00538 
00539     GIM_ShapeRetriever retriever0(shape0);
00540     GIM_ShapeRetriever retriever1(shape1);
00541 
00542     bool child_has_transform0 = shape0->childrenHasTransform();
00543     bool child_has_transform1 = shape1->childrenHasTransform();
00544 
00545     int i = pairset.size();
00546     while(i--)
00547     {
00548         GIM_PAIR * pair = &pairset[i];
00549         m_triface0 = pair->m_index1;
00550         m_triface1 = pair->m_index2;
00551         btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0);
00552         btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1);
00553 
00554         if(child_has_transform0)
00555         {
00556             body0->setWorldTransform(orgtrans0*shape0->getChildTransform(m_triface0));
00557         }
00558 
00559         if(child_has_transform1)
00560         {
00561             body1->setWorldTransform(orgtrans1*shape1->getChildTransform(m_triface1));
00562         }
00563 
00564         //collide two convex shapes
00565         convex_vs_convex_collision(body0,body1,colshape0,colshape1);
00566 
00567 
00568         if(child_has_transform0)
00569         {
00570             body0->setWorldTransform(orgtrans0);
00571         }
00572 
00573         if(child_has_transform1)
00574         {
00575             body1->setWorldTransform(orgtrans1);
00576         }
00577 
00578     }
00579 
00580     shape0->unlockChildShapes();
00581     shape1->unlockChildShapes();
00582 }
00583 
00584 void btGImpactCollisionAlgorithm::gimpact_vs_shape(btCollisionObject * body0,
00585                   btCollisionObject * body1,
00586                   btGImpactShapeInterface * shape0,
00587                   btCollisionShape * shape1,bool swapped)
00588 {
00589     if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
00590     {
00591         btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0);
00592         int& part = swapped ? m_part1 : m_part0;
00593         part = meshshape0->getMeshPartCount();
00594 
00595         while(part--)
00596         {
00597 
00598             gimpact_vs_shape(body0,
00599                   body1,
00600                   meshshape0->getMeshPart(part),
00601                   shape1,swapped);
00602 
00603         }
00604 
00605         return;
00606     }
00607 
00608     #ifdef GIMPACT_VS_PLANE_COLLISION
00609     if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
00610         shape1->getShapeType() == STATIC_PLANE_PROXYTYPE)
00611     {
00612         btGImpactMeshShapePart * shapepart = static_cast<btGImpactMeshShapePart *>(shape0);
00613         btStaticPlaneShape * planeshape = static_cast<btStaticPlaneShape * >(shape1);
00614         gimpacttrimeshpart_vs_plane_collision(body0,body1,shapepart,planeshape,swapped);
00615         return;
00616     }
00617 
00618     #endif
00619 
00620 
00621 
00622     if(shape1->isCompound())
00623     {
00624         btCompoundShape * compoundshape = static_cast<btCompoundShape *>(shape1);
00625         gimpact_vs_compoundshape(body0,body1,shape0,compoundshape,swapped);
00626         return;
00627     }
00628     else if(shape1->isConcave())
00629     {
00630         btConcaveShape * concaveshape = static_cast<btConcaveShape *>(shape1);
00631         gimpact_vs_concave(body0,body1,shape0,concaveshape,swapped);
00632         return;
00633     }
00634 
00635 
00636     btTransform orgtrans0 = body0->getWorldTransform();
00637 
00638     btTransform orgtrans1 = body1->getWorldTransform();
00639 
00640     btAlignedObjectArray<int> collided_results;
00641 
00642     gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results);
00643 
00644     if(collided_results.size() == 0) return;
00645 
00646 
00647     shape0->lockChildShapes();
00648 
00649     GIM_ShapeRetriever retriever0(shape0);
00650 
00651 
00652     bool child_has_transform0 = shape0->childrenHasTransform();
00653 
00654 
00655     int i = collided_results.size();
00656 
00657     while(i--)
00658     {
00659         int child_index = collided_results[i];
00660         if(swapped)
00661             m_triface1 = child_index;
00662         else
00663             m_triface0 = child_index;
00664 
00665         btCollisionShape * colshape0 = retriever0.getChildShape(child_index);
00666 
00667         if(child_has_transform0)
00668         {
00669             body0->setWorldTransform(orgtrans0*shape0->getChildTransform(child_index));
00670         }
00671 
00672         //collide two shapes
00673         if(swapped)
00674         {
00675             shape_vs_shape_collision(body1,body0,shape1,colshape0);
00676         }
00677         else
00678         {
00679             shape_vs_shape_collision(body0,body1,colshape0,shape1);
00680         }
00681 
00682         //restore transforms
00683         if(child_has_transform0)
00684         {
00685             body0->setWorldTransform(orgtrans0);
00686         }
00687 
00688     }
00689 
00690     shape0->unlockChildShapes();
00691 
00692 }
00693 
00694 void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(btCollisionObject * body0,
00695                   btCollisionObject * body1,
00696                   btGImpactShapeInterface * shape0,
00697                   btCompoundShape * shape1,bool swapped)
00698 {
00699     btTransform orgtrans1 = body1->getWorldTransform();
00700 
00701     int i = shape1->getNumChildShapes();
00702     while(i--)
00703     {
00704 
00705         btCollisionShape * colshape1 = shape1->getChildShape(i);
00706         btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i);
00707 
00708         body1->setWorldTransform(childtrans1);
00709 
00710         //collide child shape
00711         gimpact_vs_shape(body0, body1,
00712                       shape0,colshape1,swapped);
00713 
00714 
00715         //restore transforms
00716         body1->setWorldTransform(orgtrans1);
00717     }
00718 }
00719 
00720 void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
00721                       btCollisionObject * body0,
00722                       btCollisionObject * body1,
00723                       btGImpactMeshShapePart * shape0,
00724                       btStaticPlaneShape * shape1,bool swapped)
00725 {
00726 
00727 
00728     btTransform orgtrans0 = body0->getWorldTransform();
00729     btTransform orgtrans1 = body1->getWorldTransform();
00730 
00731     btPlaneShape * planeshape = static_cast<btPlaneShape *>(shape1);
00732     btVector4 plane;
00733     planeshape->get_plane_equation_transformed(orgtrans1,plane);
00734 
00735     //test box against plane
00736 
00737     btAABB tribox;
00738     shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max);
00739     tribox.increment_margin(planeshape->getMargin());
00740 
00741     if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return;
00742 
00743     shape0->lockChildShapes();
00744 
00745     btScalar margin = shape0->getMargin() + planeshape->getMargin();
00746 
00747     btVector3 vertex;
00748     int vi = shape0->getVertexCount();
00749     while(vi--)
00750     {
00751         shape0->getVertex(vi,vertex);
00752         vertex = orgtrans0(vertex);
00753 
00754         btScalar distance = vertex.dot(plane) - plane[3] - margin;
00755 
00756         if(distance<0.0)//add contact
00757         {
00758             if(swapped)
00759             {
00760                 addContactPoint(body1, body0,
00761                     vertex,
00762                     -plane,
00763                     distance);
00764             }
00765             else
00766             {
00767                 addContactPoint(body0, body1,
00768                     vertex,
00769                     plane,
00770                     distance);
00771             }
00772         }
00773     }
00774 
00775     shape0->unlockChildShapes();
00776 }
00777 
00778 
00779 
00780 
00781 class btGImpactTriangleCallback: public btTriangleCallback
00782 {
00783 public:
00784     btGImpactCollisionAlgorithm * algorithm;
00785     btCollisionObject * body0;
00786     btCollisionObject * body1;
00787     btGImpactShapeInterface * gimpactshape0;
00788     bool swapped;
00789     btScalar margin;
00790 
00791     virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
00792     {
00793         btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]);
00794         tri1.setMargin(margin);
00795         if(swapped)
00796         {
00797             algorithm->setPart0(partId);
00798             algorithm->setFace0(triangleIndex);
00799         }
00800         else
00801         {
00802             algorithm->setPart1(partId);
00803             algorithm->setFace1(triangleIndex);
00804         }
00805         algorithm->gimpact_vs_shape(
00806                             body0,body1,gimpactshape0,&tri1,swapped);
00807     }
00808 };
00809 
00810 
00811 
00812 
00813 void btGImpactCollisionAlgorithm::gimpact_vs_concave(
00814                   btCollisionObject * body0,
00815                   btCollisionObject * body1,
00816                   btGImpactShapeInterface * shape0,
00817                   btConcaveShape * shape1,bool swapped)
00818 {
00819     //create the callback
00820     btGImpactTriangleCallback tricallback;
00821     tricallback.algorithm = this;
00822     tricallback.body0 = body0;
00823     tricallback.body1 = body1;
00824     tricallback.gimpactshape0 = shape0;
00825     tricallback.swapped = swapped;
00826     tricallback.margin = shape1->getMargin();
00827 
00828     //getting the trimesh AABB
00829     btTransform gimpactInConcaveSpace;
00830 
00831     gimpactInConcaveSpace = body1->getWorldTransform().inverse() * body0->getWorldTransform();
00832 
00833     btVector3 minAABB,maxAABB;
00834     shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB);
00835 
00836     shape1->processAllTriangles(&tricallback,minAABB,maxAABB);
00837 
00838 }
00839 
00840 
00841 
00842 void btGImpactCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00843 {
00844     clearCache();
00845 
00846     m_resultOut = resultOut;
00847     m_dispatchInfo = &dispatchInfo;
00848     btGImpactShapeInterface * gimpactshape0;
00849     btGImpactShapeInterface * gimpactshape1;
00850 
00851     if (body0->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
00852     {
00853         gimpactshape0 = static_cast<btGImpactShapeInterface *>(body0->getCollisionShape());
00854 
00855         if( body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
00856         {
00857             gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape());
00858 
00859             gimpact_vs_gimpact(body0,body1,gimpactshape0,gimpactshape1);
00860         }
00861         else
00862         {
00863             gimpact_vs_shape(body0,body1,gimpactshape0,body1->getCollisionShape(),false);
00864         }
00865 
00866     }
00867     else if (body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
00868     {
00869         gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape());
00870 
00871         gimpact_vs_shape(body1,body0,gimpactshape1,body0->getCollisionShape(),true);
00872     }
00873 }
00874 
00875 
00876 btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00877 {
00878     return 1.f;
00879 
00880 }
00881 
00883 
00884 
00885 
00887 void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher)
00888 {
00889 
00890     static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf;
00891 
00892     int i;
00893 
00894     for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
00895     {
00896         dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf);
00897     }
00898 
00899     for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
00900     {
00901         dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf);
00902     }
00903 
00904 }