Bullet patch for compound ray cast. The patch is filed in Bullet patch tracker for...
[blender-staging.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
30 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
31 #include "LinearMath/btAabbUtil2.h"
32 #include "LinearMath/btQuickprof.h"
33 #include "LinearMath/btStackAlloc.h"
34
35
36 //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
37 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
38 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
39 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
40
41
42 btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
43 :m_dispatcher1(dispatcher),
44 m_broadphasePairCache(pairCache),
45 m_debugDrawer(0)
46 {
47         m_stackAlloc = collisionConfiguration->getStackAllocator();
48         m_dispatchInfo.m_stackAllocator = m_stackAlloc;
49 }
50
51
52 btCollisionWorld::~btCollisionWorld()
53 {
54
55         //clean up remaining objects
56         int i;
57         for (i=0;i<m_collisionObjects.size();i++)
58         {
59                 btCollisionObject* collisionObject= m_collisionObjects[i];
60
61                 btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
62                 if (bp)
63                 {
64                         //
65                         // only clear the cached algorithms
66                         //
67                         getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
68                         getBroadphase()->destroyProxy(bp,m_dispatcher1);
69                 }
70         }
71
72
73 }
74
75
76
77
78
79
80
81
82
83
84 void    btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
85 {
86
87         //check that the object isn't already added
88                 btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
89
90                 m_collisionObjects.push_back(collisionObject);
91
92                 //calculate new AABB
93                 btTransform trans = collisionObject->getWorldTransform();
94
95                 btVector3       minAabb;
96                 btVector3       maxAabb;
97                 collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
98
99                 int type = collisionObject->getCollisionShape()->getShapeType();
100                 collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
101                         minAabb,
102                         maxAabb,
103                         type,
104                         collisionObject,
105                         collisionFilterGroup,
106                         collisionFilterMask,
107                         m_dispatcher1,0
108                         ))      ;
109
110
111
112
113
114 }
115
116 void    btCollisionWorld::updateAabbs()
117 {
118         BT_PROFILE("updateAabbs");
119
120         btTransform predictedTrans;
121         for ( int i=0;i<m_collisionObjects.size();i++)
122         {
123                 btCollisionObject* colObj = m_collisionObjects[i];
124
125                 //only update aabb of active objects
126                 if (colObj->isActive())
127                 {
128                         btPoint3 minAabb,maxAabb;
129                         colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
130                         btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
131
132                         //moving objects should be moderately sized, probably something wrong if not
133                         if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
134                         {
135                                 bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
136                         } else
137                         {
138                                 //something went wrong, investigate
139                                 //this assert is unwanted in 3D modelers (danger of loosing work)
140                                 colObj->setActivationState(DISABLE_SIMULATION);
141
142                                 static bool reportMe = true;
143                                 if (reportMe && m_debugDrawer)
144                                 {
145                                         reportMe = false;
146                                         m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
147                                         m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
148                                         m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
149                                         m_debugDrawer->reportErrorWarning("Thanks.\n");
150                                 }
151                         }
152                 }
153         }
154
155 }
156
157
158
159 void    btCollisionWorld::performDiscreteCollisionDetection()
160 {
161         BT_PROFILE("performDiscreteCollisionDetection");
162
163         btDispatcherInfo& dispatchInfo = getDispatchInfo();
164
165         updateAabbs();
166
167         {
168                 BT_PROFILE("calculateOverlappingPairs");
169                 m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
170         }
171
172
173         btDispatcher* dispatcher = getDispatcher();
174         {
175                 BT_PROFILE("dispatchAllCollisionPairs");
176                 if (dispatcher)
177                         dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
178         }
179
180 }
181
182
183
184 void    btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
185 {
186
187
188         //bool removeFromBroadphase = false;
189
190         {
191
192                 btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
193                 if (bp)
194                 {
195                         //
196                         // only clear the cached algorithms
197                         //
198                         getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
199                         getBroadphase()->destroyProxy(bp,m_dispatcher1);
200                         collisionObject->setBroadphaseHandle(0);
201                 }
202         }
203
204
205         //swapremove
206         m_collisionObjects.remove(collisionObject);
207
208 }
209
210
211
212 void    btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
213                                           btCollisionObject* collisionObject,
214                                           const btCollisionShape* collisionShape,
215                                           const btTransform& colObjWorldTransform,
216                                           RayResultCallback& resultCallback)
217 {
218         btSphereShape pointShape(btScalar(0.0));
219         pointShape.setMargin(0.f);
220         const btConvexShape* castShape = &pointShape;
221
222         if (collisionShape->isConvex())
223         {
224                 btConvexCast::CastResult castResult;
225                 castResult.m_fraction = resultCallback.m_closestHitFraction;
226
227                 btConvexShape* convexShape = (btConvexShape*) collisionShape;
228                 btVoronoiSimplexSolver  simplexSolver;
229 #define USE_SUBSIMPLEX_CONVEX_CAST 1
230 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
231                 btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
232 #else
233                 //btGjkConvexCast       convexCaster(castShape,convexShape,&simplexSolver);
234                 //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
235 #endif //#USE_SUBSIMPLEX_CONVEX_CAST
236
237                 if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
238                 {
239                         //add hit
240                         if (castResult.m_normal.length2() > btScalar(0.0001))
241                         {
242                                 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
243                                 {
244 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
245                                         //rotate normal into worldspace
246                                         castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
247 #endif //USE_SUBSIMPLEX_CONVEX_CAST
248
249                                         castResult.m_normal.normalize();
250                                         btCollisionWorld::LocalRayResult localRayResult
251                                                 (
252                                                         collisionObject,
253                                                         0,
254                                                         castResult.m_normal,
255                                                         castResult.m_fraction
256                                                 );
257
258                                         bool normalInWorldSpace = true;
259                                         resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
260
261                                 }
262                         }
263                 }
264         } else {
265                 if (collisionShape->isConcave())
266                 {
267                         if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
268                         {
269                                 ///optimized version for btBvhTriangleMeshShape
270                                 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
271                                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
272                                 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
273                                 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
274
275                                 //ConvexCast::CastResult
276                                 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
277                                 {
278                                         btCollisionWorld::RayResultCallback* m_resultCallback;
279                                         btCollisionObject*      m_collisionObject;
280                                         btTriangleMeshShape*    m_triangleMesh;
281
282                                         BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
283                                                 btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh):
284                                                 btTriangleRaycastCallback(from,to),
285                                                         m_resultCallback(resultCallback),
286                                                         m_collisionObject(collisionObject),
287                                                         m_triangleMesh(triangleMesh)
288                                                 {
289                                                 }
290
291
292                                         virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
293                                         {
294                                                 btCollisionWorld::LocalShapeInfo        shapeInfo;
295                                                 shapeInfo.m_shapePart = partId;
296                                                 shapeInfo.m_triangleIndex = triangleIndex;
297
298                                                 btCollisionWorld::LocalRayResult rayResult
299                                                 (m_collisionObject,
300                                                         &shapeInfo,
301                                                         hitNormalLocal,
302                                                         hitFraction);
303
304                                                 bool    normalInWorldSpace = false;
305                                                 return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
306                                         }
307
308                                 };
309
310                                 BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
311                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
312                                 triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
313                         } else
314                         {
315                                 btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape;
316
317                                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
318
319                                 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
320                                 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
321
322                                 //ConvexCast::CastResult
323
324                                 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
325                                 {
326                                         btCollisionWorld::RayResultCallback* m_resultCallback;
327                                         btCollisionObject*      m_collisionObject;
328                                         btTriangleMeshShape*    m_triangleMesh;
329
330                                         BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
331                                                 btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh):
332                                                 btTriangleRaycastCallback(from,to),
333                                                         m_resultCallback(resultCallback),
334                                                         m_collisionObject(collisionObject),
335                                                         m_triangleMesh(triangleMesh)
336                                                 {
337                                                 }
338
339
340                                         virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
341                                         {
342                                                 btCollisionWorld::LocalShapeInfo        shapeInfo;
343                                                 shapeInfo.m_shapePart = partId;
344                                                 shapeInfo.m_triangleIndex = triangleIndex;
345
346                                                 btCollisionWorld::LocalRayResult rayResult
347                                                 (m_collisionObject,
348                                                         &shapeInfo,
349                                                         hitNormalLocal,
350                                                         hitFraction);
351
352                                                 bool    normalInWorldSpace = false;
353                                                 return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
354
355
356                                         }
357
358                                 };
359
360
361                                 BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
362                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
363
364                                 btVector3 rayAabbMinLocal = rayFromLocal;
365                                 rayAabbMinLocal.setMin(rayToLocal);
366                                 btVector3 rayAabbMaxLocal = rayFromLocal;
367                                 rayAabbMaxLocal.setMax(rayToLocal);
368
369                                 triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
370                         }
371                 } else {
372                         //todo: use AABB tree or other BVH acceleration structure!
373                         if (collisionShape->isCompound())
374                         {
375                                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
376                                 int i=0;
377                                 for (i=0;i<compoundShape->getNumChildShapes();i++)
378                                 {
379                                         btTransform childTrans = compoundShape->getChildTransform(i);
380                                         const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
381                                         btTransform childWorldTrans = colObjWorldTransform * childTrans;
382                                         // replace collision shape so that callback can determine the triangle
383                                         btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
384                                         collisionObject->setCollisionShape((btCollisionShape*)childCollisionShape);
385                                         rayTestSingle(rayFromTrans,rayToTrans,
386                                                 collisionObject,
387                                                 childCollisionShape,
388                                                 childWorldTrans,
389                                                 resultCallback);
390                                         // restore
391                                         collisionObject->setCollisionShape(saveCollisionShape);
392                                 }
393                         }
394                 }
395         }
396 }
397
398 void    btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
399                                           btCollisionObject* collisionObject,
400                                           const btCollisionShape* collisionShape,
401                                           const btTransform& colObjWorldTransform,
402                                           ConvexResultCallback& resultCallback, btScalar allowedPenetration)
403 {
404         if (collisionShape->isConvex())
405         {
406                 btConvexCast::CastResult castResult;
407                 castResult.m_allowedPenetration = allowedPenetration;
408                 castResult.m_fraction = btScalar(1.);//??
409
410                 btConvexShape* convexShape = (btConvexShape*) collisionShape;
411                 btVoronoiSimplexSolver  simplexSolver;
412                 btGjkEpaPenetrationDepthSolver  gjkEpaPenetrationSolver;
413                 
414                 btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
415                 //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
416                 //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
417
418                 btConvexCast* castPtr = &convexCaster1;
419         
420         
421                 
422                 if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
423                 {
424                         //add hit
425                         if (castResult.m_normal.length2() > btScalar(0.0001))
426                         {
427                                 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
428                                 {
429                                         castResult.m_normal.normalize();
430                                         btCollisionWorld::LocalConvexResult localConvexResult
431                                                                 (
432                                                                         collisionObject,
433                                                                         0,
434                                                                         castResult.m_normal,
435                                                                         castResult.m_hitPoint,
436                                                                         castResult.m_fraction
437                                                                 );
438
439                                         bool normalInWorldSpace = true;
440                                         resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
441
442                                 }
443                         }
444                 }
445         } else {
446                 if (collisionShape->isConcave())
447                 {
448                         if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
449                         {
450                                 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
451                                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
452                                 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
453                                 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
454                                 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
455                                 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
456
457                                 //ConvexCast::CastResult
458                                 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
459                                 {
460                                         btCollisionWorld::ConvexResultCallback* m_resultCallback;
461                                         btCollisionObject*      m_collisionObject;
462                                         btTriangleMeshShape*    m_triangleMesh;
463
464                                         BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
465                                                 btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
466                                                 btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
467                                                         m_resultCallback(resultCallback),
468                                                         m_collisionObject(collisionObject),
469                                                         m_triangleMesh(triangleMesh)
470                                                 {
471                                                 }
472
473
474                                         virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
475                                         {
476                                                 btCollisionWorld::LocalShapeInfo        shapeInfo;
477                                                 shapeInfo.m_shapePart = partId;
478                                                 shapeInfo.m_triangleIndex = triangleIndex;
479                                                 if (hitFraction <= m_resultCallback->m_closestHitFraction)
480                                                 {
481
482                                                         btCollisionWorld::LocalConvexResult convexResult
483                                                         (m_collisionObject,
484                                                                 &shapeInfo,
485                                                                 hitNormalLocal,
486                                                                 hitPointLocal,
487                                                                 hitFraction);
488
489                                                         bool    normalInWorldSpace = true;
490
491
492                                                         return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
493                                                 }
494                                                 return hitFraction;
495                                         }
496
497                                 };
498
499                                 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
500                                 tccb.m_hitFraction = resultCallback.m_closestHitFraction;
501                                 btVector3 boxMinLocal, boxMaxLocal;
502                                 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
503                                 triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
504                         } else
505                         {
506                                 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
507                                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
508                                 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
509                                 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
510                                 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
511                                 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
512
513                                 //ConvexCast::CastResult
514                                 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
515                                 {
516                                         btCollisionWorld::ConvexResultCallback* m_resultCallback;
517                                         btCollisionObject*      m_collisionObject;
518                                         btTriangleMeshShape*    m_triangleMesh;
519
520                                         BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
521                                                 btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
522                                                 btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
523                                                         m_resultCallback(resultCallback),
524                                                         m_collisionObject(collisionObject),
525                                                         m_triangleMesh(triangleMesh)
526                                                 {
527                                                 }
528
529
530                                         virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
531                                         {
532                                                 btCollisionWorld::LocalShapeInfo        shapeInfo;
533                                                 shapeInfo.m_shapePart = partId;
534                                                 shapeInfo.m_triangleIndex = triangleIndex;
535                                                 if (hitFraction <= m_resultCallback->m_closestHitFraction)
536                                                 {
537
538                                                         btCollisionWorld::LocalConvexResult convexResult
539                                                         (m_collisionObject,
540                                                                 &shapeInfo,
541                                                                 hitNormalLocal,
542                                                                 hitPointLocal,
543                                                                 hitFraction);
544
545                                                         bool    normalInWorldSpace = false;
546
547                                                         return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
548                                                 }
549                                                 return hitFraction;
550                                         }
551
552                                 };
553
554                                 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
555                                 tccb.m_hitFraction = resultCallback.m_closestHitFraction;
556                                 btVector3 boxMinLocal, boxMaxLocal;
557                                 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
558
559                                 btVector3 rayAabbMinLocal = convexFromLocal;
560                                 rayAabbMinLocal.setMin(convexToLocal);
561                                 btVector3 rayAabbMaxLocal = convexFromLocal;
562                                 rayAabbMaxLocal.setMax(convexToLocal);
563                                 rayAabbMinLocal += boxMinLocal;
564                                 rayAabbMaxLocal += boxMaxLocal;
565                                 triangleMesh->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
566                         }
567                 } else {
568                         //todo: use AABB tree or other BVH acceleration structure!
569                         if (collisionShape->isCompound())
570                         {
571                                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
572                                 int i=0;
573                                 for (i=0;i<compoundShape->getNumChildShapes();i++)
574                                 {
575                                         btTransform childTrans = compoundShape->getChildTransform(i);
576                                         const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
577                                         btTransform childWorldTrans = colObjWorldTransform * childTrans;
578                                         // replace collision shape so that callback can determine the triangle
579                                         btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
580                                         collisionObject->setCollisionShape((btCollisionShape*)childCollisionShape);
581                                         objectQuerySingle(castShape, convexFromTrans,convexToTrans,
582                                                 collisionObject,
583                                                 childCollisionShape,
584                                                 childWorldTrans,
585                                                 resultCallback, allowedPenetration);
586                                         // restore
587                                         collisionObject->setCollisionShape(saveCollisionShape);
588                                 }
589                         }
590                 }
591         }
592 }
593
594 void    btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
595 {
596
597
598         btTransform     rayFromTrans,rayToTrans;
599         rayFromTrans.setIdentity();
600         rayFromTrans.setOrigin(rayFromWorld);
601         rayToTrans.setIdentity();
602
603         rayToTrans.setOrigin(rayToWorld);
604
605         /// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
606
607         int i;
608         for (i=0;i<m_collisionObjects.size();i++)
609         {
610                 ///terminate further ray tests, once the closestHitFraction reached zero
611                 if (resultCallback.m_closestHitFraction == btScalar(0.f))
612                         break;
613
614                 btCollisionObject*      collisionObject= m_collisionObjects[i];
615                 //only perform raycast if filterMask matches
616                 if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
617                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
618                         btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
619                         collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
620
621                         btScalar hitLambda = resultCallback.m_closestHitFraction;
622                         btVector3 hitNormal;
623                         if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
624                         {
625                                 rayTestSingle(rayFromTrans,rayToTrans,
626                                         collisionObject,
627                                                 collisionObject->getCollisionShape(),
628                                                 collisionObject->getWorldTransform(),
629                                                 resultCallback);
630                         }
631                 }
632
633         }
634
635 }
636
637 void    btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback) const
638 {
639         btTransform     convexFromTrans,convexToTrans;
640         convexFromTrans = convexFromWorld;
641         convexToTrans = convexToWorld;
642         btVector3 castShapeAabbMin, castShapeAabbMax;
643         /* Compute AABB that encompasses angular movement */
644         {
645                 btVector3 linVel, angVel;
646                 btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
647                 btTransform R;
648                 R.setIdentity ();
649                 R.setRotation (convexFromTrans.getRotation());
650                 castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
651         }
652
653         /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
654         // do a ray-shape query using convexCaster (CCD)
655         int i;
656         for (i=0;i<m_collisionObjects.size();i++)
657         {
658                 btCollisionObject*      collisionObject= m_collisionObjects[i];
659                 //only perform raycast if filterMask matches
660                 if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
661                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
662                         btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
663                         collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
664                         AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
665                         btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
666                         btVector3 hitNormal;
667                         if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
668                         {
669                                 objectQuerySingle(castShape, convexFromTrans,convexToTrans,
670                                         collisionObject,
671                                                 collisionObject->getCollisionShape(),
672                                                 collisionObject->getWorldTransform(),
673                                                 resultCallback,
674                                                 getDispatchInfo().m_allowedCcdPenetration);
675                         }
676                 }
677         }
678
679 }