Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

Last change on this file since 3188 was 2882, checked in by rgrieder, 16 years ago

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