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