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