btSimulationIslandManager.cpp

Go to the documentation of this file.
00001 
00002 /*
00003 Bullet Continuous Collision Detection and Physics Library
00004 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00005 
00006 This software is provided 'as-is', without any express or implied warranty.
00007 In no event will the authors be held liable for any damages arising from the use of this software.
00008 Permission is granted to anyone to use this software for any purpose, 
00009 including commercial applications, and to alter it and redistribute it freely, 
00010 subject to the following restrictions:
00011 
00012 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.
00013 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00014 3. This notice may not be removed or altered from any source distribution.
00015 */
00016 
00017 
00018 #include "LinearMath/btScalar.h"
00019 #include "btSimulationIslandManager.h"
00020 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
00021 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
00022 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00023 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
00024 
00025 //#include <stdio.h>
00026 #include "LinearMath/btQuickprof.h"
00027 
00028 btSimulationIslandManager::btSimulationIslandManager():
00029 m_splitIslands(true)
00030 {
00031 }
00032 
00033 btSimulationIslandManager::~btSimulationIslandManager()
00034 {
00035 }
00036 
00037 
00038 void btSimulationIslandManager::initUnionFind(int n)
00039 {
00040                 m_unionFind.reset(n);
00041 }
00042                 
00043 
00044 void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld)
00045 {
00046         
00047         {
00048                 btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
00049                 const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
00050                 if (numOverlappingPairs)
00051                 {
00052                 btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
00053                 
00054                 for (int i=0;i<numOverlappingPairs;i++)
00055                 {
00056                         const btBroadphasePair& collisionPair = pairPtr[i];
00057                         btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
00058                         btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
00059 
00060                         if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
00061                                 ((colObj1) && ((colObj1)->mergesSimulationIslands())))
00062                         {
00063 
00064                                 m_unionFind.unite((colObj0)->getIslandTag(),
00065                                         (colObj1)->getIslandTag());
00066                         }
00067                 }
00068                 }
00069         }
00070 }
00071 
00072 #ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
00073 void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
00074 {
00075 
00076         // put the index into m_controllers into m_tag   
00077         int index = 0;
00078         {
00079 
00080                 int i;
00081                 for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
00082                 {
00083                         btCollisionObject*   collisionObject= colWorld->getCollisionObjectArray()[i];
00084                         //Adding filtering here
00085                         if (!collisionObject->isStaticOrKinematicObject())
00086                         {
00087                                 collisionObject->setIslandTag(index++);
00088                         }
00089                         collisionObject->setCompanionId(-1);
00090                         collisionObject->setHitFraction(btScalar(1.));
00091                 }
00092         }
00093         // do the union find
00094 
00095         initUnionFind( index );
00096 
00097         findUnions(dispatcher,colWorld);
00098 }
00099 
00100 void   btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
00101 {
00102         // put the islandId ('find' value) into m_tag   
00103         {
00104                 int index = 0;
00105                 int i;
00106                 for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
00107                 {
00108                         btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
00109                         if (!collisionObject->isStaticOrKinematicObject())
00110                         {
00111                                 collisionObject->setIslandTag( m_unionFind.find(index) );
00112                                 //Set the correct object offset in Collision Object Array
00113                                 m_unionFind.getElement(index).m_sz = i;
00114                                 collisionObject->setCompanionId(-1);
00115                                 index++;
00116                         } else
00117                         {
00118                                 collisionObject->setIslandTag(-1);
00119                                 collisionObject->setCompanionId(-2);
00120                         }
00121                 }
00122         }
00123 }
00124 
00125 
00126 #else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
00127 void    btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
00128 {
00129 
00130         initUnionFind( int (colWorld->getCollisionObjectArray().size()));
00131 
00132         // put the index into m_controllers into m_tag  
00133         {
00134 
00135                 int index = 0;
00136                 int i;
00137                 for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
00138                 {
00139                         btCollisionObject*      collisionObject= colWorld->getCollisionObjectArray()[i];
00140                         collisionObject->setIslandTag(index);
00141                         collisionObject->setCompanionId(-1);
00142                         collisionObject->setHitFraction(btScalar(1.));
00143                         index++;
00144 
00145                 }
00146         }
00147         // do the union find
00148 
00149         findUnions(dispatcher,colWorld);
00150 }
00151 
00152 void    btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
00153 {
00154         // put the islandId ('find' value) into m_tag   
00155         {
00156 
00157 
00158                 int index = 0;
00159                 int i;
00160                 for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
00161                 {
00162                         btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
00163                         if (!collisionObject->isStaticOrKinematicObject())
00164                         {
00165                                 collisionObject->setIslandTag( m_unionFind.find(index) );
00166                                 collisionObject->setCompanionId(-1);
00167                         } else
00168                         {
00169                                 collisionObject->setIslandTag(-1);
00170                                 collisionObject->setCompanionId(-2);
00171                         }
00172                         index++;
00173                 }
00174         }
00175 }
00176 
00177 #endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
00178 
00179 inline  int     getIslandId(const btPersistentManifold* lhs)
00180 {
00181         int islandId;
00182         const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
00183         const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
00184         islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
00185         return islandId;
00186 
00187 }
00188 
00189 
00190 
00192 class btPersistentManifoldSortPredicate
00193 {
00194         public:
00195 
00196                 SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const
00197                 {
00198                         return getIslandId(lhs) < getIslandId(rhs);
00199                 }
00200 };
00201 
00202 
00203 void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
00204 {
00205 
00206         BT_PROFILE("islandUnionFindAndQuickSort");
00207         
00208         btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
00209 
00210         m_islandmanifold.resize(0);
00211 
00212         //we are going to sort the unionfind array, and store the element id in the size
00213         //afterwards, we clean unionfind, to make sure no-one uses it anymore
00214         
00215         getUnionFind().sortIslands();
00216         int numElem = getUnionFind().getNumElements();
00217 
00218         int endIslandIndex=1;
00219         int startIslandIndex;
00220 
00221 
00222         //update the sleeping state for bodies, if all are sleeping
00223         for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
00224         {
00225                 int islandId = getUnionFind().getElement(startIslandIndex).m_id;
00226                 for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
00227                 {
00228                 }
00229 
00230                 //int numSleeping = 0;
00231 
00232                 bool allSleeping = true;
00233 
00234                 int idx;
00235                 for (idx=startIslandIndex;idx<endIslandIndex;idx++)
00236                 {
00237                         int i = getUnionFind().getElement(idx).m_sz;
00238 
00239                         btCollisionObject* colObj0 = collisionObjects[i];
00240                         if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
00241                         {
00242 //                              printf("error in island management\n");
00243                         }
00244 
00245                         btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
00246                         if (colObj0->getIslandTag() == islandId)
00247                         {
00248                                 if (colObj0->getActivationState()== ACTIVE_TAG)
00249                                 {
00250                                         allSleeping = false;
00251                                 }
00252                                 if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
00253                                 {
00254                                         allSleeping = false;
00255                                 }
00256                         }
00257                 }
00258                         
00259 
00260                 if (allSleeping)
00261                 {
00262                         int idx;
00263                         for (idx=startIslandIndex;idx<endIslandIndex;idx++)
00264                         {
00265                                 int i = getUnionFind().getElement(idx).m_sz;
00266                                 btCollisionObject* colObj0 = collisionObjects[i];
00267                                 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
00268                                 {
00269 //                                      printf("error in island management\n");
00270                                 }
00271 
00272                                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
00273 
00274                                 if (colObj0->getIslandTag() == islandId)
00275                                 {
00276                                         colObj0->setActivationState( ISLAND_SLEEPING );
00277                                 }
00278                         }
00279                 } else
00280                 {
00281 
00282                         int idx;
00283                         for (idx=startIslandIndex;idx<endIslandIndex;idx++)
00284                         {
00285                                 int i = getUnionFind().getElement(idx).m_sz;
00286 
00287                                 btCollisionObject* colObj0 = collisionObjects[i];
00288                                 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
00289                                 {
00290 //                                      printf("error in island management\n");
00291                                 }
00292 
00293                                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
00294 
00295                                 if (colObj0->getIslandTag() == islandId)
00296                                 {
00297                                         if ( colObj0->getActivationState() == ISLAND_SLEEPING)
00298                                         {
00299                                                 colObj0->setActivationState( WANTS_DEACTIVATION);
00300                                                 colObj0->setDeactivationTime(0.f);
00301                                         }
00302                                 }
00303                         }
00304                 }
00305         }
00306 
00307         
00308         int i;
00309         int maxNumManifolds = dispatcher->getNumManifolds();
00310 
00311 //#define SPLIT_ISLANDS 1
00312 //#ifdef SPLIT_ISLANDS
00313 
00314         
00315 //#endif //SPLIT_ISLANDS
00316 
00317         
00318         for (i=0;i<maxNumManifolds ;i++)
00319         {
00320                  btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
00321                  
00322                  const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
00323                  const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
00324                 
00326                  if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
00327                         ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
00328                 {
00329                 
00330                         //kinematic objects don't merge islands, but wake up all connected objects
00331                         if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
00332                         {
00333                                 if (colObj0->hasContactResponse())
00334                                         colObj1->activate();
00335                         }
00336                         if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
00337                         {
00338                                 if (colObj1->hasContactResponse())
00339                                         colObj0->activate();
00340                         }
00341                         if(m_splitIslands)
00342                         { 
00343                                 //filtering for response
00344                                 if (dispatcher->needsResponse(colObj0,colObj1))
00345                                         m_islandmanifold.push_back(manifold);
00346                         }
00347                 }
00348         }
00349 }
00350 
00351 
00352 
00354 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
00355 {
00356         btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
00357 
00358         buildIslands(dispatcher,collisionWorld);
00359 
00360         int endIslandIndex=1;
00361         int startIslandIndex;
00362         int numElem = getUnionFind().getNumElements();
00363 
00364         BT_PROFILE("processIslands");
00365 
00366         if(!m_splitIslands)
00367         {
00368                 btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
00369                 int maxNumManifolds = dispatcher->getNumManifolds();
00370                 callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
00371         }
00372         else
00373         {
00374                 // Sort manifolds, based on islands
00375                 // Sort the vector using predicate and std::sort
00376                 //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
00377 
00378                 int numManifolds = int (m_islandmanifold.size());
00379 
00380                 //tried a radix sort, but quicksort/heapsort seems still faster
00381                 //@todo rewrite island management
00382                 m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
00383                 //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
00384 
00385                 //now process all active islands (sets of manifolds for now)
00386 
00387                 int startManifoldIndex = 0;
00388                 int endManifoldIndex = 1;
00389 
00390                 //int islandId;
00391 
00392                 
00393 
00394         //      printf("Start Islands\n");
00395 
00396                 //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
00397                 for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
00398                 {
00399                         int islandId = getUnionFind().getElement(startIslandIndex).m_id;
00400 
00401 
00402                            bool islandSleeping = true;
00403                         
00404                                         for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
00405                                         {
00406                                                         int i = getUnionFind().getElement(endIslandIndex).m_sz;
00407                                                         btCollisionObject* colObj0 = collisionObjects[i];
00408                                                         m_islandBodies.push_back(colObj0);
00409                                                         if (colObj0->isActive())
00410                                                                         islandSleeping = false;
00411                                         }
00412                         
00413 
00414                         //find the accompanying contact manifold for this islandId
00415                         int numIslandManifolds = 0;
00416                         btPersistentManifold** startManifold = 0;
00417 
00418                         if (startManifoldIndex<numManifolds)
00419                         {
00420                                 int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
00421                                 if (curIslandId == islandId)
00422                                 {
00423                                         startManifold = &m_islandmanifold[startManifoldIndex];
00424                                 
00425                                         for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
00426                                         {
00427 
00428                                         }
00430                                         numIslandManifolds = endManifoldIndex-startManifoldIndex;
00431                                 }
00432 
00433                         }
00434 
00435                         if (!islandSleeping)
00436                         {
00437                                 callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
00438         //                      printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
00439                         }
00440                         
00441                         if (numIslandManifolds)
00442                         {
00443                                 startManifoldIndex = endManifoldIndex;
00444                         }
00445 
00446                         m_islandBodies.resize(0);
00447                 }
00448         } // else if(!splitIslands) 
00449 
00450 }