Bugfix: in the new image function, uv test grid option
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btSimulationIslandManager.cpp
1
2
3 #include "LinearMath/btScalar.h"
4 #include "btSimulationIslandManager.h"
5 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
6 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
7 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
8 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
9
10 #include <stdio.h>
11 #include "LinearMath/btQuickprof.h"
12
13 btSimulationIslandManager::btSimulationIslandManager()
14 {
15 }
16
17 btSimulationIslandManager::~btSimulationIslandManager()
18 {
19 }
20
21
22 void btSimulationIslandManager::initUnionFind(int n)
23 {
24                 m_unionFind.reset(n);
25 }
26                 
27
28 void btSimulationIslandManager::findUnions(btDispatcher* dispatcher)
29 {
30         
31         {
32                 for (int i=0;i<dispatcher->getNumManifolds();i++)
33                 {
34                         const btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
35                         //static objects (invmass btScalar(0.)) don't merge !
36
37                          const  btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
38                          const  btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
39
40                         if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
41                                 ((colObj1) && ((colObj1)->mergesSimulationIslands())))
42                         {
43
44                                 m_unionFind.unite((colObj0)->getIslandTag(),
45                                         (colObj1)->getIslandTag());
46                         }
47                 }
48         }
49 }
50
51
52 void    btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
53 {
54         
55         initUnionFind( int (colWorld->getCollisionObjectArray().size()));
56         
57         // put the index into m_controllers into m_tag  
58         {
59                 
60                 int index = 0;
61                 int i;
62                 for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
63                 {
64                         btCollisionObject*      collisionObject= colWorld->getCollisionObjectArray()[i];
65                         collisionObject->setIslandTag(index);
66                         collisionObject->setCompanionId(-1);
67                         collisionObject->setHitFraction(btScalar(1.));
68                         index++;
69                         
70                 }
71         }
72         // do the union find
73         
74         findUnions(dispatcher);
75         
76
77         
78 }
79
80
81
82
83 void    btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
84 {
85         // put the islandId ('find' value) into m_tag   
86         {
87                 
88                 
89                 int index = 0;
90                 int i;
91                 for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
92                 {
93                         btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
94                         if (collisionObject->mergesSimulationIslands())
95                         {
96                                 collisionObject->setIslandTag( m_unionFind.find(index) );
97                                 collisionObject->setCompanionId(-1);
98                         } else
99                         {
100                                 collisionObject->setIslandTag(-1);
101                                 collisionObject->setCompanionId(-2);
102                         }
103                         index++;
104                 }
105         }
106 }
107
108 inline  int     getIslandId(const btPersistentManifold* lhs)
109 {
110         int islandId;
111         const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
112         const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
113         islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
114         return islandId;
115
116 }
117
118
119
120 /// function object that routes calls to operator<
121 class btPersistentManifoldSortPredicate
122 {
123         public:
124
125                 SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs )
126                 {
127                         return getIslandId(lhs) < getIslandId(rhs);
128                 }
129 };
130
131
132
133
134
135 //
136 // todo: this is random access, it can be walked 'cache friendly'!
137 //
138 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback)
139 {
140
141         
142         
143         /*if (0)
144         {
145                 int maxNumManifolds = dispatcher->getNumManifolds();
146                 btCollisionDispatcher* colDis = (btCollisionDispatcher*)dispatcher;
147                 btPersistentManifold** manifold = colDis->getInternalManifoldPointer();
148                 callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, 0);
149                 return;
150         }
151         */
152
153
154         BEGIN_PROFILE("islandUnionFindAndHeapSort");
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                         assert((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                                 assert((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                                 assert((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                                         }
245                                 }
246                         }
247                 }
248         }
249
250         btAlignedObjectArray<btPersistentManifold*>  islandmanifold;
251         int i;
252         int maxNumManifolds = dispatcher->getNumManifolds();
253         islandmanifold.reserve(maxNumManifolds);
254
255         for (i=0;i<maxNumManifolds ;i++)
256         {
257                  btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
258                  
259                  btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
260                  btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
261                 
262                  //todo: check sleeping conditions!
263                  if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
264                         ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
265                 {
266                 
267                         //kinematic objects don't merge islands, but wake up all connected objects
268                         if (colObj0->isStaticOrKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
269                         {
270                                 colObj1->activate();
271                         }
272                         if (colObj1->isStaticOrKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
273                         {
274                                 colObj0->activate();
275                         }
276
277                         //filtering for response
278                         if (dispatcher->needsResponse(colObj0,colObj1))
279                                 islandmanifold.push_back(manifold);
280                 }
281         }
282
283         int numManifolds = int (islandmanifold.size());
284
285         // Sort manifolds, based on islands
286         // Sort the vector using predicate and std::sort
287         //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
288
289         //we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
290         islandmanifold.heapSort(btPersistentManifoldSortPredicate());
291
292         //now process all active islands (sets of manifolds for now)
293
294         int startManifoldIndex = 0;
295         int endManifoldIndex = 1;
296
297         //int islandId;
298
299         END_PROFILE("islandUnionFindAndHeapSort");
300
301         btAlignedObjectArray<btCollisionObject*>        islandBodies;
302
303
304         //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
305         for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
306         {
307                 int islandId = getUnionFind().getElement(startIslandIndex).m_id;
308
309
310                bool islandSleeping = false;
311                 
312                 for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
313                 {
314                         int i = getUnionFind().getElement(endIslandIndex).m_sz;
315                         btCollisionObject* colObj0 = collisionObjects[i];
316                                                 islandBodies.push_back(colObj0);
317                         if (!colObj0->isActive())
318                                 islandSleeping = true;
319                 }
320                 
321
322                 //find the accompanying contact manifold for this islandId
323                 int numIslandManifolds = 0;
324                 btPersistentManifold** startManifold = 0;
325
326                 if (startManifoldIndex<numManifolds)
327                 {
328                         int curIslandId = getIslandId(islandmanifold[startManifoldIndex]);
329                         if (curIslandId == islandId)
330                         {
331                                 startManifold = &islandmanifold[startManifoldIndex];
332                         
333                                 for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
334                                 {
335
336                                 }
337                                 /// Process the actual simulation, only if not sleeping/deactivated
338                                 numIslandManifolds = endManifoldIndex-startManifoldIndex;
339                         }
340
341                 }
342
343                 if (!islandSleeping)
344                 {
345                         callback->ProcessIsland(&islandBodies[0],islandBodies.size(),startManifold,numIslandManifolds, islandId);
346                 }
347                 
348                 if (numIslandManifolds)
349                 {
350                         startManifoldIndex = endManifoldIndex;
351                 }
352
353                 islandBodies.resize(0);
354         }
355
356         
357 }