bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / 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                 if (numOverlappingPairs)
51                 {
52                 btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
53                 
54                 for (int i=0;i<numOverlappingPairs;i++)
55                 {
56                         const btBroadphasePair& collisionPair = pairPtr[i];
57                         btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
58                         btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
59
60                         if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
61                                 ((colObj1) && ((colObj1)->mergesSimulationIslands())))
62                         {
63
64                                 m_unionFind.unite((colObj0)->getIslandTag(),
65                                         (colObj1)->getIslandTag());
66                         }
67                 }
68                 }
69         }
70 }
71
72 #ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
73 void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
74 {
75
76         // put the index into m_controllers into m_tag   
77         int index = 0;
78         {
79
80                 int i;
81                 for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
82                 {
83                         btCollisionObject*   collisionObject= colWorld->getCollisionObjectArray()[i];
84                         //Adding filtering here
85                         if (!collisionObject->isStaticOrKinematicObject())
86                         {
87                                 collisionObject->setIslandTag(index++);
88                         }
89                         collisionObject->setCompanionId(-1);
90                         collisionObject->setHitFraction(btScalar(1.));
91                 }
92         }
93         // do the union find
94
95         initUnionFind( index );
96
97         findUnions(dispatcher,colWorld);
98 }
99
100 void   btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
101 {
102         // put the islandId ('find' value) into m_tag   
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                                 //Set the correct object offset in Collision Object Array
113                                 m_unionFind.getElement(index).m_sz = i;
114                                 collisionObject->setCompanionId(-1);
115                                 index++;
116                         } else
117                         {
118                                 collisionObject->setIslandTag(-1);
119                                 collisionObject->setCompanionId(-2);
120                         }
121                 }
122         }
123 }
124
125
126 #else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
127 void    btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
128 {
129
130         initUnionFind( int (colWorld->getCollisionObjectArray().size()));
131
132         // put the index into m_controllers into m_tag  
133         {
134
135                 int index = 0;
136                 int i;
137                 for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
138                 {
139                         btCollisionObject*      collisionObject= colWorld->getCollisionObjectArray()[i];
140                         collisionObject->setIslandTag(index);
141                         collisionObject->setCompanionId(-1);
142                         collisionObject->setHitFraction(btScalar(1.));
143                         index++;
144
145                 }
146         }
147         // do the union find
148
149         findUnions(dispatcher,colWorld);
150 }
151
152 void    btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
153 {
154         // put the islandId ('find' value) into m_tag   
155         {
156
157
158                 int index = 0;
159                 int i;
160                 for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
161                 {
162                         btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
163                         if (!collisionObject->isStaticOrKinematicObject())
164                         {
165                                 collisionObject->setIslandTag( m_unionFind.find(index) );
166                                 collisionObject->setCompanionId(-1);
167                         } else
168                         {
169                                 collisionObject->setIslandTag(-1);
170                                 collisionObject->setCompanionId(-2);
171                         }
172                         index++;
173                 }
174         }
175 }
176
177 #endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
178
179 inline  int     getIslandId(const btPersistentManifold* lhs)
180 {
181         int islandId;
182         const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
183         const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
184         islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
185         return islandId;
186
187 }
188
189
190
191 /// function object that routes calls to operator<
192 class btPersistentManifoldSortPredicate
193 {
194         public:
195
196                 SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const
197                 {
198                         return getIslandId(lhs) < getIslandId(rhs);
199                 }
200 };
201
202
203 void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
204 {
205
206         BT_PROFILE("islandUnionFindAndQuickSort");
207         
208         btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
209
210         m_islandmanifold.resize(0);
211
212         //we are going to sort the unionfind array, and store the element id in the size
213         //afterwards, we clean unionfind, to make sure no-one uses it anymore
214         
215         getUnionFind().sortIslands();
216         int numElem = getUnionFind().getNumElements();
217
218         int endIslandIndex=1;
219         int startIslandIndex;
220
221
222         //update the sleeping state for bodies, if all are sleeping
223         for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
224         {
225                 int islandId = getUnionFind().getElement(startIslandIndex).m_id;
226                 for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
227                 {
228                 }
229
230                 //int numSleeping = 0;
231
232                 bool allSleeping = true;
233
234                 int idx;
235                 for (idx=startIslandIndex;idx<endIslandIndex;idx++)
236                 {
237                         int i = getUnionFind().getElement(idx).m_sz;
238
239                         btCollisionObject* colObj0 = collisionObjects[i];
240                         if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
241                         {
242 //                              printf("error in island management\n");
243                         }
244
245                         btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
246                         if (colObj0->getIslandTag() == islandId)
247                         {
248                                 if (colObj0->getActivationState()== ACTIVE_TAG)
249                                 {
250                                         allSleeping = false;
251                                 }
252                                 if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
253                                 {
254                                         allSleeping = false;
255                                 }
256                         }
257                 }
258                         
259
260                 if (allSleeping)
261                 {
262                         int idx;
263                         for (idx=startIslandIndex;idx<endIslandIndex;idx++)
264                         {
265                                 int i = getUnionFind().getElement(idx).m_sz;
266                                 btCollisionObject* colObj0 = collisionObjects[i];
267                                 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
268                                 {
269 //                                      printf("error in island management\n");
270                                 }
271
272                                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
273
274                                 if (colObj0->getIslandTag() == islandId)
275                                 {
276                                         colObj0->setActivationState( ISLAND_SLEEPING );
277                                 }
278                         }
279                 } else
280                 {
281
282                         int idx;
283                         for (idx=startIslandIndex;idx<endIslandIndex;idx++)
284                         {
285                                 int i = getUnionFind().getElement(idx).m_sz;
286
287                                 btCollisionObject* colObj0 = collisionObjects[i];
288                                 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
289                                 {
290 //                                      printf("error in island management\n");
291                                 }
292
293                                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
294
295                                 if (colObj0->getIslandTag() == islandId)
296                                 {
297                                         if ( colObj0->getActivationState() == ISLAND_SLEEPING)
298                                         {
299                                                 colObj0->setActivationState( WANTS_DEACTIVATION);
300                                                 colObj0->setDeactivationTime(0.f);
301                                         }
302                                 }
303                         }
304                 }
305         }
306
307         
308         int i;
309         int maxNumManifolds = dispatcher->getNumManifolds();
310
311 //#define SPLIT_ISLANDS 1
312 //#ifdef SPLIT_ISLANDS
313
314         
315 //#endif //SPLIT_ISLANDS
316
317         
318         for (i=0;i<maxNumManifolds ;i++)
319         {
320                  btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
321                  
322                  const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
323                  const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
324                 
325                  ///@todo: check sleeping conditions!
326                  if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
327                         ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
328                 {
329                 
330                         //kinematic objects don't merge islands, but wake up all connected objects
331                         if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
332                         {
333                                 if (colObj0->hasContactResponse())
334                                         colObj1->activate();
335                         }
336                         if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
337                         {
338                                 if (colObj1->hasContactResponse())
339                                         colObj0->activate();
340                         }
341                         if(m_splitIslands)
342                         { 
343                                 //filtering for response
344                                 if (dispatcher->needsResponse(colObj0,colObj1))
345                                         m_islandmanifold.push_back(manifold);
346                         }
347                 }
348         }
349 }
350
351
352
353 ///@todo: this is random access, it can be walked 'cache friendly'!
354 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
355 {
356         btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
357
358         buildIslands(dispatcher,collisionWorld);
359
360         int endIslandIndex=1;
361         int startIslandIndex;
362         int numElem = getUnionFind().getNumElements();
363
364         BT_PROFILE("processIslands");
365
366         if(!m_splitIslands)
367         {
368                 btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
369                 int maxNumManifolds = dispatcher->getNumManifolds();
370                 callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
371         }
372         else
373         {
374                 // Sort manifolds, based on islands
375                 // Sort the vector using predicate and std::sort
376                 //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
377
378                 int numManifolds = int (m_islandmanifold.size());
379
380                 //tried a radix sort, but quicksort/heapsort seems still faster
381                 //@todo rewrite island management
382                 m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
383                 //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
384
385                 //now process all active islands (sets of manifolds for now)
386
387                 int startManifoldIndex = 0;
388                 int endManifoldIndex = 1;
389
390                 //int islandId;
391
392                 
393
394         //      printf("Start Islands\n");
395
396                 //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
397                 for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
398                 {
399                         int islandId = getUnionFind().getElement(startIslandIndex).m_id;
400
401
402                            bool islandSleeping = true;
403                         
404                                         for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
405                                         {
406                                                         int i = getUnionFind().getElement(endIslandIndex).m_sz;
407                                                         btCollisionObject* colObj0 = collisionObjects[i];
408                                                         m_islandBodies.push_back(colObj0);
409                                                         if (colObj0->isActive())
410                                                                         islandSleeping = false;
411                                         }
412                         
413
414                         //find the accompanying contact manifold for this islandId
415                         int numIslandManifolds = 0;
416                         btPersistentManifold** startManifold = 0;
417
418                         if (startManifoldIndex<numManifolds)
419                         {
420                                 int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
421                                 if (curIslandId == islandId)
422                                 {
423                                         startManifold = &m_islandmanifold[startManifoldIndex];
424                                 
425                                         for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
426                                         {
427
428                                         }
429                                         /// Process the actual simulation, only if not sleeping/deactivated
430                                         numIslandManifolds = endManifoldIndex-startManifoldIndex;
431                                 }
432
433                         }
434
435                         if (!islandSleeping)
436                         {
437                                 callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
438         //                      printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
439                         }
440                         
441                         if (numIslandManifolds)
442                         {
443                                 startManifoldIndex = endManifoldIndex;
444                         }
445
446                         m_islandBodies.resize(0);
447                 }
448         } // else if(!splitIslands) 
449
450 }