bullet: Update to current svn, r2636
[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
23 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
24 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
25 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
26 #include "LinearMath/btPoolAllocator.h"
27 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
28 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
29
30 int gNumManifold = 0;
31
32 #ifdef BT_DEBUG
33 #include <stdio.h>
34 #endif
35
36
37 btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): 
38 m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD),
39         m_collisionConfiguration(collisionConfiguration)
40 {
41         int i;
42
43         setNearCallback(defaultNearCallback);
44         
45         m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool();
46
47         m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool();
48
49         for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
50         {
51                 for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
52                 {
53                         m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
54                         btAssert(m_doubleDispatch[i][j]);
55                 }
56         }
57         
58         
59 }
60
61
62 void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
63 {
64         m_doubleDispatch[proxyType0][proxyType1] = createFunc;
65 }
66
67 btCollisionDispatcher::~btCollisionDispatcher()
68 {
69 }
70
71 btPersistentManifold*   btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1) 
72
73         gNumManifold++;
74         
75         //btAssert(gNumManifold < 65535);
76         
77
78
79         //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
80         
81         btScalar contactBreakingThreshold =  (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? 
82                 btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold) , body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
83                 : gContactBreakingThreshold ;
84
85         btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
86                 
87         void* mem = 0;
88         
89         if (m_persistentManifoldPoolAllocator->getFreeCount())
90         {
91                 mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
92         } else
93         {
94                 //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
95                 if ((m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION)==0)
96                 {
97                         mem = btAlignedAlloc(sizeof(btPersistentManifold),16);
98                 } else
99                 {
100                         btAssert(0);
101                         //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
102                         return 0;
103                 }
104         }
105         btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold);
106         manifold->m_index1a = m_manifoldsPtr.size();
107         m_manifoldsPtr.push_back(manifold);
108
109         return manifold;
110 }
111
112 void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
113 {
114         manifold->clearManifold();
115 }
116
117         
118 void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
119 {
120         
121         gNumManifold--;
122
123         //printf("releaseManifold: gNumManifold %d\n",gNumManifold);
124         clearManifold(manifold);
125
126         int findIndex = manifold->m_index1a;
127         btAssert(findIndex < m_manifoldsPtr.size());
128         m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
129         m_manifoldsPtr[findIndex]->m_index1a = findIndex;
130         m_manifoldsPtr.pop_back();
131
132         manifold->~btPersistentManifold();
133         if (m_persistentManifoldPoolAllocator->validPtr(manifold))
134         {
135                 m_persistentManifoldPoolAllocator->freeMemory(manifold);
136         } else
137         {
138                 btAlignedFree(manifold);
139         }
140         
141 }
142
143         
144
145 btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold)
146 {
147         
148         btCollisionAlgorithmConstructionInfo ci;
149
150         ci.m_dispatcher1 = this;
151         ci.m_manifold = sharedManifold;
152         btCollisionAlgorithm* algo = m_doubleDispatch[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0Wrap,body1Wrap);
153
154         return algo;
155 }
156
157
158
159
160 bool    btCollisionDispatcher::needsResponse(const btCollisionObject* body0,const btCollisionObject* body1)
161 {
162         //here you can do filtering
163         bool hasResponse = 
164                 (body0->hasContactResponse() && body1->hasContactResponse());
165         //no response between two static/kinematic bodies:
166         hasResponse = hasResponse &&
167                 ((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject()));
168         return hasResponse;
169 }
170
171 bool    btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const btCollisionObject* body1)
172 {
173         btAssert(body0);
174         btAssert(body1);
175
176         bool needsCollision = true;
177
178 #ifdef BT_DEBUG
179         if (!(m_dispatcherFlags & btCollisionDispatcher::CD_STATIC_STATIC_REPORTED))
180         {
181                 //broadphase filtering already deals with this
182                 if (body0->isStaticOrKinematicObject() && body1->isStaticOrKinematicObject())
183                 {
184                         m_dispatcherFlags |= btCollisionDispatcher::CD_STATIC_STATIC_REPORTED;
185                         printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
186                 }
187         }
188 #endif //BT_DEBUG
189
190         if ((!body0->isActive()) && (!body1->isActive()))
191                 needsCollision = false;
192         else if (!body0->checkCollideWith(body1))
193                 needsCollision = false;
194         
195         return needsCollision ;
196
197 }
198
199
200
201 ///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc)
202 ///this is useful for the collision dispatcher.
203 class btCollisionPairCallback : public btOverlapCallback
204 {
205         const btDispatcherInfo& m_dispatchInfo;
206         btCollisionDispatcher*  m_dispatcher;
207
208 public:
209
210         btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher*     dispatcher)
211         :m_dispatchInfo(dispatchInfo),
212         m_dispatcher(dispatcher)
213         {
214         }
215
216         /*btCollisionPairCallback& operator=(btCollisionPairCallback& other)
217         {
218                 m_dispatchInfo = other.m_dispatchInfo;
219                 m_dispatcher = other.m_dispatcher;
220                 return *this;
221         }
222         */
223
224
225         virtual ~btCollisionPairCallback() {}
226
227
228         virtual bool    processOverlap(btBroadphasePair& pair)
229         {
230                 (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
231
232                 return false;
233         }
234 };
235
236
237
238 void    btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) 
239 {
240         //m_blockedForChanges = true;
241
242         btCollisionPairCallback collisionCallback(dispatchInfo,this);
243
244         pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);
245
246         //m_blockedForChanges = false;
247
248 }
249
250
251
252
253 //by default, Bullet will use this near callback
254 void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
255 {
256                 btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
257                 btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
258
259                 if (dispatcher.needsCollision(colObj0,colObj1))
260                 {
261                         btCollisionObjectWrapper obj0Wrap(0,colObj0->getCollisionShape(),colObj0,colObj0->getWorldTransform(),-1,-1);
262                         btCollisionObjectWrapper obj1Wrap(0,colObj1->getCollisionShape(),colObj1,colObj1->getWorldTransform(),-1,-1);
263
264
265                         //dispatcher will keep algorithms persistent in the collision pair
266                         if (!collisionPair.m_algorithm)
267                         {
268                                 collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap);
269                         }
270
271                         if (collisionPair.m_algorithm)
272                         {
273                                 btManifoldResult contactPointResult(&obj0Wrap,&obj1Wrap);
274                                 
275                                 if (dispatchInfo.m_dispatchFunc ==              btDispatcherInfo::DISPATCH_DISCRETE)
276                                 {
277                                         //discrete collision detection query
278                                         
279                                         collisionPair.m_algorithm->processCollision(&obj0Wrap,&obj1Wrap,dispatchInfo,&contactPointResult);
280                                 } else
281                                 {
282                                         //continuous collision detection query, time of impact (toi)
283                                         btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
284                                         if (dispatchInfo.m_timeOfImpact > toi)
285                                                 dispatchInfo.m_timeOfImpact = toi;
286
287                                 }
288                         }
289                 }
290
291 }
292
293
294 void* btCollisionDispatcher::allocateCollisionAlgorithm(int size)
295 {
296         if (m_collisionAlgorithmPoolAllocator->getFreeCount())
297         {
298                 return m_collisionAlgorithmPoolAllocator->allocate(size);
299         }
300         
301         //warn user for overflow?
302         return  btAlignedAlloc(static_cast<size_t>(size), 16);
303 }
304
305 void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)
306 {
307         if (m_collisionAlgorithmPoolAllocator->validPtr(ptr))
308         {
309                 m_collisionAlgorithmPoolAllocator->freeMemory(ptr);
310         } else
311         {
312                 btAlignedFree(ptr);
313         }
314 }