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 "btCollisionWorld.h"
17 #include "btCollisionDispatcher.h"
18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
19 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
20 #include "BulletCollision/CollisionShapes/btConvexShape.h"
21 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
22 #include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting
23 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting
24 #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" //for raycasting
25 #include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" //for raycasting
26 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
27 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
28 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
29 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
30 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
31 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
32 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
33 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
34 #include "LinearMath/btAabbUtil2.h"
35 #include "LinearMath/btQuickprof.h"
36 #include "LinearMath/btSerializer.h"
37 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
38 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
39
40 //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
41
42 //#define USE_BRUTEFORCE_RAYBROADPHASE 1
43 //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation' or 'updateAabbs' before using a rayTest
44 //#define RECALCULATE_AABB_RAYCAST 1
45
46 //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
47 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
48 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
49 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
50
51 ///for debug drawing
52
53 //for debug rendering
54 #include "BulletCollision/CollisionShapes/btBoxShape.h"
55 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
56 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
57 #include "BulletCollision/CollisionShapes/btConeShape.h"
58 #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
59 #include "BulletCollision/CollisionShapes/btCylinderShape.h"
60 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
61 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
62 #include "BulletCollision/CollisionShapes/btSphereShape.h"
63 #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
64 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
65 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
66
btCollisionWorld(btDispatcher * dispatcher,btBroadphaseInterface * pairCache,btCollisionConfiguration * collisionConfiguration)67 btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
68 : m_dispatcher1(dispatcher),
69 m_broadphasePairCache(pairCache),
70 m_debugDrawer(0),
71 m_forceUpdateAllAabbs(true)
72 {
73 }
74
~btCollisionWorld()75 btCollisionWorld::~btCollisionWorld()
76 {
77 //clean up remaining objects
78 int i;
79 for (i = 0; i < m_collisionObjects.size(); i++)
80 {
81 btCollisionObject* collisionObject = m_collisionObjects[i];
82
83 btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
84 if (bp)
85 {
86 //
87 // only clear the cached algorithms
88 //
89 getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp, m_dispatcher1);
90 getBroadphase()->destroyProxy(bp, m_dispatcher1);
91 collisionObject->setBroadphaseHandle(0);
92 }
93 }
94 }
95
refreshBroadphaseProxy(btCollisionObject * collisionObject)96 void btCollisionWorld::refreshBroadphaseProxy(btCollisionObject* collisionObject)
97 {
98 if (collisionObject->getBroadphaseHandle())
99 {
100 int collisionFilterGroup = collisionObject->getBroadphaseHandle()->m_collisionFilterGroup;
101 int collisionFilterMask = collisionObject->getBroadphaseHandle()->m_collisionFilterMask;
102
103 getBroadphase()->destroyProxy(collisionObject->getBroadphaseHandle(), getDispatcher());
104
105 //calculate new AABB
106 btTransform trans = collisionObject->getWorldTransform();
107
108 btVector3 minAabb;
109 btVector3 maxAabb;
110 collisionObject->getCollisionShape()->getAabb(trans, minAabb, maxAabb);
111
112 int type = collisionObject->getCollisionShape()->getShapeType();
113 collisionObject->setBroadphaseHandle(getBroadphase()->createProxy(
114 minAabb,
115 maxAabb,
116 type,
117 collisionObject,
118 collisionFilterGroup,
119 collisionFilterMask,
120 m_dispatcher1));
121 }
122 }
123
addCollisionObject(btCollisionObject * collisionObject,int collisionFilterGroup,int collisionFilterMask)124 void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask)
125 {
126 btAssert(collisionObject);
127
128 //check that the object isn't already added
129 btAssert(m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
130 btAssert(collisionObject->getWorldArrayIndex() == -1); // do not add the same object to more than one collision world
131
132 collisionObject->setWorldArrayIndex(m_collisionObjects.size());
133 m_collisionObjects.push_back(collisionObject);
134
135 //calculate new AABB
136 btTransform trans = collisionObject->getWorldTransform();
137
138 btVector3 minAabb;
139 btVector3 maxAabb;
140 collisionObject->getCollisionShape()->getAabb(trans, minAabb, maxAabb);
141
142 int type = collisionObject->getCollisionShape()->getShapeType();
143 collisionObject->setBroadphaseHandle(getBroadphase()->createProxy(
144 minAabb,
145 maxAabb,
146 type,
147 collisionObject,
148 collisionFilterGroup,
149 collisionFilterMask,
150 m_dispatcher1));
151 }
152
updateSingleAabb(btCollisionObject * colObj)153 void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
154 {
155 btVector3 minAabb, maxAabb;
156 colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb);
157 //need to increase the aabb for contact thresholds
158 btVector3 contactThreshold(gContactBreakingThreshold, gContactBreakingThreshold, gContactBreakingThreshold);
159 minAabb -= contactThreshold;
160 maxAabb += contactThreshold;
161
162 if (getDispatchInfo().m_useContinuous && colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
163 {
164 btVector3 minAabb2, maxAabb2;
165 colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(), minAabb2, maxAabb2);
166 minAabb2 -= contactThreshold;
167 maxAabb2 += contactThreshold;
168 minAabb.setMin(minAabb2);
169 maxAabb.setMax(maxAabb2);
170 }
171
172 btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
173
174 //moving objects should be moderately sized, probably something wrong if not
175 if (colObj->isStaticObject() || ((maxAabb - minAabb).length2() < btScalar(1e12)))
176 {
177 bp->setAabb(colObj->getBroadphaseHandle(), minAabb, maxAabb, m_dispatcher1);
178 }
179 else
180 {
181 //something went wrong, investigate
182 //this assert is unwanted in 3D modelers (danger of loosing work)
183 colObj->setActivationState(DISABLE_SIMULATION);
184
185 static bool reportMe = true;
186 if (reportMe && m_debugDrawer)
187 {
188 reportMe = false;
189 m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
190 m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
191 m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
192 m_debugDrawer->reportErrorWarning("Thanks.\n");
193 }
194 }
195 }
196
updateAabbs()197 void btCollisionWorld::updateAabbs()
198 {
199 BT_PROFILE("updateAabbs");
200
201 btTransform predictedTrans;
202 for (int i = 0; i < m_collisionObjects.size(); i++)
203 {
204 btCollisionObject* colObj = m_collisionObjects[i];
205 btAssert(colObj->getWorldArrayIndex() == i);
206
207 //only update aabb of active objects
208 if (m_forceUpdateAllAabbs || colObj->isActive())
209 {
210 updateSingleAabb(colObj);
211 }
212 }
213 }
214
computeOverlappingPairs()215 void btCollisionWorld::computeOverlappingPairs()
216 {
217 BT_PROFILE("calculateOverlappingPairs");
218 m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
219 }
220
performDiscreteCollisionDetection()221 void btCollisionWorld::performDiscreteCollisionDetection()
222 {
223 BT_PROFILE("performDiscreteCollisionDetection");
224
225 btDispatcherInfo& dispatchInfo = getDispatchInfo();
226
227 updateAabbs();
228
229 computeOverlappingPairs();
230
231 btDispatcher* dispatcher = getDispatcher();
232 {
233 BT_PROFILE("dispatchAllCollisionPairs");
234 if (dispatcher)
235 dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(), dispatchInfo, m_dispatcher1);
236 }
237 }
238
removeCollisionObject(btCollisionObject * collisionObject)239 void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
240 {
241 //bool removeFromBroadphase = false;
242
243 {
244 btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
245 if (bp)
246 {
247 //
248 // only clear the cached algorithms
249 //
250 getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp, m_dispatcher1);
251 getBroadphase()->destroyProxy(bp, m_dispatcher1);
252 collisionObject->setBroadphaseHandle(0);
253 }
254 }
255
256 int iObj = collisionObject->getWorldArrayIndex();
257 // btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously?
258 if (iObj >= 0 && iObj < m_collisionObjects.size())
259 {
260 btAssert(collisionObject == m_collisionObjects[iObj]);
261 m_collisionObjects.swap(iObj, m_collisionObjects.size() - 1);
262 m_collisionObjects.pop_back();
263 if (iObj < m_collisionObjects.size())
264 {
265 m_collisionObjects[iObj]->setWorldArrayIndex(iObj);
266 }
267 }
268 else
269 {
270 // slow linear search
271 //swapremove
272 m_collisionObjects.remove(collisionObject);
273 }
274 collisionObject->setWorldArrayIndex(-1);
275 }
276
rayTestSingle(const btTransform & rayFromTrans,const btTransform & rayToTrans,btCollisionObject * collisionObject,const btCollisionShape * collisionShape,const btTransform & colObjWorldTransform,RayResultCallback & resultCallback)277 void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
278 btCollisionObject* collisionObject,
279 const btCollisionShape* collisionShape,
280 const btTransform& colObjWorldTransform,
281 RayResultCallback& resultCallback)
282 {
283 btCollisionObjectWrapper colObWrap(0, collisionShape, collisionObject, colObjWorldTransform, -1, -1);
284 btCollisionWorld::rayTestSingleInternal(rayFromTrans, rayToTrans, &colObWrap, resultCallback);
285 }
286
rayTestSingleInternal(const btTransform & rayFromTrans,const btTransform & rayToTrans,const btCollisionObjectWrapper * collisionObjectWrap,RayResultCallback & resultCallback)287 void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, const btTransform& rayToTrans,
288 const btCollisionObjectWrapper* collisionObjectWrap,
289 RayResultCallback& resultCallback)
290 {
291 btSphereShape pointShape(btScalar(0.0));
292 pointShape.setMargin(0.f);
293 const btConvexShape* castShape = &pointShape;
294 const btCollisionShape* collisionShape = collisionObjectWrap->getCollisionShape();
295 const btTransform& colObjWorldTransform = collisionObjectWrap->getWorldTransform();
296
297 if (collisionShape->isConvex())
298 {
299 // BT_PROFILE("rayTestConvex");
300 btConvexCast::CastResult castResult;
301 castResult.m_fraction = resultCallback.m_closestHitFraction;
302
303 btConvexShape* convexShape = (btConvexShape*)collisionShape;
304 btVoronoiSimplexSolver simplexSolver;
305 btSubsimplexConvexCast subSimplexConvexCaster(castShape, convexShape, &simplexSolver);
306
307 btGjkConvexCast gjkConvexCaster(castShape, convexShape, &simplexSolver);
308
309 //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
310
311 btConvexCast* convexCasterPtr = 0;
312 //use kF_UseSubSimplexConvexCastRaytest by default
313 if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest)
314 convexCasterPtr = &gjkConvexCaster;
315 else
316 convexCasterPtr = &subSimplexConvexCaster;
317
318 btConvexCast& convexCaster = *convexCasterPtr;
319
320 if (convexCaster.calcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
321 {
322 //add hit
323 if (castResult.m_normal.length2() > btScalar(0.0001))
324 {
325 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
326 {
327 //todo: figure out what this is about. When is rayFromTest.getBasis() not identity?
328 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
329 //rotate normal into worldspace
330 castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
331 #endif //USE_SUBSIMPLEX_CONVEX_CAST
332
333 castResult.m_normal.normalize();
334 btCollisionWorld::LocalRayResult localRayResult(
335 collisionObjectWrap->getCollisionObject(),
336 0,
337 castResult.m_normal,
338 castResult.m_fraction);
339
340 bool normalInWorldSpace = true;
341 resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
342 }
343 }
344 }
345 }
346 else
347 {
348 if (collisionShape->isConcave())
349 {
350 //ConvexCast::CastResult
351 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
352 {
353 btCollisionWorld::RayResultCallback* m_resultCallback;
354 const btCollisionObject* m_collisionObject;
355 const btConcaveShape* m_triangleMesh;
356
357 btTransform m_colObjWorldTransform;
358
359 BridgeTriangleRaycastCallback(const btVector3& from, const btVector3& to,
360 btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject, const btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform) : //@BP Mod
361 btTriangleRaycastCallback(from, to, resultCallback->m_flags),
362 m_resultCallback(resultCallback),
363 m_collisionObject(collisionObject),
364 m_triangleMesh(triangleMesh),
365 m_colObjWorldTransform(colObjWorldTransform)
366 {
367 }
368
369 virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex)
370 {
371 btCollisionWorld::LocalShapeInfo shapeInfo;
372 shapeInfo.m_shapePart = partId;
373 shapeInfo.m_triangleIndex = triangleIndex;
374
375 btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
376
377 btCollisionWorld::LocalRayResult rayResult(m_collisionObject,
378 &shapeInfo,
379 hitNormalWorld,
380 hitFraction);
381
382 bool normalInWorldSpace = true;
383 return m_resultCallback->addSingleResult(rayResult, normalInWorldSpace);
384 }
385 };
386
387 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
388 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
389 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
390
391 // BT_PROFILE("rayTestConcave");
392 if (collisionShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
393 {
394 ///optimized version for btBvhTriangleMeshShape
395 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
396
397 BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), triangleMesh, colObjWorldTransform);
398 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
399 triangleMesh->performRaycast(&rcb, rayFromLocal, rayToLocal);
400 }
401 else if (collisionShape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
402 {
403 ///optimized version for btScaledBvhTriangleMeshShape
404 btScaledBvhTriangleMeshShape* scaledTriangleMesh = (btScaledBvhTriangleMeshShape*)collisionShape;
405 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)scaledTriangleMesh->getChildShape();
406
407 //scale the ray positions
408 btVector3 scale = scaledTriangleMesh->getLocalScaling();
409 btVector3 rayFromLocalScaled = rayFromLocal / scale;
410 btVector3 rayToLocalScaled = rayToLocal / scale;
411
412 //perform raycast in the underlying btBvhTriangleMeshShape
413 BridgeTriangleRaycastCallback rcb(rayFromLocalScaled, rayToLocalScaled, &resultCallback, collisionObjectWrap->getCollisionObject(), triangleMesh, colObjWorldTransform);
414 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
415 triangleMesh->performRaycast(&rcb, rayFromLocalScaled, rayToLocalScaled);
416 }
417 else if (((resultCallback.m_flags&btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator)==0)
418 && collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE
419 )
420 {
421 ///optimized version for btHeightfieldTerrainShape
422 btHeightfieldTerrainShape* heightField = (btHeightfieldTerrainShape*)collisionShape;
423 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
424 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
425 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
426
427 BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), heightField, colObjWorldTransform);
428 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
429 heightField->performRaycast(&rcb, rayFromLocal, rayToLocal);
430 }
431 else
432 {
433 //generic (slower) case
434 btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
435
436 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
437
438 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
439 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
440
441 //ConvexCast::CastResult
442
443 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
444 {
445 btCollisionWorld::RayResultCallback* m_resultCallback;
446 const btCollisionObject* m_collisionObject;
447 btConcaveShape* m_triangleMesh;
448
449 btTransform m_colObjWorldTransform;
450
451 BridgeTriangleRaycastCallback(const btVector3& from, const btVector3& to,
452 btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject, btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform) : //@BP Mod
453 btTriangleRaycastCallback(from, to, resultCallback->m_flags),
454 m_resultCallback(resultCallback),
455 m_collisionObject(collisionObject),
456 m_triangleMesh(triangleMesh),
457 m_colObjWorldTransform(colObjWorldTransform)
458 {
459 }
460
461 virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex)
462 {
463 btCollisionWorld::LocalShapeInfo shapeInfo;
464 shapeInfo.m_shapePart = partId;
465 shapeInfo.m_triangleIndex = triangleIndex;
466
467 btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
468
469 btCollisionWorld::LocalRayResult rayResult(m_collisionObject,
470 &shapeInfo,
471 hitNormalWorld,
472 hitFraction);
473
474 bool normalInWorldSpace = true;
475 return m_resultCallback->addSingleResult(rayResult, normalInWorldSpace);
476 }
477 };
478
479 BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), concaveShape, colObjWorldTransform);
480 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
481
482 btVector3 rayAabbMinLocal = rayFromLocal;
483 rayAabbMinLocal.setMin(rayToLocal);
484 btVector3 rayAabbMaxLocal = rayFromLocal;
485 rayAabbMaxLocal.setMax(rayToLocal);
486
487 concaveShape->processAllTriangles(&rcb, rayAabbMinLocal, rayAabbMaxLocal);
488 }
489 }
490 else
491 {
492 // BT_PROFILE("rayTestCompound");
493 if (collisionShape->isCompound())
494 {
495 struct LocalInfoAdder2 : public RayResultCallback
496 {
497 RayResultCallback* m_userCallback;
498 int m_i;
499
500 LocalInfoAdder2(int i, RayResultCallback* user)
501 : m_userCallback(user), m_i(i)
502 {
503 m_closestHitFraction = m_userCallback->m_closestHitFraction;
504 m_flags = m_userCallback->m_flags;
505 }
506 virtual bool needsCollision(btBroadphaseProxy* p) const
507 {
508 return m_userCallback->needsCollision(p);
509 }
510
511 virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& r, bool b)
512 {
513 btCollisionWorld::LocalShapeInfo shapeInfo;
514 shapeInfo.m_shapePart = -1;
515 shapeInfo.m_triangleIndex = m_i;
516 if (r.m_localShapeInfo == NULL)
517 r.m_localShapeInfo = &shapeInfo;
518
519 const btScalar result = m_userCallback->addSingleResult(r, b);
520 m_closestHitFraction = m_userCallback->m_closestHitFraction;
521 return result;
522 }
523 };
524
525 struct RayTester : btDbvt::ICollide
526 {
527 const btCollisionObject* m_collisionObject;
528 const btCompoundShape* m_compoundShape;
529 const btTransform& m_colObjWorldTransform;
530 const btTransform& m_rayFromTrans;
531 const btTransform& m_rayToTrans;
532 RayResultCallback& m_resultCallback;
533
534 RayTester(const btCollisionObject* collisionObject,
535 const btCompoundShape* compoundShape,
536 const btTransform& colObjWorldTransform,
537 const btTransform& rayFromTrans,
538 const btTransform& rayToTrans,
539 RayResultCallback& resultCallback) : m_collisionObject(collisionObject),
540 m_compoundShape(compoundShape),
541 m_colObjWorldTransform(colObjWorldTransform),
542 m_rayFromTrans(rayFromTrans),
543 m_rayToTrans(rayToTrans),
544 m_resultCallback(resultCallback)
545 {
546 }
547
548 void ProcessLeaf(int i)
549 {
550 const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
551 const btTransform& childTrans = m_compoundShape->getChildTransform(i);
552 btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
553
554 btCollisionObjectWrapper tmpOb(0, childCollisionShape, m_collisionObject, childWorldTrans, -1, i);
555 // replace collision shape so that callback can determine the triangle
556
557 LocalInfoAdder2 my_cb(i, &m_resultCallback);
558
559 rayTestSingleInternal(
560 m_rayFromTrans,
561 m_rayToTrans,
562 &tmpOb,
563 my_cb);
564 }
565
566 void Process(const btDbvtNode* leaf)
567 {
568 ProcessLeaf(leaf->dataAsInt);
569 }
570 };
571
572 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
573 const btDbvt* dbvt = compoundShape->getDynamicAabbTree();
574
575 RayTester rayCB(
576 collisionObjectWrap->getCollisionObject(),
577 compoundShape,
578 colObjWorldTransform,
579 rayFromTrans,
580 rayToTrans,
581 resultCallback);
582 #ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
583 if (dbvt)
584 {
585 btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
586 btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
587 btDbvt::rayTest(dbvt->m_root, localRayFrom, localRayTo, rayCB);
588 }
589 else
590 #endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
591 {
592 for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
593 {
594 rayCB.ProcessLeaf(i);
595 }
596 }
597 }
598 }
599 }
600 }
601
objectQuerySingle(const btConvexShape * castShape,const btTransform & convexFromTrans,const btTransform & convexToTrans,btCollisionObject * collisionObject,const btCollisionShape * collisionShape,const btTransform & colObjWorldTransform,ConvexResultCallback & resultCallback,btScalar allowedPenetration)602 void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans,
603 btCollisionObject* collisionObject,
604 const btCollisionShape* collisionShape,
605 const btTransform& colObjWorldTransform,
606 ConvexResultCallback& resultCallback, btScalar allowedPenetration)
607 {
608 btCollisionObjectWrapper tmpOb(0, collisionShape, collisionObject, colObjWorldTransform, -1, -1);
609 btCollisionWorld::objectQuerySingleInternal(castShape, convexFromTrans, convexToTrans, &tmpOb, resultCallback, allowedPenetration);
610 }
611
objectQuerySingleInternal(const btConvexShape * castShape,const btTransform & convexFromTrans,const btTransform & convexToTrans,const btCollisionObjectWrapper * colObjWrap,ConvexResultCallback & resultCallback,btScalar allowedPenetration)612 void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans,
613 const btCollisionObjectWrapper* colObjWrap,
614 ConvexResultCallback& resultCallback, btScalar allowedPenetration)
615 {
616 const btCollisionShape* collisionShape = colObjWrap->getCollisionShape();
617 const btTransform& colObjWorldTransform = colObjWrap->getWorldTransform();
618
619 if (collisionShape->isConvex())
620 {
621 //BT_PROFILE("convexSweepConvex");
622 btConvexCast::CastResult castResult;
623 castResult.m_allowedPenetration = allowedPenetration;
624 castResult.m_fraction = resultCallback.m_closestHitFraction; //btScalar(1.);//??
625
626 btConvexShape* convexShape = (btConvexShape*)collisionShape;
627 btVoronoiSimplexSolver simplexSolver;
628 btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
629
630 btContinuousConvexCollision convexCaster1(castShape, convexShape, &simplexSolver, &gjkEpaPenetrationSolver);
631 //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
632 //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
633
634 btConvexCast* castPtr = &convexCaster1;
635
636 if (castPtr->calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
637 {
638 //add hit
639 if (castResult.m_normal.length2() > btScalar(0.0001))
640 {
641 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
642 {
643 castResult.m_normal.normalize();
644 btCollisionWorld::LocalConvexResult localConvexResult(
645 colObjWrap->getCollisionObject(),
646 0,
647 castResult.m_normal,
648 castResult.m_hitPoint,
649 castResult.m_fraction);
650
651 bool normalInWorldSpace = true;
652 resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
653 }
654 }
655 }
656 }
657 else
658 {
659 if (collisionShape->isConcave())
660 {
661 if (collisionShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
662 {
663 //BT_PROFILE("convexSweepbtBvhTriangleMesh");
664 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
665 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
666 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
667 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
668 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
669 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
670
671 //ConvexCast::CastResult
672 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
673 {
674 btCollisionWorld::ConvexResultCallback* m_resultCallback;
675 const btCollisionObject* m_collisionObject;
676 btTriangleMeshShape* m_triangleMesh;
677
678 BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from, const btTransform& to,
679 btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject, btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld) : btTriangleConvexcastCallback(castShape, from, to, triangleToWorld, triangleMesh->getMargin()),
680 m_resultCallback(resultCallback),
681 m_collisionObject(collisionObject),
682 m_triangleMesh(triangleMesh)
683 {
684 }
685
686 virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex)
687 {
688 btCollisionWorld::LocalShapeInfo shapeInfo;
689 shapeInfo.m_shapePart = partId;
690 shapeInfo.m_triangleIndex = triangleIndex;
691 if (hitFraction <= m_resultCallback->m_closestHitFraction)
692 {
693 btCollisionWorld::LocalConvexResult convexResult(m_collisionObject,
694 &shapeInfo,
695 hitNormalLocal,
696 hitPointLocal,
697 hitFraction);
698
699 bool normalInWorldSpace = true;
700
701 return m_resultCallback->addSingleResult(convexResult, normalInWorldSpace);
702 }
703 return hitFraction;
704 }
705 };
706
707 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans, convexToTrans, &resultCallback, colObjWrap->getCollisionObject(), triangleMesh, colObjWorldTransform);
708 tccb.m_hitFraction = resultCallback.m_closestHitFraction;
709 tccb.m_allowedPenetration = allowedPenetration;
710 btVector3 boxMinLocal, boxMaxLocal;
711 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
712 triangleMesh->performConvexcast(&tccb, convexFromLocal, convexToLocal, boxMinLocal, boxMaxLocal);
713 }
714 else
715 {
716 if (collisionShape->getShapeType() == STATIC_PLANE_PROXYTYPE)
717 {
718 btConvexCast::CastResult castResult;
719 castResult.m_allowedPenetration = allowedPenetration;
720 castResult.m_fraction = resultCallback.m_closestHitFraction;
721 btStaticPlaneShape* planeShape = (btStaticPlaneShape*)collisionShape;
722 btContinuousConvexCollision convexCaster1(castShape, planeShape);
723 btConvexCast* castPtr = &convexCaster1;
724
725 if (castPtr->calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
726 {
727 //add hit
728 if (castResult.m_normal.length2() > btScalar(0.0001))
729 {
730 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
731 {
732 castResult.m_normal.normalize();
733 btCollisionWorld::LocalConvexResult localConvexResult(
734 colObjWrap->getCollisionObject(),
735 0,
736 castResult.m_normal,
737 castResult.m_hitPoint,
738 castResult.m_fraction);
739
740 bool normalInWorldSpace = true;
741 resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
742 }
743 }
744 }
745 }
746 else
747 {
748 //BT_PROFILE("convexSweepConcave");
749 btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
750 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
751 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
752 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
753 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
754 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
755
756 //ConvexCast::CastResult
757 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
758 {
759 btCollisionWorld::ConvexResultCallback* m_resultCallback;
760 const btCollisionObject* m_collisionObject;
761 btConcaveShape* m_triangleMesh;
762
763 BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from, const btTransform& to,
764 btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject, btConcaveShape* triangleMesh, const btTransform& triangleToWorld) : btTriangleConvexcastCallback(castShape, from, to, triangleToWorld, triangleMesh->getMargin()),
765 m_resultCallback(resultCallback),
766 m_collisionObject(collisionObject),
767 m_triangleMesh(triangleMesh)
768 {
769 }
770
771 virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex)
772 {
773 btCollisionWorld::LocalShapeInfo shapeInfo;
774 shapeInfo.m_shapePart = partId;
775 shapeInfo.m_triangleIndex = triangleIndex;
776 if (hitFraction <= m_resultCallback->m_closestHitFraction)
777 {
778 btCollisionWorld::LocalConvexResult convexResult(m_collisionObject,
779 &shapeInfo,
780 hitNormalLocal,
781 hitPointLocal,
782 hitFraction);
783
784 bool normalInWorldSpace = true;
785
786 return m_resultCallback->addSingleResult(convexResult, normalInWorldSpace);
787 }
788 return hitFraction;
789 }
790 };
791
792 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans, convexToTrans, &resultCallback, colObjWrap->getCollisionObject(), concaveShape, colObjWorldTransform);
793 tccb.m_hitFraction = resultCallback.m_closestHitFraction;
794 tccb.m_allowedPenetration = allowedPenetration;
795 btVector3 boxMinLocal, boxMaxLocal;
796 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
797
798 btVector3 rayAabbMinLocal = convexFromLocal;
799 rayAabbMinLocal.setMin(convexToLocal);
800 btVector3 rayAabbMaxLocal = convexFromLocal;
801 rayAabbMaxLocal.setMax(convexToLocal);
802 rayAabbMinLocal += boxMinLocal;
803 rayAabbMaxLocal += boxMaxLocal;
804 concaveShape->processAllTriangles(&tccb, rayAabbMinLocal, rayAabbMaxLocal);
805 }
806 }
807 }
808 else
809 {
810 if (collisionShape->isCompound())
811 {
812 struct btCompoundLeafCallback : btDbvt::ICollide
813 {
814 btCompoundLeafCallback(
815 const btCollisionObjectWrapper* colObjWrap,
816 const btConvexShape* castShape,
817 const btTransform& convexFromTrans,
818 const btTransform& convexToTrans,
819 btScalar allowedPenetration,
820 const btCompoundShape* compoundShape,
821 const btTransform& colObjWorldTransform,
822 ConvexResultCallback& resultCallback)
823 : m_colObjWrap(colObjWrap),
824 m_castShape(castShape),
825 m_convexFromTrans(convexFromTrans),
826 m_convexToTrans(convexToTrans),
827 m_allowedPenetration(allowedPenetration),
828 m_compoundShape(compoundShape),
829 m_colObjWorldTransform(colObjWorldTransform),
830 m_resultCallback(resultCallback)
831 {
832 }
833
834 const btCollisionObjectWrapper* m_colObjWrap;
835 const btConvexShape* m_castShape;
836 const btTransform& m_convexFromTrans;
837 const btTransform& m_convexToTrans;
838 btScalar m_allowedPenetration;
839 const btCompoundShape* m_compoundShape;
840 const btTransform& m_colObjWorldTransform;
841 ConvexResultCallback& m_resultCallback;
842
843 public:
844 void ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape)
845 {
846 btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
847
848 struct LocalInfoAdder : public ConvexResultCallback
849 {
850 ConvexResultCallback* m_userCallback;
851 int m_i;
852
853 LocalInfoAdder(int i, ConvexResultCallback* user)
854 : m_userCallback(user), m_i(i)
855 {
856 m_closestHitFraction = m_userCallback->m_closestHitFraction;
857 }
858 virtual bool needsCollision(btBroadphaseProxy* p) const
859 {
860 return m_userCallback->needsCollision(p);
861 }
862 virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& r, bool b)
863 {
864 btCollisionWorld::LocalShapeInfo shapeInfo;
865 shapeInfo.m_shapePart = -1;
866 shapeInfo.m_triangleIndex = m_i;
867 if (r.m_localShapeInfo == NULL)
868 r.m_localShapeInfo = &shapeInfo;
869 const btScalar result = m_userCallback->addSingleResult(r, b);
870 m_closestHitFraction = m_userCallback->m_closestHitFraction;
871 return result;
872 }
873 };
874
875 LocalInfoAdder my_cb(index, &m_resultCallback);
876
877 btCollisionObjectWrapper tmpObj(m_colObjWrap, childCollisionShape, m_colObjWrap->getCollisionObject(), childWorldTrans, -1, index);
878
879 objectQuerySingleInternal(m_castShape, m_convexFromTrans, m_convexToTrans, &tmpObj, my_cb, m_allowedPenetration);
880 }
881
882 void Process(const btDbvtNode* leaf)
883 {
884 // Processing leaf node
885 int index = leaf->dataAsInt;
886
887 btTransform childTrans = m_compoundShape->getChildTransform(index);
888 const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(index);
889
890 ProcessChild(index, childTrans, childCollisionShape);
891 }
892 };
893
894 BT_PROFILE("convexSweepCompound");
895 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
896
897 btVector3 fromLocalAabbMin, fromLocalAabbMax;
898 btVector3 toLocalAabbMin, toLocalAabbMax;
899
900 castShape->getAabb(colObjWorldTransform.inverse() * convexFromTrans, fromLocalAabbMin, fromLocalAabbMax);
901 castShape->getAabb(colObjWorldTransform.inverse() * convexToTrans, toLocalAabbMin, toLocalAabbMax);
902
903 fromLocalAabbMin.setMin(toLocalAabbMin);
904 fromLocalAabbMax.setMax(toLocalAabbMax);
905
906 btCompoundLeafCallback callback(colObjWrap, castShape, convexFromTrans, convexToTrans,
907 allowedPenetration, compoundShape, colObjWorldTransform, resultCallback);
908
909 const btDbvt* tree = compoundShape->getDynamicAabbTree();
910 if (tree)
911 {
912 const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax);
913 tree->collideTV(tree->m_root, bounds, callback);
914 }
915 else
916 {
917 int i;
918 for (i = 0; i < compoundShape->getNumChildShapes(); i++)
919 {
920 const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
921 btTransform childTrans = compoundShape->getChildTransform(i);
922 callback.ProcessChild(i, childTrans, childCollisionShape);
923 }
924 }
925 }
926 }
927 }
928 }
929
930 struct btSingleRayCallback : public btBroadphaseRayCallback
931 {
932 btVector3 m_rayFromWorld;
933 btVector3 m_rayToWorld;
934 btTransform m_rayFromTrans;
935 btTransform m_rayToTrans;
936 btVector3 m_hitNormal;
937
938 const btCollisionWorld* m_world;
939 btCollisionWorld::RayResultCallback& m_resultCallback;
940
btSingleRayCallbackbtSingleRayCallback941 btSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btCollisionWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
942 : m_rayFromWorld(rayFromWorld),
943 m_rayToWorld(rayToWorld),
944 m_world(world),
945 m_resultCallback(resultCallback)
946 {
947 m_rayFromTrans.setIdentity();
948 m_rayFromTrans.setOrigin(m_rayFromWorld);
949 m_rayToTrans.setIdentity();
950 m_rayToTrans.setOrigin(m_rayToWorld);
951
952 btVector3 rayDir = (rayToWorld - rayFromWorld);
953
954 rayDir.normalize();
955 ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
956 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
957 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
958 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
959 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
960 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
961 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
962
963 m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
964 }
965
processbtSingleRayCallback966 virtual bool process(const btBroadphaseProxy* proxy)
967 {
968 ///terminate further ray tests, once the closestHitFraction reached zero
969 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
970 return false;
971
972 btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
973
974 //only perform raycast if filterMask matches
975 if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
976 {
977 //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
978 //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
979 #if 0
980 #ifdef RECALCULATE_AABB
981 btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
982 collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
983 #else
984 //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
985 const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
986 const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
987 #endif
988 #endif
989 //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
990 //culling already done by broadphase
991 //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
992 {
993 m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
994 collisionObject,
995 collisionObject->getCollisionShape(),
996 collisionObject->getWorldTransform(),
997 m_resultCallback);
998 }
999 }
1000 return true;
1001 }
1002 };
1003
rayTest(const btVector3 & rayFromWorld,const btVector3 & rayToWorld,RayResultCallback & resultCallback) const1004 void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
1005 {
1006 //BT_PROFILE("rayTest");
1007 /// use the broadphase to accelerate the search for objects, based on their aabb
1008 /// and for each object with ray-aabb overlap, perform an exact ray test
1009 btSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
1010
1011 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
1012 m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
1013 #else
1014 for (int i = 0; i < this->getNumCollisionObjects(); i++)
1015 {
1016 rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
1017 }
1018 #endif //USE_BRUTEFORCE_RAYBROADPHASE
1019 }
1020
1021 struct btSingleSweepCallback : public btBroadphaseRayCallback
1022 {
1023 btTransform m_convexFromTrans;
1024 btTransform m_convexToTrans;
1025 btVector3 m_hitNormal;
1026 const btCollisionWorld* m_world;
1027 btCollisionWorld::ConvexResultCallback& m_resultCallback;
1028 btScalar m_allowedCcdPenetration;
1029 const btConvexShape* m_castShape;
1030
btSingleSweepCallbackbtSingleSweepCallback1031 btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, const btCollisionWorld* world, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedPenetration)
1032 : m_convexFromTrans(convexFromTrans),
1033 m_convexToTrans(convexToTrans),
1034 m_world(world),
1035 m_resultCallback(resultCallback),
1036 m_allowedCcdPenetration(allowedPenetration),
1037 m_castShape(castShape)
1038 {
1039 btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin() - m_convexFromTrans.getOrigin());
1040 btVector3 rayDir = unnormalizedRayDir.normalized();
1041 ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
1042 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
1043 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
1044 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
1045 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
1046 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
1047 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
1048
1049 m_lambda_max = rayDir.dot(unnormalizedRayDir);
1050 }
1051
processbtSingleSweepCallback1052 virtual bool process(const btBroadphaseProxy* proxy)
1053 {
1054 ///terminate further convex sweep tests, once the closestHitFraction reached zero
1055 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
1056 return false;
1057
1058 btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
1059
1060 //only perform raycast if filterMask matches
1061 if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
1062 {
1063 //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
1064 m_world->objectQuerySingle(m_castShape, m_convexFromTrans, m_convexToTrans,
1065 collisionObject,
1066 collisionObject->getCollisionShape(),
1067 collisionObject->getWorldTransform(),
1068 m_resultCallback,
1069 m_allowedCcdPenetration);
1070 }
1071
1072 return true;
1073 }
1074 };
1075
convexSweepTest(const btConvexShape * castShape,const btTransform & convexFromWorld,const btTransform & convexToWorld,ConvexResultCallback & resultCallback,btScalar allowedCcdPenetration) const1076 void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
1077 {
1078 BT_PROFILE("convexSweepTest");
1079 /// use the broadphase to accelerate the search for objects, based on their aabb
1080 /// and for each object with ray-aabb overlap, perform an exact ray test
1081 /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
1082
1083 btTransform convexFromTrans, convexToTrans;
1084 convexFromTrans = convexFromWorld;
1085 convexToTrans = convexToWorld;
1086 btVector3 castShapeAabbMin, castShapeAabbMax;
1087 /* Compute AABB that encompasses angular movement */
1088 {
1089 btVector3 linVel, angVel;
1090 btTransformUtil::calculateVelocity(convexFromTrans, convexToTrans, 1.0f, linVel, angVel);
1091 btVector3 zeroLinVel;
1092 zeroLinVel.setValue(0, 0, 0);
1093 btTransform R;
1094 R.setIdentity();
1095 R.setRotation(convexFromTrans.getRotation());
1096 castShape->calculateTemporalAabb(R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax);
1097 }
1098
1099 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
1100
1101 btSingleSweepCallback convexCB(castShape, convexFromWorld, convexToWorld, this, resultCallback, allowedCcdPenetration);
1102
1103 m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(), convexToTrans.getOrigin(), convexCB, castShapeAabbMin, castShapeAabbMax);
1104
1105 #else
1106 /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
1107 // do a ray-shape query using convexCaster (CCD)
1108 int i;
1109 for (i = 0; i < m_collisionObjects.size(); i++)
1110 {
1111 btCollisionObject* collisionObject = m_collisionObjects[i];
1112 //only perform raycast if filterMask matches
1113 if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
1114 {
1115 //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
1116 btVector3 collisionObjectAabbMin, collisionObjectAabbMax;
1117 collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax);
1118 AabbExpand(collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
1119 btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
1120 btVector3 hitNormal;
1121 if (btRayAabb(convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal))
1122 {
1123 objectQuerySingle(castShape, convexFromTrans, convexToTrans,
1124 collisionObject,
1125 collisionObject->getCollisionShape(),
1126 collisionObject->getWorldTransform(),
1127 resultCallback,
1128 allowedCcdPenetration);
1129 }
1130 }
1131 }
1132 #endif //USE_BRUTEFORCE_RAYBROADPHASE
1133 }
1134
1135 struct btBridgedManifoldResult : public btManifoldResult
1136 {
1137 btCollisionWorld::ContactResultCallback& m_resultCallback;
1138
btBridgedManifoldResultbtBridgedManifoldResult1139 btBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap, btCollisionWorld::ContactResultCallback& resultCallback)
1140 : btManifoldResult(obj0Wrap, obj1Wrap),
1141 m_resultCallback(resultCallback)
1142 {
1143 }
1144
addContactPointbtBridgedManifoldResult1145 virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
1146 {
1147 bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
1148 btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
1149 btVector3 localA;
1150 btVector3 localB;
1151 if (isSwapped)
1152 {
1153 localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA);
1154 localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
1155 }
1156 else
1157 {
1158 localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA);
1159 localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
1160 }
1161
1162 btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth);
1163 newPt.m_positionWorldOnA = pointA;
1164 newPt.m_positionWorldOnB = pointInWorld;
1165
1166 //BP mod, store contact triangles.
1167 if (isSwapped)
1168 {
1169 newPt.m_partId0 = m_partId1;
1170 newPt.m_partId1 = m_partId0;
1171 newPt.m_index0 = m_index1;
1172 newPt.m_index1 = m_index0;
1173 }
1174 else
1175 {
1176 newPt.m_partId0 = m_partId0;
1177 newPt.m_partId1 = m_partId1;
1178 newPt.m_index0 = m_index0;
1179 newPt.m_index1 = m_index1;
1180 }
1181
1182 //experimental feature info, for per-triangle material etc.
1183 const btCollisionObjectWrapper* obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap;
1184 const btCollisionObjectWrapper* obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap;
1185 m_resultCallback.addSingleResult(newPt, obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1);
1186 }
1187 };
1188
1189 struct btSingleContactCallback : public btBroadphaseAabbCallback
1190 {
1191 btCollisionObject* m_collisionObject;
1192 btCollisionWorld* m_world;
1193 btCollisionWorld::ContactResultCallback& m_resultCallback;
1194
btSingleContactCallbackbtSingleContactCallback1195 btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world, btCollisionWorld::ContactResultCallback& resultCallback)
1196 : m_collisionObject(collisionObject),
1197 m_world(world),
1198 m_resultCallback(resultCallback)
1199 {
1200 }
1201
processbtSingleContactCallback1202 virtual bool process(const btBroadphaseProxy* proxy)
1203 {
1204 btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
1205 if (collisionObject == m_collisionObject)
1206 return true;
1207
1208 //only perform raycast if filterMask matches
1209 if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
1210 {
1211 btCollisionObjectWrapper ob0(0, m_collisionObject->getCollisionShape(), m_collisionObject, m_collisionObject->getWorldTransform(), -1, -1);
1212 btCollisionObjectWrapper ob1(0, collisionObject->getCollisionShape(), collisionObject, collisionObject->getWorldTransform(), -1, -1);
1213
1214 btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0, &ob1, 0, BT_CLOSEST_POINT_ALGORITHMS);
1215 if (algorithm)
1216 {
1217 btBridgedManifoldResult contactPointResult(&ob0, &ob1, m_resultCallback);
1218 //discrete collision detection query
1219
1220 algorithm->processCollision(&ob0, &ob1, m_world->getDispatchInfo(), &contactPointResult);
1221
1222 algorithm->~btCollisionAlgorithm();
1223 m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
1224 }
1225 }
1226 return true;
1227 }
1228 };
1229
1230 ///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
1231 ///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
contactTest(btCollisionObject * colObj,ContactResultCallback & resultCallback)1232 void btCollisionWorld::contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback)
1233 {
1234 btVector3 aabbMin, aabbMax;
1235 colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), aabbMin, aabbMax);
1236 btSingleContactCallback contactCB(colObj, this, resultCallback);
1237
1238 m_broadphasePairCache->aabbTest(aabbMin, aabbMax, contactCB);
1239 }
1240
1241 ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
1242 ///it reports one or more contact points (including the one with deepest penetration)
contactPairTest(btCollisionObject * colObjA,btCollisionObject * colObjB,ContactResultCallback & resultCallback)1243 void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
1244 {
1245 btCollisionObjectWrapper obA(0, colObjA->getCollisionShape(), colObjA, colObjA->getWorldTransform(), -1, -1);
1246 btCollisionObjectWrapper obB(0, colObjB->getCollisionShape(), colObjB, colObjB->getWorldTransform(), -1, -1);
1247
1248 btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA, &obB, 0, BT_CLOSEST_POINT_ALGORITHMS);
1249 if (algorithm)
1250 {
1251 btBridgedManifoldResult contactPointResult(&obA, &obB, resultCallback);
1252 contactPointResult.m_closestPointDistanceThreshold = resultCallback.m_closestDistanceThreshold;
1253 //discrete collision detection query
1254 algorithm->processCollision(&obA, &obB, getDispatchInfo(), &contactPointResult);
1255
1256 algorithm->~btCollisionAlgorithm();
1257 getDispatcher()->freeCollisionAlgorithm(algorithm);
1258 }
1259 }
1260
1261 class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
1262 {
1263 btIDebugDraw* m_debugDrawer;
1264 btVector3 m_color;
1265 btTransform m_worldTrans;
1266
1267 public:
DebugDrawcallback(btIDebugDraw * debugDrawer,const btTransform & worldTrans,const btVector3 & color)1268 DebugDrawcallback(btIDebugDraw* debugDrawer, const btTransform& worldTrans, const btVector3& color) : m_debugDrawer(debugDrawer),
1269 m_color(color),
1270 m_worldTrans(worldTrans)
1271 {
1272 }
1273
internalProcessTriangleIndex(btVector3 * triangle,int partId,int triangleIndex)1274 virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
1275 {
1276 processTriangle(triangle, partId, triangleIndex);
1277 }
1278
processTriangle(btVector3 * triangle,int partId,int triangleIndex)1279 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
1280 {
1281 (void)partId;
1282 (void)triangleIndex;
1283
1284 btVector3 wv0, wv1, wv2;
1285 wv0 = m_worldTrans * triangle[0];
1286 wv1 = m_worldTrans * triangle[1];
1287 wv2 = m_worldTrans * triangle[2];
1288 btVector3 center = (wv0 + wv1 + wv2) * btScalar(1. / 3.);
1289
1290 if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals)
1291 {
1292 btVector3 normal = (wv1 - wv0).cross(wv2 - wv0);
1293 normal.normalize();
1294 btVector3 normalColor(1, 1, 0);
1295 m_debugDrawer->drawLine(center, center + normal, normalColor);
1296 }
1297 m_debugDrawer->drawLine(wv0, wv1, m_color);
1298 m_debugDrawer->drawLine(wv1, wv2, m_color);
1299 m_debugDrawer->drawLine(wv2, wv0, m_color);
1300 }
1301 };
1302
debugDrawObject(const btTransform & worldTransform,const btCollisionShape * shape,const btVector3 & color)1303 void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
1304 {
1305 // Draw a small simplex at the center of the object
1306 if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames)
1307 {
1308 getDebugDrawer()->drawTransform(worldTransform, .1);
1309 }
1310
1311 if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
1312 {
1313 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
1314 for (int i = compoundShape->getNumChildShapes() - 1; i >= 0; i--)
1315 {
1316 btTransform childTrans = compoundShape->getChildTransform(i);
1317 const btCollisionShape* colShape = compoundShape->getChildShape(i);
1318 debugDrawObject(worldTransform * childTrans, colShape, color);
1319 }
1320 }
1321 else
1322 {
1323 switch (shape->getShapeType())
1324 {
1325 case BOX_SHAPE_PROXYTYPE:
1326 {
1327 const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
1328 btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
1329 getDebugDrawer()->drawBox(-halfExtents, halfExtents, worldTransform, color);
1330 break;
1331 }
1332
1333 case SPHERE_SHAPE_PROXYTYPE:
1334 {
1335 const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
1336 btScalar radius = sphereShape->getMargin(); //radius doesn't include the margin, so draw with margin
1337
1338 getDebugDrawer()->drawSphere(radius, worldTransform, color);
1339 break;
1340 }
1341 case MULTI_SPHERE_SHAPE_PROXYTYPE:
1342 {
1343 const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
1344
1345 btTransform childTransform;
1346 childTransform.setIdentity();
1347
1348 for (int i = multiSphereShape->getSphereCount() - 1; i >= 0; i--)
1349 {
1350 childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
1351 getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform * childTransform, color);
1352 }
1353
1354 break;
1355 }
1356 case CAPSULE_SHAPE_PROXYTYPE:
1357 {
1358 const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
1359
1360 btScalar radius = capsuleShape->getRadius();
1361 btScalar halfHeight = capsuleShape->getHalfHeight();
1362
1363 int upAxis = capsuleShape->getUpAxis();
1364 getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
1365 break;
1366 }
1367 case CONE_SHAPE_PROXYTYPE:
1368 {
1369 const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
1370 btScalar radius = coneShape->getRadius(); //+coneShape->getMargin();
1371 btScalar height = coneShape->getHeight(); //+coneShape->getMargin();
1372
1373 int upAxis = coneShape->getConeUpIndex();
1374 getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
1375 break;
1376 }
1377 case CYLINDER_SHAPE_PROXYTYPE:
1378 {
1379 const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
1380 int upAxis = cylinder->getUpAxis();
1381 btScalar radius = cylinder->getRadius();
1382 btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
1383 getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
1384 break;
1385 }
1386
1387 case STATIC_PLANE_PROXYTYPE:
1388 {
1389 const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
1390 btScalar planeConst = staticPlaneShape->getPlaneConstant();
1391 const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
1392 getDebugDrawer()->drawPlane(planeNormal, planeConst, worldTransform, color);
1393 break;
1394 }
1395 default:
1396 {
1397 /// for polyhedral shapes
1398 if (shape->isPolyhedral())
1399 {
1400 btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*)shape;
1401
1402 int i;
1403 if (polyshape->getConvexPolyhedron())
1404 {
1405 const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
1406 for (i = 0; i < poly->m_faces.size(); i++)
1407 {
1408 btVector3 centroid(0, 0, 0);
1409 int numVerts = poly->m_faces[i].m_indices.size();
1410 if (numVerts)
1411 {
1412 int lastV = poly->m_faces[i].m_indices[numVerts - 1];
1413 for (int v = 0; v < poly->m_faces[i].m_indices.size(); v++)
1414 {
1415 int curVert = poly->m_faces[i].m_indices[v];
1416 centroid += poly->m_vertices[curVert];
1417 getDebugDrawer()->drawLine(worldTransform * poly->m_vertices[lastV], worldTransform * poly->m_vertices[curVert], color);
1418 lastV = curVert;
1419 }
1420 }
1421 centroid *= btScalar(1.f) / btScalar(numVerts);
1422 if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals)
1423 {
1424 btVector3 normalColor(1, 1, 0);
1425 btVector3 faceNormal(poly->m_faces[i].m_plane[0], poly->m_faces[i].m_plane[1], poly->m_faces[i].m_plane[2]);
1426 getDebugDrawer()->drawLine(worldTransform * centroid, worldTransform * (centroid + faceNormal), normalColor);
1427 }
1428 }
1429 }
1430 else
1431 {
1432 for (i = 0; i < polyshape->getNumEdges(); i++)
1433 {
1434 btVector3 a, b;
1435 polyshape->getEdge(i, a, b);
1436 btVector3 wa = worldTransform * a;
1437 btVector3 wb = worldTransform * b;
1438 getDebugDrawer()->drawLine(wa, wb, color);
1439 }
1440 }
1441 }
1442
1443 if (shape->isConcave())
1444 {
1445 btConcaveShape* concaveMesh = (btConcaveShape*)shape;
1446
1447 ///@todo pass camera, for some culling? no -> we are not a graphics lib
1448 btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
1449 btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
1450
1451 DebugDrawcallback drawCallback(getDebugDrawer(), worldTransform, color);
1452 concaveMesh->processAllTriangles(&drawCallback, aabbMin, aabbMax);
1453 }
1454
1455 if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
1456 {
1457 btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*)shape;
1458 //todo: pass camera for some culling
1459 btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
1460 btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
1461 //DebugDrawcallback drawCallback;
1462 DebugDrawcallback drawCallback(getDebugDrawer(), worldTransform, color);
1463 convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback, aabbMin, aabbMax);
1464 }
1465 }
1466 }
1467 }
1468 }
1469
debugDrawWorld()1470 void btCollisionWorld::debugDrawWorld()
1471 {
1472 if (getDebugDrawer())
1473 {
1474 getDebugDrawer()->clearLines();
1475
1476 btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors();
1477
1478 if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
1479 {
1480 if (getDispatcher())
1481 {
1482 int numManifolds = getDispatcher()->getNumManifolds();
1483
1484 for (int i = 0; i < numManifolds; i++)
1485 {
1486 btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
1487 //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
1488 //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
1489
1490 int numContacts = contactManifold->getNumContacts();
1491 for (int j = 0; j < numContacts; j++)
1492 {
1493 btManifoldPoint& cp = contactManifold->getContactPoint(j);
1494 getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB, cp.m_normalWorldOnB, cp.getDistance(), cp.getLifeTime(), defaultColors.m_contactPoint);
1495 }
1496 }
1497 }
1498 }
1499
1500 if ((getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)))
1501 {
1502 int i;
1503
1504 for (i = 0; i < m_collisionObjects.size(); i++)
1505 {
1506 btCollisionObject* colObj = m_collisionObjects[i];
1507 if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT) == 0)
1508 {
1509 if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
1510 {
1511 btVector3 color(btScalar(0.4), btScalar(0.4), btScalar(0.4));
1512
1513 switch (colObj->getActivationState())
1514 {
1515 case ACTIVE_TAG:
1516 color = defaultColors.m_activeObject;
1517 break;
1518 case ISLAND_SLEEPING:
1519 color = defaultColors.m_deactivatedObject;
1520 break;
1521 case WANTS_DEACTIVATION:
1522 color = defaultColors.m_wantsDeactivationObject;
1523 break;
1524 case DISABLE_DEACTIVATION:
1525 color = defaultColors.m_disabledDeactivationObject;
1526 break;
1527 case DISABLE_SIMULATION:
1528 color = defaultColors.m_disabledSimulationObject;
1529 break;
1530 default:
1531 {
1532 color = btVector3(btScalar(.3), btScalar(0.3), btScalar(0.3));
1533 }
1534 };
1535
1536 colObj->getCustomDebugColor(color);
1537
1538 debugDrawObject(colObj->getWorldTransform(), colObj->getCollisionShape(), color);
1539 }
1540 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
1541 {
1542 btVector3 minAabb, maxAabb;
1543 btVector3 colorvec = defaultColors.m_aabb;
1544 colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb);
1545 btVector3 contactThreshold(gContactBreakingThreshold, gContactBreakingThreshold, gContactBreakingThreshold);
1546 minAabb -= contactThreshold;
1547 maxAabb += contactThreshold;
1548
1549 btVector3 minAabb2, maxAabb2;
1550
1551 if (getDispatchInfo().m_useContinuous && colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
1552 {
1553 colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(), minAabb2, maxAabb2);
1554 minAabb2 -= contactThreshold;
1555 maxAabb2 += contactThreshold;
1556 minAabb.setMin(minAabb2);
1557 maxAabb.setMax(maxAabb2);
1558 }
1559
1560 m_debugDrawer->drawAabb(minAabb, maxAabb, colorvec);
1561 }
1562 }
1563 }
1564 }
1565 }
1566 }
1567
serializeCollisionObjects(btSerializer * serializer)1568 void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
1569 {
1570 int i;
1571
1572 ///keep track of shapes already serialized
1573 btHashMap<btHashPtr, btCollisionShape*> serializedShapes;
1574
1575 for (i = 0; i < m_collisionObjects.size(); i++)
1576 {
1577 btCollisionObject* colObj = m_collisionObjects[i];
1578 btCollisionShape* shape = colObj->getCollisionShape();
1579
1580 if (!serializedShapes.find(shape))
1581 {
1582 serializedShapes.insert(shape, shape);
1583 shape->serializeSingleShape(serializer);
1584 }
1585 }
1586
1587 //serialize all collision objects
1588 for (i = 0; i < m_collisionObjects.size(); i++)
1589 {
1590 btCollisionObject* colObj = m_collisionObjects[i];
1591 if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
1592 {
1593 colObj->serializeSingleObject(serializer);
1594 }
1595 }
1596 }
1597
serializeContactManifolds(btSerializer * serializer)1598 void btCollisionWorld::serializeContactManifolds(btSerializer* serializer)
1599 {
1600 if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS)
1601 {
1602 int numManifolds = getDispatcher()->getNumManifolds();
1603 for (int i = 0; i < numManifolds; i++)
1604 {
1605 const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i];
1606 //don't serialize empty manifolds, they just take space
1607 //(may have to do it anyway if it destroys determinism)
1608 if (manifold->getNumContacts() == 0)
1609 continue;
1610
1611 btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1);
1612 const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer);
1613 serializer->finalizeChunk(chunk, structType, BT_CONTACTMANIFOLD_CODE, (void*)manifold);
1614 }
1615 }
1616 }
1617
serialize(btSerializer * serializer)1618 void btCollisionWorld::serialize(btSerializer* serializer)
1619 {
1620 serializer->startSerialization();
1621
1622 serializeCollisionObjects(serializer);
1623
1624 serializeContactManifolds(serializer);
1625
1626 serializer->finishSerialization();
1627 }
1628