3f51e9ec0a61472f225c5bd777e0d03ebba23dda
[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 <algorithm>
12
13
14 btSimulationIslandManager::btSimulationIslandManager()
15 {
16 }
17
18 btSimulationIslandManager::~btSimulationIslandManager()
19 {
20 }
21
22
23 void btSimulationIslandManager::initUnionFind(int n)
24 {
25                 m_unionFind.reset(n);
26 }
27                 
28
29 void btSimulationIslandManager::findUnions(btDispatcher* dispatcher)
30 {
31         
32         {
33                 for (int i=0;i<dispatcher->getNumManifolds();i++)
34                 {
35                         const btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
36                         //static objects (invmass 0.f) don't merge !
37
38                          const  btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
39                          const  btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
40
41                         if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
42                                 ((colObj1) && ((colObj1)->mergesSimulationIslands())))
43                         {
44
45                                 m_unionFind.unite((colObj0)->getIslandTag(),
46                                         (colObj1)->getIslandTag());
47                         }
48                 }
49         }
50 }
51
52
53 void    btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
54 {
55         
56         initUnionFind( int (colWorld->getCollisionObjectArray().size()));
57         
58         // put the index into m_controllers into m_tag  
59         {
60                 std::vector<btCollisionObject*>::iterator i;
61                 
62                 int index = 0;
63                 for (i=colWorld->getCollisionObjectArray().begin();
64                 !(i==colWorld->getCollisionObjectArray().end()); i++)
65                 {
66                         
67                         btCollisionObject*      collisionObject= (*i);
68                         collisionObject->setIslandTag(index);
69                         collisionObject->setHitFraction(1.f);
70                         index++;
71                         
72                 }
73         }
74         // do the union find
75         
76         findUnions(dispatcher);
77         
78
79         
80 }
81
82
83
84
85 void    btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
86 {
87         // put the islandId ('find' value) into m_tag   
88         {
89                 
90                 
91                 std::vector<btCollisionObject*>::iterator i;
92                 
93                 int index = 0;
94                 for (i=colWorld->getCollisionObjectArray().begin();
95                 !(i==colWorld->getCollisionObjectArray().end()); i++)
96                 {
97                         btCollisionObject* collisionObject= (*i);
98                         
99                         if (collisionObject->mergesSimulationIslands())
100                         {
101                                 collisionObject->setIslandTag( m_unionFind.find(index) );
102                         } else
103                         {
104                                 collisionObject->setIslandTag(-1);
105                         }
106                         index++;
107                 }
108         }
109 }
110
111 inline  int     getIslandId(const btPersistentManifold* lhs)
112 {
113         int islandId;
114         const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
115         const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
116         islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
117         return islandId;
118
119 }
120
121 bool btPersistentManifoldSortPredicate(const btPersistentManifold* lhs, const btPersistentManifold* rhs)
122 {
123         int rIslandId0,lIslandId0;
124         rIslandId0 = getIslandId(rhs);
125         lIslandId0 = getIslandId(lhs);
126         return lIslandId0 < rIslandId0;
127 }
128
129
130 //
131 // todo: this is random access, it can be walked 'cache friendly'!
132 //
133 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback)
134 {
135         //we are going to sort the unionfind array, and store the element id in the size
136         //afterwards, we clean unionfind, to make sure no-one uses it anymore
137         
138         getUnionFind().sortIslands();
139         int numElem = getUnionFind().getNumElements();
140
141         int endIslandIndex=1;
142
143         //update the sleeping state for bodies, if all are sleeping
144         for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
145         {
146                 int islandId = getUnionFind().getElement(startIslandIndex).m_id;
147                 for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
148                 {
149                 }
150
151                 //int numSleeping = 0;
152
153                 bool allSleeping = true;
154
155                 int idx;
156                 for (idx=startIslandIndex;idx<endIslandIndex;idx++)
157                 {
158                         int i = getUnionFind().getElement(idx).m_sz;
159
160                         btCollisionObject* colObj0 = collisionObjects[i];
161                         if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
162                         {
163                                 printf("error in island management\n");
164                         }
165
166                         assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
167                         if (colObj0->getIslandTag() == islandId)
168                         {
169                                 if (colObj0->getActivationState()== ACTIVE_TAG)
170                                 {
171                                         allSleeping = false;
172                                 }
173                                 if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
174                                 {
175                                         allSleeping = false;
176                                 }
177                         }
178                 }
179                         
180
181                 if (allSleeping)
182                 {
183                         int idx;
184                         for (idx=startIslandIndex;idx<endIslandIndex;idx++)
185                         {
186                                 int i = getUnionFind().getElement(idx).m_sz;
187                                 btCollisionObject* colObj0 = collisionObjects[i];
188                                 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
189                                 {
190                                         printf("error in island management\n");
191                                 }
192
193                                 assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
194
195                                 if (colObj0->getIslandTag() == islandId)
196                                 {
197                                         colObj0->setActivationState( ISLAND_SLEEPING );
198                                 }
199                         }
200                 } else
201                 {
202
203                         int idx;
204                         for (idx=startIslandIndex;idx<endIslandIndex;idx++)
205                         {
206                                 int i = getUnionFind().getElement(idx).m_sz;
207
208                                 btCollisionObject* colObj0 = collisionObjects[i];
209                                 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
210                                 {
211                                         printf("error in island management\n");
212                                 }
213
214                                 assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
215
216                                 if (colObj0->getIslandTag() == islandId)
217                                 {
218                                         if ( colObj0->getActivationState() == ISLAND_SLEEPING)
219                                         {
220                                                 colObj0->setActivationState( WANTS_DEACTIVATION);
221                                         }
222                                 }
223                         }
224                 }
225         }
226
227         std::vector<btPersistentManifold*>  islandmanifold;
228         int i;
229         int maxNumManifolds = dispatcher->getNumManifolds();
230         islandmanifold.reserve(maxNumManifolds);
231
232         for (i=0;i<maxNumManifolds ;i++)
233         {
234                  btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
235                  
236                  btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
237                  btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
238                 
239                  //todo: check sleeping conditions!
240                  if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
241                         ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
242                 {
243                 
244                         //kinematic objects don't merge islands, but wake up all connected objects
245                         if (colObj0->isStaticOrKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
246                         {
247                                 colObj1->activate();
248                         }
249                         if (colObj1->isStaticOrKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
250                         {
251                                 colObj0->activate();
252                         }
253
254                         //filtering for response
255                         if (dispatcher->needsResponse(colObj0,colObj1))
256                                 islandmanifold.push_back(manifold);
257                 }
258         }
259
260         int numManifolds = int (islandmanifold.size());
261
262         // Sort manifolds, based on islands
263         // Sort the vector using predicate and std::sort
264         std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
265
266         //now process all active islands (sets of manifolds for now)
267
268         int startManifoldIndex = 0;
269         int endManifoldIndex = 1;
270
271         int islandId;
272
273
274                 //update the sleeping state for bodies, if all are sleeping
275         for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
276         {
277                 int islandId = getUnionFind().getElement(startIslandIndex).m_id;
278
279                 for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
280                 {
281                 }
282
283                 //find the accompanying contact manifold for this islandId
284                 int numIslandManifolds = 0;
285                 btPersistentManifold** startManifold = 0;
286
287                 if (startManifoldIndex<numManifolds)
288                 {
289                         int curIslandId = getIslandId(islandmanifold[startManifoldIndex]);
290                         if (curIslandId == islandId)
291                         {
292                                 startManifold = &islandmanifold[startManifoldIndex];
293                         
294                                 for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
295                                 {
296
297                                 }
298                                 /// Process the actual simulation, only if not sleeping/deactivated
299                                 numIslandManifolds = endManifoldIndex-startManifoldIndex;
300                         }
301
302                 }
303
304                 callback->ProcessIsland(startManifold,numIslandManifolds, islandId);
305
306                 if (numIslandManifolds)
307                 {
308                         startManifoldIndex = endManifoldIndex;
309                 }
310         }
311 }