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