updating Bullet 2.x with latest changes. The integration + C-API will follow at some...
[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                 if (allSleeping)
181                 {
182                         int idx;
183                         for (idx=startIslandIndex;idx<endIslandIndex;idx++)
184                         {
185                                 int i = getUnionFind().getElement(idx).m_sz;
186                                 btCollisionObject* colObj0 = collisionObjects[i];
187                                 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
188                                 {
189                                         printf("error in island management\n");
190                                 }
191
192                                 assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
193
194                                 if (colObj0->getIslandTag() == islandId)
195                                 {
196                                         colObj0->setActivationState( ISLAND_SLEEPING );
197                                 }
198                         }
199                 } else
200                 {
201
202                         int idx;
203                         for (idx=startIslandIndex;idx<endIslandIndex;idx++)
204                         {
205                                 int i = getUnionFind().getElement(idx).m_sz;
206
207                                 btCollisionObject* colObj0 = collisionObjects[i];
208                                 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
209                                 {
210                                         printf("error in island management\n");
211                                 }
212
213                                 assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
214
215                                 if (colObj0->getIslandTag() == islandId)
216                                 {
217                                         if ( colObj0->getActivationState() == ISLAND_SLEEPING)
218                                         {
219                                                 colObj0->setActivationState( WANTS_DEACTIVATION);
220                                         }
221                                 }
222                         }
223                 }
224         }
225
226         std::vector<btPersistentManifold*>  islandmanifold;
227         int i;
228         int maxNumManifolds = dispatcher->getNumManifolds();
229         islandmanifold.reserve(maxNumManifolds);
230
231         for (i=0;i<maxNumManifolds ;i++)
232         {
233                  btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
234                  
235                  btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
236                  btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
237                 
238                  //todo: check sleeping conditions!
239                  if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
240                         ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
241                 {
242                         //kinematic objects don't merge islands, but wake up all connected objects
243                         if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
244                         {
245                                 colObj1->setActivationState(ACTIVE_TAG);
246                         }
247                         if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
248                         {
249                                 colObj0->setActivationState(ACTIVE_TAG);
250                         }
251
252                         //filtering for response
253                         if (dispatcher->needsResponse(colObj0,colObj1))
254                                 islandmanifold.push_back(manifold);
255                 }
256         }
257
258         int numManifolds = int (islandmanifold.size());
259
260         // Sort manifolds, based on islands
261         // Sort the vector using predicate and std::sort
262         std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
263
264         //now process all active islands (sets of manifolds for now)
265
266         int startManifoldIndex = 0;
267         int endManifoldIndex = 1;
268
269         for (startManifoldIndex=0;startManifoldIndex<numManifolds;startManifoldIndex = endManifoldIndex)
270         {
271                 int islandId = getIslandId(islandmanifold[startManifoldIndex]);
272                 for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
273                 {
274                 }
275                 /// Process the actual simulation, only if not sleeping/deactivated
276                 int numIslandManifolds = endManifoldIndex-startManifoldIndex;
277                 if (numIslandManifolds)
278                 {
279                         callback->ProcessIsland(&islandmanifold[startManifoldIndex],numIslandManifolds);
280                 }
281         }
282 }