← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
 
1
 
 
2
/*
 
3
Bullet Continuous Collision Detection and Physics Library
 
4
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
5
 
 
6
This software is provided 'as-is', without any express or implied warranty.
 
7
In no event will the authors be held liable for any damages arising from the use of this software.
 
8
Permission is granted to anyone to use this software for any purpose, 
 
9
including commercial applications, and to alter it and redistribute it freely, 
 
10
subject to the following restrictions:
 
11
 
 
12
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.
 
13
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
 
14
3. 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
 
 
28
btSimulationIslandManager::btSimulationIslandManager():
 
29
m_splitIslands(true)
 
30
{
 
31
}
 
32
 
 
33
btSimulationIslandManager::~btSimulationIslandManager()
 
34
{
 
35
}
 
36
 
 
37
 
 
38
void btSimulationIslandManager::initUnionFind(int n)
 
39
{
 
40
                m_unionFind.reset(n);
 
41
}
 
42
                
 
43
 
 
44
void 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
 
70
void   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
 
 
97
void   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
 
124
void    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
 
 
149
void    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
 
 
176
inline  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<
 
189
class 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
 
 
200
void 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'!
 
349
void 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
}