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