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