fix gcc warnings about no newline at end of file
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btCollisionDispatcher.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
18 #include "btCollisionDispatcher.h"
19
20
21 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
22 #include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h"
23 #include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
24 #include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h"
25 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
26 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
27 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
28 #include <algorithm>
29 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
30
31 int gNumManifold = 0;
32
33 #include <stdio.h>
34
35         
36 btCollisionDispatcher::btCollisionDispatcher(bool noDefaultAlgorithms)
37 :m_useIslands(true),
38 m_convexConvexCreateFunc(0),
39 m_count(0),
40 m_convexConcaveCreateFunc(0),
41 m_swappedConvexConcaveCreateFunc(0),
42 m_compoundCreateFunc(0),
43 m_swappedCompoundCreateFunc(0),
44 m_emptyCreateFunc(0)
45 {
46         int i;
47
48         setNearCallback(defaultNearCallback);
49
50         m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc;
51         for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
52         {
53                 for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
54                 {
55                         m_doubleDispatch[i][j] = m_emptyCreateFunc;
56                 }
57         }
58 }
59
60
61 btCollisionDispatcher::btCollisionDispatcher (): 
62         m_useIslands(true),
63                 m_count(0)
64 {
65         int i;
66
67         setNearCallback(defaultNearCallback);
68         
69         //default CreationFunctions, filling the m_doubleDispatch table
70         m_convexConvexCreateFunc = new btConvexConvexAlgorithm::CreateFunc;
71         m_convexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::CreateFunc;
72         m_swappedConvexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
73         m_compoundCreateFunc = new btCompoundCollisionAlgorithm::CreateFunc;
74         m_swappedCompoundCreateFunc = new btCompoundCollisionAlgorithm::SwappedCreateFunc;
75         m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc;
76
77         for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
78         {
79                 for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
80                 {
81                         m_doubleDispatch[i][j] = internalFindCreateFunc(i,j);
82                         assert(m_doubleDispatch[i][j]);
83                 }
84         }
85         
86         
87 };
88
89 void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
90 {
91         m_doubleDispatch[proxyType0][proxyType1] = createFunc;
92 }
93
94 btCollisionDispatcher::~btCollisionDispatcher()
95 {
96         delete m_convexConvexCreateFunc;
97         delete m_convexConcaveCreateFunc;
98         delete m_swappedConvexConcaveCreateFunc;
99         delete m_compoundCreateFunc;
100         delete m_swappedCompoundCreateFunc;
101         delete m_emptyCreateFunc;
102 }
103
104 btPersistentManifold*   btCollisionDispatcher::getNewManifold(void* b0,void* b1) 
105
106         gNumManifold++;
107         
108         //btAssert(gNumManifold < 65535);
109         
110
111         btCollisionObject* body0 = (btCollisionObject*)b0;
112         btCollisionObject* body1 = (btCollisionObject*)b1;
113         
114         btPersistentManifold* manifold = new btPersistentManifold (body0,body1);
115         m_manifoldsPtr.push_back(manifold);
116
117         return manifold;
118 }
119
120 void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
121 {
122         manifold->clearManifold();
123 }
124
125         
126 void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
127 {
128         
129         gNumManifold--;
130
131         //printf("releaseManifold: gNumManifold %d\n",gNumManifold);
132
133         clearManifold(manifold);
134
135         std::vector<btPersistentManifold*>::iterator i =
136                 std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold);
137         if (!(i == m_manifoldsPtr.end()))
138         {
139                 std::swap(*i, m_manifoldsPtr.back());
140                 m_manifoldsPtr.pop_back();
141                 delete manifold;
142
143         }
144         
145         
146 }
147
148         
149
150 btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
151 {
152
153 #ifdef USE_DISPATCH_REGISTRY_ARRAY
154         
155         btCollisionAlgorithmConstructionInfo ci;
156         ci.m_dispatcher = this;
157         ci.m_manifold = sharedManifold;
158         btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()]
159         ->CreateCollisionAlgorithm(ci,body0,body1);
160 #else
161         btCollisionAlgorithm* algo = internalFindAlgorithm(body0,body1);
162 #endif //USE_DISPATCH_REGISTRY_ARRAY
163         return algo;
164 }
165
166
167 btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(int proxyType0,int proxyType1)
168 {
169         
170         if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
171         {
172                 return m_convexConvexCreateFunc;
173         }
174
175         if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1))
176         {
177                 return m_convexConcaveCreateFunc;
178         }
179
180         if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0))
181         {
182                 return m_swappedConvexConcaveCreateFunc;
183         }
184
185         if (btBroadphaseProxy::isCompound(proxyType0))
186         {
187                 return m_compoundCreateFunc;
188         } else
189         {
190                 if (btBroadphaseProxy::isCompound(proxyType1))
191                 {
192                         return m_swappedCompoundCreateFunc;
193                 }
194         }
195
196         //failed to find an algorithm
197         return m_emptyCreateFunc;
198 }
199
200
201 #ifndef USE_DISPATCH_REGISTRY_ARRAY
202
203 btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
204 {
205         m_count++;
206         
207         btCollisionAlgorithmConstructionInfo ci;
208         ci.m_dispatcher = this;
209         
210         if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConvex() )
211         {
212                 return new btConvexConvexAlgorithm(sharedManifold,ci,body0,body1);
213         }
214
215         if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConcave())
216         {
217                 return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
218         }
219
220         if (body1->getCollisionShape()->isConvex() && body0->getCollisionShape()->isConcave())
221         {
222                 return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
223         }
224
225         if (body0->getCollisionShape()->isCompound())
226         {
227                 return new btCompoundCollisionAlgorithm(ci,body0,body1,false);
228         } else
229         {
230                 if (body1->getCollisionShape()->isCompound())
231                 {
232                         return new btCompoundCollisionAlgorithm(ci,body0,body1,true);
233                 }
234         }
235
236         //failed to find an algorithm
237         return new btEmptyAlgorithm(ci);
238         
239 }
240 #endif //USE_DISPATCH_REGISTRY_ARRAY
241
242 bool    btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1)
243 {
244         //here you can do filtering
245         bool hasResponse = 
246                 (body0->hasContactResponse() && body1->hasContactResponse());
247         //no response between two static/kinematic bodies:
248         hasResponse = hasResponse &&
249                 ((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject()));
250         return hasResponse;
251 }
252
253 bool    btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1)
254 {
255         assert(body0);
256         assert(body1);
257
258         bool needsCollision = true;
259
260         //broadphase filtering already deals with this
261         if ((body0->isStaticObject() || body0->isKinematicObject()) &&
262                 (body1->isStaticObject() || body1->isKinematicObject()))
263         {
264                 printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
265         }
266                 
267         if ((!body0->isActive()) && (!body1->isActive()))
268                 needsCollision = false;
269         
270         return needsCollision ;
271
272 }
273
274
275
276 ///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc)
277 ///this is useful for the collision dispatcher.
278 class btCollisionPairCallback : public btOverlapCallback
279 {
280         btDispatcherInfo& m_dispatchInfo;
281         btCollisionDispatcher*  m_dispatcher;
282
283 public:
284
285         btCollisionPairCallback(btDispatcherInfo& dispatchInfo,btCollisionDispatcher*   dispatcher)
286         :m_dispatchInfo(dispatchInfo),
287         m_dispatcher(dispatcher)
288         {
289         }
290
291         virtual bool    processOverlap(btBroadphasePair& pair)
292         {
293                 (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
294
295                 return false;
296         }
297 };
298
299
300 void    btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)
301 {
302         //m_blockedForChanges = true;
303
304         btCollisionPairCallback collisionCallback(dispatchInfo,this);
305
306         pairCache->processAllOverlappingPairs(&collisionCallback);
307
308         //m_blockedForChanges = false;
309
310 }
311
312
313
314
315 //by default, Bullet will use this near callback
316 void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, btDispatcherInfo& dispatchInfo)
317 {
318                 btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
319                 btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
320
321                 if (dispatcher.needsCollision(colObj0,colObj1))
322                 {
323                         //dispatcher will keep algorithms persistent in the collision pair
324                         if (!collisionPair.m_algorithm)
325                         {
326                                 collisionPair.m_algorithm = dispatcher.findAlgorithm(colObj0,colObj1);
327                         }
328
329                         if (collisionPair.m_algorithm)
330                         {
331                                 btManifoldResult contactPointResult(colObj0,colObj1);
332                                 
333                                 if (dispatchInfo.m_dispatchFunc ==              btDispatcherInfo::DISPATCH_DISCRETE)
334                                 {
335                                         //discrete collision detection query
336                                         collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult);
337                                 } else
338                                 {
339                                         //continuous collision detection query, time of impact (toi)
340                                         float toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
341                                         if (dispatchInfo.m_timeOfImpact > toi)
342                                                 dispatchInfo.m_timeOfImpact = toi;
343
344                                 }
345                         }
346                 }
347
348 }