workaround for a irregularity/bug in physics system (happens very seldom, just 1...
[blender.git] / extern / bullet / Bullet / CollisionDispatch / CollisionDispatcher.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 "CollisionDispatcher.h"
18
19
20 #include "BroadphaseCollision/CollisionAlgorithm.h"
21 #include "CollisionDispatch/ConvexConvexAlgorithm.h"
22 #include "CollisionDispatch/EmptyCollisionAlgorithm.h"
23 #include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h"
24 #include "CollisionShapes/CollisionShape.h"
25 #include "CollisionDispatch/CollisionObject.h"
26 #include <algorithm>
27
28 int gNumManifold = 0;
29
30 void CollisionDispatcher::FindUnions()
31 {
32         if (m_useIslands)
33         {
34                 for (int i=0;i<GetNumManifolds();i++)
35                 {
36                         const PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
37                         //static objects (invmass 0.f) don't merge !
38
39                          const  CollisionObject* colObj0 = static_cast<const CollisionObject*>(manifold->GetBody0());
40                          const  CollisionObject* colObj1 = static_cast<const CollisionObject*>(manifold->GetBody1());
41
42                          if (colObj0 && colObj1 && NeedsResponse(*colObj0,*colObj1))
43                          {
44                                 if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
45                                         ((colObj1) && ((colObj1)->mergesSimulationIslands())))
46                                 {
47
48                                         m_unionFind.unite((colObj0)->m_islandTag1,
49                                                 (colObj1)->m_islandTag1);
50                                 }
51                          }
52                         
53                         
54                 }
55         }
56         
57 }
58         
59
60         
61 CollisionDispatcher::CollisionDispatcher (): 
62         m_useIslands(true),
63                 m_defaultManifoldResult(0,0,0),
64                 m_count(0)
65 {
66         int i;
67         
68         for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
69         {
70                 for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
71                 {
72                         m_doubleDispatch[i][j] = 0;
73                 }
74         }
75         
76         
77 };
78         
79 PersistentManifold*     CollisionDispatcher::GetNewManifold(void* b0,void* b1) 
80
81         gNumManifold++;
82         //printf("GetNewManifoldResult: gNumManifold %d\n",gNumManifold);
83
84         CollisionObject* body0 = (CollisionObject*)b0;
85         CollisionObject* body1 = (CollisionObject*)b1;
86         
87         PersistentManifold* manifold = new PersistentManifold (body0,body1);
88         m_manifoldsPtr.push_back(manifold);
89
90         return manifold;
91 }
92
93 void CollisionDispatcher::ClearManifold(PersistentManifold* manifold)
94 {
95         manifold->ClearManifold();
96 }
97
98         
99 void CollisionDispatcher::ReleaseManifold(PersistentManifold* manifold)
100 {
101         
102         gNumManifold--;
103
104         //printf("ReleaseManifold: gNumManifold %d\n",gNumManifold);
105
106         ClearManifold(manifold);
107
108         std::vector<PersistentManifold*>::iterator i =
109                 std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold);
110         if (!(i == m_manifoldsPtr.end()))
111         {
112                 std::swap(*i, m_manifoldsPtr.back());
113                 m_manifoldsPtr.pop_back();
114                 delete manifold;
115
116         }
117         
118         
119 }
120
121         
122 //
123 // todo: this is random access, it can be walked 'cache friendly'!
124 //
125 void CollisionDispatcher::BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback)
126 {
127         int numBodies  = collisionObjects.size();
128
129         for (int islandId=0;islandId<numBodies;islandId++)
130         {
131
132                 std::vector<PersistentManifold*>  islandmanifold;
133                 
134                 //int numSleeping = 0;
135
136                 bool allSleeping = true;
137
138                 int i;
139                 for (i=0;i<numBodies;i++)
140                 {
141                         CollisionObject* colObj0 = collisionObjects[i];
142                         if (colObj0->m_islandTag1 == islandId)
143                         {
144                                 if (colObj0->GetActivationState()== ACTIVE_TAG)
145                                 {
146                                         allSleeping = false;
147                                 }
148                                 if (colObj0->GetActivationState()== DISABLE_DEACTIVATION)
149                                 {
150                                         allSleeping = false;
151                                 }
152                         }
153                 }
154
155                 
156                 for (i=0;i<GetNumManifolds();i++)
157                 {
158                          PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
159                          
160                          //filtering for response
161
162                          CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
163                          CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
164                          {
165                                 if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
166                                         ((colObj1) && (colObj1)->m_islandTag1 == (islandId)))
167                                 {
168
169                                         if (NeedsResponse(*colObj0,*colObj1))
170                                                 islandmanifold.push_back(manifold);
171                                 }
172                          }
173                 }
174                 if (allSleeping)
175                 {
176                         int i;
177                         for (i=0;i<numBodies;i++)
178                         {
179                                 CollisionObject* colObj0 = collisionObjects[i];
180                                 if (colObj0->m_islandTag1 == islandId)
181                                 {
182                                         colObj0->SetActivationState( ISLAND_SLEEPING );
183                                 }
184                         }
185
186                         
187                 } else
188                 {
189
190                         int i;
191                         for (i=0;i<numBodies;i++)
192                         {
193                                 CollisionObject* colObj0 = collisionObjects[i];
194                                 if (colObj0->m_islandTag1 == islandId)
195                                 {
196                                         if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
197                                         {
198                                                 colObj0->SetActivationState( WANTS_DEACTIVATION);
199                                         }
200                                 }
201                         }
202
203                         /// Process the actual simulation, only if not sleeping/deactivated
204                         if (islandmanifold.size())
205                         {
206                                 callback->ProcessIsland(&islandmanifold[0],islandmanifold.size());
207                         }
208
209                 }
210         }
211 }
212
213
214
215 CollisionAlgorithm* CollisionDispatcher::InternalFindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
216 {
217         m_count++;
218         CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject;
219         CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject;
220
221         CollisionAlgorithmConstructionInfo ci;
222         ci.m_dispatcher = this;
223         
224         if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConvex() )
225         {
226                 return new ConvexConvexAlgorithm(0,ci,&proxy0,&proxy1);                 
227         }
228
229         if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConcave())
230         {
231                 return new ConvexConcaveCollisionAlgorithm(ci,&proxy0,&proxy1);
232         }
233
234         if (body1->m_collisionShape->IsConvex() && body0->m_collisionShape->IsConcave())
235         {
236                 return new ConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0);
237         }
238
239         //failed to find an algorithm
240         return new EmptyAlgorithm(ci);
241         
242 }
243
244 bool    CollisionDispatcher::NeedsResponse(const  CollisionObject& colObj0,const CollisionObject& colObj1)
245 {
246
247         
248         //here you can do filtering
249         bool hasResponse = 
250                 (!(colObj0.m_collisionFlags & CollisionObject::noContactResponse)) &&
251                 (!(colObj1.m_collisionFlags & CollisionObject::noContactResponse));
252         hasResponse = hasResponse &&
253                 (colObj0.IsActive() || colObj1.IsActive());
254         return hasResponse;
255 }
256
257 bool    CollisionDispatcher::NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
258 {
259
260         CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject;
261         CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject;
262
263         assert(body0);
264         assert(body1);
265
266         bool needsCollision = true;
267
268         if ((body0->m_collisionFlags & CollisionObject::isStatic) && 
269                 (body1->m_collisionFlags & CollisionObject::isStatic))
270                 needsCollision = false;
271                 
272         if ((!body0->IsActive()) && (!body1->IsActive()))
273                 needsCollision = false;
274         
275         return needsCollision ;
276
277 }
278
279 ///allows the user to get contact point callbacks 
280 ManifoldResult* CollisionDispatcher::GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold)
281 {
282
283
284         //in-place, this prevents parallel dispatching, but just adding a list would fix that.
285         ManifoldResult* manifoldResult = new (&m_defaultManifoldResult) ManifoldResult(obj0,obj1,manifold);
286         return manifoldResult;
287 }
288         
289 ///allows the user to get contact point callbacks 
290 void    CollisionDispatcher::ReleaseManifoldResult(ManifoldResult*)
291 {
292
293 }