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
btCollisionDispatcher(btCollisionConfiguration * collisionConfiguration)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
registerCollisionCreateFunc(int proxyType0,int proxyType1,btCollisionAlgorithmCreateFunc * createFunc)62 void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
63 {
64 m_doubleDispatch[proxyType0][proxyType1] = createFunc;
65 }
66
~btCollisionDispatcher()67 btCollisionDispatcher::~btCollisionDispatcher()
68 {
69 }
70
getNewManifold(const btCollisionObject * body0,const btCollisionObject * body1)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
clearManifold(btPersistentManifold * manifold)112 void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
113 {
114 manifold->clearManifold();
115 }
116
117
releaseManifold(btPersistentManifold * manifold)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
findAlgorithm(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,btPersistentManifold * sharedManifold)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
needsResponse(const btCollisionObject * body0,const btCollisionObject * body1)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
needsCollision(const btCollisionObject * body0,const btCollisionObject * body1)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)) || (!body1->checkCollideWith(body0)))
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
btCollisionPairCallback(const btDispatcherInfo & dispatchInfo,btCollisionDispatcher * dispatcher)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
~btCollisionPairCallback()225 virtual ~btCollisionPairCallback() {}
226
227
processOverlap(btBroadphasePair & pair)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
dispatchAllCollisionPairs(btOverlappingPairCache * pairCache,const btDispatcherInfo & dispatchInfo,btDispatcher * dispatcher)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
defaultNearCallback(btBroadphasePair & collisionPair,btCollisionDispatcher & dispatcher,const btDispatcherInfo & dispatchInfo)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
allocateCollisionAlgorithm(int size)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
freeCollisionAlgorithm(void * ptr)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 }
315