Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @ 2259

Last change on this file since 2259 was 2192, checked in by rgrieder, 16 years ago

Reverted all changes of attempt to update physics branch.

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