Back to index

supertuxkart  0.5+dfsg1
btSimulationIslandManager.cpp
Go to the documentation of this file.
00001 
00002 
00003 #include "LinearMath/btScalar.h"
00004 #include "btSimulationIslandManager.h"
00005 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
00006 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
00007 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00008 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
00009 
00010 //#include <stdio.h>
00011 #include "LinearMath/btQuickprof.h"
00012 
00013 btSimulationIslandManager::btSimulationIslandManager()
00014 {
00015 }
00016 
00017 btSimulationIslandManager::~btSimulationIslandManager()
00018 {
00019 }
00020 
00021 
00022 void btSimulationIslandManager::initUnionFind(int n)
00023 {
00024               m_unionFind.reset(n);
00025 }
00026               
00027 
00028 void btSimulationIslandManager::findUnions(btDispatcher* dispatcher,btCollisionWorld* colWorld)
00029 {
00030        
00031        {
00032               btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
00033 
00034               for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++)
00035               {
00036                      const btBroadphasePair& collisionPair = pairPtr[i];
00037                      btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
00038                      btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
00039 
00040                      if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
00041                             ((colObj1) && ((colObj1)->mergesSimulationIslands())))
00042                      {
00043 
00044                             m_unionFind.unite((colObj0)->getIslandTag(),
00045                                    (colObj1)->getIslandTag());
00046                      }
00047               }
00048        }
00049 }
00050 
00051 
00052 void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
00053 {
00054        
00055        initUnionFind( int (colWorld->getCollisionObjectArray().size()));
00056        
00057        // put the index into m_controllers into m_tag   
00058        {
00059               
00060               int index = 0;
00061               int i;
00062               for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
00063               {
00064                      btCollisionObject*   collisionObject= colWorld->getCollisionObjectArray()[i];
00065                      collisionObject->setIslandTag(index);
00066                      collisionObject->setCompanionId(-1);
00067                      collisionObject->setHitFraction(btScalar(1.));
00068                      index++;
00069                      
00070               }
00071        }
00072        // do the union find
00073        
00074        findUnions(dispatcher,colWorld);
00075        
00076 
00077        
00078 }
00079 
00080 
00081 
00082 
00083 void   btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
00084 {
00085        // put the islandId ('find' value) into m_tag    
00086        {
00087               
00088               
00089               int index = 0;
00090               int i;
00091               for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
00092               {
00093                      btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
00094                      if (collisionObject->mergesSimulationIslands())
00095                      {
00096                             collisionObject->setIslandTag( m_unionFind.find(index) );
00097                             collisionObject->setCompanionId(-1);
00098                      } else
00099                      {
00100                             collisionObject->setIslandTag(-1);
00101                             collisionObject->setCompanionId(-2);
00102                      }
00103                      index++;
00104               }
00105        }
00106 }
00107 
00108 inline int    getIslandId(const btPersistentManifold* lhs)
00109 {
00110        int islandId;
00111        const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
00112        const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
00113        islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
00114        return islandId;
00115 
00116 }
00117 
00118 
00119 
00121 class btPersistentManifoldSortPredicate
00122 {
00123        public:
00124 
00125               SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs )
00126               {
00127                      return getIslandId(lhs) < getIslandId(rhs);
00128               }
00129 };
00130 
00131 
00132 
00133 
00134 
00135 //
00136 // todo: this is random access, it can be walked 'cache friendly'!
00137 //
00138 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback)
00139 {
00140 
00141        BT_PROFILE("islandUnionFindAndQuickSort");
00142        
00143        //we are going to sort the unionfind array, and store the element id in the size
00144        //afterwards, we clean unionfind, to make sure no-one uses it anymore
00145        
00146        getUnionFind().sortIslands();
00147        int numElem = getUnionFind().getNumElements();
00148 
00149        int endIslandIndex=1;
00150        int startIslandIndex;
00151 
00152 
00153        //update the sleeping state for bodies, if all are sleeping
00154        for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
00155        {
00156               int islandId = getUnionFind().getElement(startIslandIndex).m_id;
00157               for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
00158               {
00159               }
00160 
00161               //int numSleeping = 0;
00162 
00163               bool allSleeping = true;
00164 
00165               int idx;
00166               for (idx=startIslandIndex;idx<endIslandIndex;idx++)
00167               {
00168                      int i = getUnionFind().getElement(idx).m_sz;
00169 
00170                      btCollisionObject* colObj0 = collisionObjects[i];
00171                      if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
00172                      {
00173 //                          printf("error in island management\n");
00174                      }
00175 
00176                      assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
00177                      if (colObj0->getIslandTag() == islandId)
00178                      {
00179                             if (colObj0->getActivationState()== ACTIVE_TAG)
00180                             {
00181                                    allSleeping = false;
00182                             }
00183                             if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
00184                             {
00185                                    allSleeping = false;
00186                             }
00187                      }
00188               }
00189                      
00190 
00191               if (allSleeping)
00192               {
00193                      int idx;
00194                      for (idx=startIslandIndex;idx<endIslandIndex;idx++)
00195                      {
00196                             int i = getUnionFind().getElement(idx).m_sz;
00197                             btCollisionObject* colObj0 = collisionObjects[i];
00198                             if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
00199                             {
00200 //                                 printf("error in island management\n");
00201                             }
00202 
00203                             assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
00204 
00205                             if (colObj0->getIslandTag() == islandId)
00206                             {
00207                                    colObj0->setActivationState( ISLAND_SLEEPING );
00208                             }
00209                      }
00210               } else
00211               {
00212 
00213                      int idx;
00214                      for (idx=startIslandIndex;idx<endIslandIndex;idx++)
00215                      {
00216                             int i = getUnionFind().getElement(idx).m_sz;
00217 
00218                             btCollisionObject* colObj0 = collisionObjects[i];
00219                             if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
00220                             {
00221 //                                 printf("error in island management\n");
00222                             }
00223 
00224                             assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
00225 
00226                             if (colObj0->getIslandTag() == islandId)
00227                             {
00228                                    if ( colObj0->getActivationState() == ISLAND_SLEEPING)
00229                                    {
00230                                           colObj0->setActivationState( WANTS_DEACTIVATION);
00231                                    }
00232                             }
00233                      }
00234               }
00235        }
00236 
00237        
00238        int i;
00239        int maxNumManifolds = dispatcher->getNumManifolds();
00240 
00241 #define SPLIT_ISLANDS 1
00242 #ifdef SPLIT_ISLANDS
00243 
00244        
00245 #endif //SPLIT_ISLANDS
00246 
00247        
00248        for (i=0;i<maxNumManifolds ;i++)
00249        {
00250                btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
00251                
00252                btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
00253                btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
00254               
00255                //todo: check sleeping conditions!
00256                if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
00257                      ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
00258               {
00259               
00260                      //kinematic objects don't merge islands, but wake up all connected objects
00261                      if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
00262                      {
00263                             colObj1->activate();
00264                      }
00265                      if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
00266                      {
00267                             colObj0->activate();
00268                      }
00269 #ifdef SPLIT_ISLANDS
00270        //            //filtering for response
00271                      if (dispatcher->needsResponse(colObj0,colObj1))
00272                             m_islandmanifold.push_back(manifold);
00273 #endif //SPLIT_ISLANDS
00274               }
00275        }
00276 
00277 #ifndef SPLIT_ISLANDS
00278        btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
00279        
00280        callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
00281 #else
00282        // Sort manifolds, based on islands
00283        // Sort the vector using predicate and std::sort
00284        //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
00285 
00286        int numManifolds = int (m_islandmanifold.size());
00287 
00288        //we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
00289        m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
00290 
00291        //now process all active islands (sets of manifolds for now)
00292 
00293        int startManifoldIndex = 0;
00294        int endManifoldIndex = 1;
00295 
00296        //int islandId;
00297 
00298        
00299 
00300 //     printf("Start Islands\n");
00301 
00302        //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
00303        for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
00304        {
00305               int islandId = getUnionFind().getElement(startIslandIndex).m_id;
00306 
00307 
00308               bool islandSleeping = false;
00309                 
00310                 for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
00311                 {
00312                         int i = getUnionFind().getElement(endIslandIndex).m_sz;
00313                         btCollisionObject* colObj0 = collisionObjects[i];
00314                                           m_islandBodies.push_back(colObj0);
00315                         if (!colObj0->isActive())
00316                                 islandSleeping = true;
00317                 }
00318                 
00319 
00320               //find the accompanying contact manifold for this islandId
00321               int numIslandManifolds = 0;
00322               btPersistentManifold** startManifold = 0;
00323 
00324               if (startManifoldIndex<numManifolds)
00325               {
00326                      int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
00327                      if (curIslandId == islandId)
00328                      {
00329                             startManifold = &m_islandmanifold[startManifoldIndex];
00330                      
00331                             for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
00332                             {
00333 
00334                             }
00336                             numIslandManifolds = endManifoldIndex-startManifoldIndex;
00337                      }
00338 
00339               }
00340 
00341               if (!islandSleeping)
00342               {
00343                      callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
00344 //                   printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
00345               }
00346               
00347               if (numIslandManifolds)
00348               {
00349                      startManifoldIndex = endManifoldIndex;
00350               }
00351 
00352               m_islandBodies.resize(0);
00353        }
00354 #endif //SPLIT_ISLANDS
00355 
00356        m_islandmanifold.resize(0);
00357 }