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