Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @ 8393

Last change on this file since 8393 was 8393, checked in by rgrieder, 13 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

  • Property svn:eol-style set to native
File size: 12.6 KB
Line 
1
2/*
3Bullet Continuous Collision Detection and Physics Library
4Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
5
6This software is provided 'as-is', without any express or implied warranty.
7In no event will the authors be held liable for any damages arising from the use of this software.
8Permission is granted to anyone to use this software for any purpose,
9including commercial applications, and to alter it and redistribute it freely,
10subject to the following restrictions:
11
121. 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.
132. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
143. This notice may not be removed or altered from any source distribution.
15*/
16
17
18#include "LinearMath/btScalar.h"
19#include "btSimulationIslandManager.h"
20#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
21#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
22#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
23#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
24
25//#include <stdio.h>
26#include "LinearMath/btQuickprof.h"
27
28btSimulationIslandManager::btSimulationIslandManager():
29m_splitIslands(true)
30{
31}
32
33btSimulationIslandManager::~btSimulationIslandManager()
34{
35}
36
37
38void btSimulationIslandManager::initUnionFind(int n)
39{
40                m_unionFind.reset(n);
41}
42               
43
44void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld)
45{
46       
47        {
48                btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
49                const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
50                btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
51               
52                for (int i=0;i<numOverlappingPairs;i++)
53                {
54                        const btBroadphasePair& collisionPair = pairPtr[i];
55                        btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
56                        btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
57
58                        if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
59                                ((colObj1) && ((colObj1)->mergesSimulationIslands())))
60                        {
61
62                                m_unionFind.unite((colObj0)->getIslandTag(),
63                                        (colObj1)->getIslandTag());
64                        }
65                }
66        }
67}
68
69#ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
70void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
71{
72
73        // put the index into m_controllers into m_tag   
74        int index = 0;
75        {
76
77                int i;
78                for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
79                {
80                        btCollisionObject*   collisionObject= colWorld->getCollisionObjectArray()[i];
81                        //Adding filtering here
82                        if (!collisionObject->isStaticOrKinematicObject())
83                        {
84                                collisionObject->setIslandTag(index++);
85                        }
86                        collisionObject->setCompanionId(-1);
87                        collisionObject->setHitFraction(btScalar(1.));
88                }
89        }
90        // do the union find
91
92        initUnionFind( index );
93
94        findUnions(dispatcher,colWorld);
95}
96
97void   btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
98{
99        // put the islandId ('find' value) into m_tag   
100        {
101                int index = 0;
102                int i;
103                for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
104                {
105                        btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
106                        if (!collisionObject->isStaticOrKinematicObject())
107                        {
108                                collisionObject->setIslandTag( m_unionFind.find(index) );
109                                //Set the correct object offset in Collision Object Array
110                                m_unionFind.getElement(index).m_sz = i;
111                                collisionObject->setCompanionId(-1);
112                                index++;
113                        } else
114                        {
115                                collisionObject->setIslandTag(-1);
116                                collisionObject->setCompanionId(-2);
117                        }
118                }
119        }
120}
121
122
123#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
124void    btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
125{
126
127        initUnionFind( int (colWorld->getCollisionObjectArray().size()));
128
129        // put the index into m_controllers into m_tag 
130        {
131
132                int index = 0;
133                int i;
134                for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
135                {
136                        btCollisionObject*      collisionObject= colWorld->getCollisionObjectArray()[i];
137                        collisionObject->setIslandTag(index);
138                        collisionObject->setCompanionId(-1);
139                        collisionObject->setHitFraction(btScalar(1.));
140                        index++;
141
142                }
143        }
144        // do the union find
145
146        findUnions(dispatcher,colWorld);
147}
148
149void    btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
150{
151        // put the islandId ('find' value) into m_tag   
152        {
153
154
155                int index = 0;
156                int i;
157                for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
158                {
159                        btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
160                        if (!collisionObject->isStaticOrKinematicObject())
161                        {
162                                collisionObject->setIslandTag( m_unionFind.find(index) );
163                                collisionObject->setCompanionId(-1);
164                        } else
165                        {
166                                collisionObject->setIslandTag(-1);
167                                collisionObject->setCompanionId(-2);
168                        }
169                        index++;
170                }
171        }
172}
173
174#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
175
176inline  int     getIslandId(const btPersistentManifold* lhs)
177{
178        int islandId;
179        const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
180        const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
181        islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
182        return islandId;
183
184}
185
186
187
188/// function object that routes calls to operator<
189class btPersistentManifoldSortPredicate
190{
191        public:
192
193                SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs )
194                {
195                        return getIslandId(lhs) < getIslandId(rhs);
196                }
197};
198
199
200void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
201{
202
203        BT_PROFILE("islandUnionFindAndQuickSort");
204       
205        btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
206
207        m_islandmanifold.resize(0);
208
209        //we are going to sort the unionfind array, and store the element id in the size
210        //afterwards, we clean unionfind, to make sure no-one uses it anymore
211       
212        getUnionFind().sortIslands();
213        int numElem = getUnionFind().getNumElements();
214
215        int endIslandIndex=1;
216        int startIslandIndex;
217
218
219        //update the sleeping state for bodies, if all are sleeping
220        for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
221        {
222                int islandId = getUnionFind().getElement(startIslandIndex).m_id;
223                for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
224                {
225                }
226
227                //int numSleeping = 0;
228
229                bool allSleeping = true;
230
231                int idx;
232                for (idx=startIslandIndex;idx<endIslandIndex;idx++)
233                {
234                        int i = getUnionFind().getElement(idx).m_sz;
235
236                        btCollisionObject* colObj0 = collisionObjects[i];
237                        if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
238                        {
239//                              printf("error in island management\n");
240                        }
241
242                        btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
243                        if (colObj0->getIslandTag() == islandId)
244                        {
245                                if (colObj0->getActivationState()== ACTIVE_TAG)
246                                {
247                                        allSleeping = false;
248                                }
249                                if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
250                                {
251                                        allSleeping = false;
252                                }
253                        }
254                }
255                       
256
257                if (allSleeping)
258                {
259                        int idx;
260                        for (idx=startIslandIndex;idx<endIslandIndex;idx++)
261                        {
262                                int i = getUnionFind().getElement(idx).m_sz;
263                                btCollisionObject* colObj0 = collisionObjects[i];
264                                if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
265                                {
266//                                      printf("error in island management\n");
267                                }
268
269                                btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
270
271                                if (colObj0->getIslandTag() == islandId)
272                                {
273                                        colObj0->setActivationState( ISLAND_SLEEPING );
274                                }
275                        }
276                } else
277                {
278
279                        int idx;
280                        for (idx=startIslandIndex;idx<endIslandIndex;idx++)
281                        {
282                                int i = getUnionFind().getElement(idx).m_sz;
283
284                                btCollisionObject* colObj0 = collisionObjects[i];
285                                if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
286                                {
287//                                      printf("error in island management\n");
288                                }
289
290                                btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
291
292                                if (colObj0->getIslandTag() == islandId)
293                                {
294                                        if ( colObj0->getActivationState() == ISLAND_SLEEPING)
295                                        {
296                                                colObj0->setActivationState( WANTS_DEACTIVATION);
297                                                colObj0->setDeactivationTime(0.f);
298                                        }
299                                }
300                        }
301                }
302        }
303
304       
305        int i;
306        int maxNumManifolds = dispatcher->getNumManifolds();
307
308//#define SPLIT_ISLANDS 1
309//#ifdef SPLIT_ISLANDS
310
311       
312//#endif //SPLIT_ISLANDS
313
314       
315        for (i=0;i<maxNumManifolds ;i++)
316        {
317                 btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
318                 
319                 btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
320                 btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
321               
322                 ///@todo: check sleeping conditions!
323                 if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
324                        ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
325                {
326               
327                        //kinematic objects don't merge islands, but wake up all connected objects
328                        if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
329                        {
330                                colObj1->activate();
331                        }
332                        if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
333                        {
334                                colObj0->activate();
335                        }
336                        if(m_splitIslands)
337                        { 
338                                //filtering for response
339                                if (dispatcher->needsResponse(colObj0,colObj1))
340                                        m_islandmanifold.push_back(manifold);
341                        }
342                }
343        }
344}
345
346
347
348///@todo: this is random access, it can be walked 'cache friendly'!
349void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
350{
351        btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
352
353        buildIslands(dispatcher,collisionWorld);
354
355        int endIslandIndex=1;
356        int startIslandIndex;
357        int numElem = getUnionFind().getNumElements();
358
359        BT_PROFILE("processIslands");
360
361        if(!m_splitIslands)
362        {
363                btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
364                int maxNumManifolds = dispatcher->getNumManifolds();
365                callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
366        }
367        else
368        {
369                // Sort manifolds, based on islands
370                // Sort the vector using predicate and std::sort
371                //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
372
373                int numManifolds = int (m_islandmanifold.size());
374
375                //we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
376                m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
377
378                //now process all active islands (sets of manifolds for now)
379
380                int startManifoldIndex = 0;
381                int endManifoldIndex = 1;
382
383                //int islandId;
384
385               
386
387        //      printf("Start Islands\n");
388
389                //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
390                for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
391                {
392                        int islandId = getUnionFind().getElement(startIslandIndex).m_id;
393
394
395                           bool islandSleeping = true;
396                       
397                                        for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
398                                        {
399                                                        int i = getUnionFind().getElement(endIslandIndex).m_sz;
400                                                        btCollisionObject* colObj0 = collisionObjects[i];
401                                                        m_islandBodies.push_back(colObj0);
402                                                        if (colObj0->isActive())
403                                                                        islandSleeping = false;
404                                        }
405                       
406
407                        //find the accompanying contact manifold for this islandId
408                        int numIslandManifolds = 0;
409                        btPersistentManifold** startManifold = 0;
410
411                        if (startManifoldIndex<numManifolds)
412                        {
413                                int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
414                                if (curIslandId == islandId)
415                                {
416                                        startManifold = &m_islandmanifold[startManifoldIndex];
417                               
418                                        for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
419                                        {
420
421                                        }
422                                        /// Process the actual simulation, only if not sleeping/deactivated
423                                        numIslandManifolds = endManifoldIndex-startManifoldIndex;
424                                }
425
426                        }
427
428                        if (!islandSleeping)
429                        {
430                                callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
431        //                      printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
432                        }
433                       
434                        if (numIslandManifolds)
435                        {
436                                startManifoldIndex = endManifoldIndex;
437                        }
438
439                        m_islandBodies.resize(0);
440                }
441        } // else if(!splitIslands)
442
443}
Note: See TracBrowser for help on using the repository browser.