ba2859e77d8fcf94565d6e0dce13be535533dbae
[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         //here you can do filtering
247         bool hasResponse = 
248                 (!(colObj0.m_collisionFlags & CollisionObject::noContactResponse)) &&
249                 (!(colObj1.m_collisionFlags & CollisionObject::noContactResponse));
250         return hasResponse;
251 }
252
253 bool    CollisionDispatcher::NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
254 {
255
256         CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject;
257         CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject;
258
259         assert(body0);
260         assert(body1);
261
262         bool needsCollision = true;
263
264         if ((body0->m_collisionFlags & CollisionObject::isStatic) && 
265                 (body1->m_collisionFlags & CollisionObject::isStatic))
266                 needsCollision = false;
267         
268         if ((body0->GetActivationState() == 2) &&(body1->GetActivationState() == 2))
269                 needsCollision = false;
270         
271         return needsCollision ;
272
273 }
274
275 ///allows the user to get contact point callbacks 
276 ManifoldResult* CollisionDispatcher::GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold)
277 {
278
279
280         //in-place, this prevents parallel dispatching, but just adding a list would fix that.
281         ManifoldResult* manifoldResult = new (&m_defaultManifoldResult) ManifoldResult(obj0,obj1,manifold);
282         return manifoldResult;
283 }
284         
285 ///allows the user to get contact point callbacks 
286 void    CollisionDispatcher::ReleaseManifoldResult(ManifoldResult*)
287 {
288
289 }