BGE: ray casting works on soft body, the hit polygon is also returned. The modificati...
[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
30 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
31 #include "LinearMath/btAabbUtil2.h"
32 #include "LinearMath/btQuickprof.h"
33 #include "LinearMath/btStackAlloc.h"
34 #include "BulletSoftBody/btSoftBody.h"
35
36 //#define USE_BRUTEFORCE_RAYBROADPHASE 1
37 //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation'  or 'updateAabbs' before using a rayTest
38 //#define RECALCULATE_AABB_RAYCAST 1
39
40 //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
41 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
42 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
43 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
44
45
46 btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
47 :m_dispatcher1(dispatcher),
48 m_broadphasePairCache(pairCache),
49 m_debugDrawer(0)
50 {
51         m_stackAlloc = collisionConfiguration->getStackAllocator();
52         m_dispatchInfo.m_stackAllocator = m_stackAlloc;
53 }
54
55
56 btCollisionWorld::~btCollisionWorld()
57 {
58
59         //clean up remaining objects
60         int i;
61         for (i=0;i<m_collisionObjects.size();i++)
62         {
63                 btCollisionObject* collisionObject= m_collisionObjects[i];
64
65                 btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
66                 if (bp)
67                 {
68                         //
69                         // only clear the cached algorithms
70                         //
71                         getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
72                         getBroadphase()->destroyProxy(bp,m_dispatcher1);
73                         collisionObject->setBroadphaseHandle(0);
74                 }
75         }
76
77
78 }
79
80
81
82
83
84
85
86
87
88
89 void    btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
90 {
91
92         //check that the object isn't already added
93                 btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
94
95                 m_collisionObjects.push_back(collisionObject);
96
97                 //calculate new AABB
98                 btTransform trans = collisionObject->getWorldTransform();
99
100                 btVector3       minAabb;
101                 btVector3       maxAabb;
102                 collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
103
104                 int type = collisionObject->getCollisionShape()->getShapeType();
105                 collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
106                         minAabb,
107                         maxAabb,
108                         type,
109                         collisionObject,
110                         collisionFilterGroup,
111                         collisionFilterMask,
112                         m_dispatcher1,0
113                         ))      ;
114
115
116
117
118
119 }
120
121
122
123 void    btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
124 {
125         btVector3 minAabb,maxAabb;
126         colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
127         //need to increase the aabb for contact thresholds
128         btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
129         minAabb -= contactThreshold;
130         maxAabb += contactThreshold;
131
132         btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
133
134         //moving objects should be moderately sized, probably something wrong if not
135         if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
136         {
137                 bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
138         } else
139         {
140                 //something went wrong, investigate
141                 //this assert is unwanted in 3D modelers (danger of loosing work)
142                 colObj->setActivationState(DISABLE_SIMULATION);
143
144                 static bool reportMe = true;
145                 if (reportMe && m_debugDrawer)
146                 {
147                         reportMe = false;
148                         m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
149                         m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
150                         m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
151                         m_debugDrawer->reportErrorWarning("Thanks.\n");
152                 }
153         }
154 }
155
156 void    btCollisionWorld::updateAabbs()
157 {
158         BT_PROFILE("updateAabbs");
159
160         btTransform predictedTrans;
161         for ( int i=0;i<m_collisionObjects.size();i++)
162         {
163                 btCollisionObject* colObj = m_collisionObjects[i];
164
165                 //only update aabb of active objects
166                 if (colObj->isActive())
167                 {
168                         updateSingleAabb(colObj);
169                 }
170         }
171 }
172
173
174
175 void    btCollisionWorld::performDiscreteCollisionDetection()
176 {
177         BT_PROFILE("performDiscreteCollisionDetection");
178
179         btDispatcherInfo& dispatchInfo = getDispatchInfo();
180
181         updateAabbs();
182
183         {
184                 BT_PROFILE("calculateOverlappingPairs");
185                 m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
186         }
187
188
189         btDispatcher* dispatcher = getDispatcher();
190         {
191                 BT_PROFILE("dispatchAllCollisionPairs");
192                 if (dispatcher)
193                         dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
194         }
195
196 }
197
198
199
200 void    btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
201 {
202
203
204         //bool removeFromBroadphase = false;
205
206         {
207
208                 btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
209                 if (bp)
210                 {
211                         //
212                         // only clear the cached algorithms
213                         //
214                         getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
215                         getBroadphase()->destroyProxy(bp,m_dispatcher1);
216                         collisionObject->setBroadphaseHandle(0);
217                 }
218         }
219
220
221         //swapremove
222         m_collisionObjects.remove(collisionObject);
223
224 }
225
226
227
228 void    btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
229                                           btCollisionObject* collisionObject,
230                                           const btCollisionShape* collisionShape,
231                                           const btTransform& colObjWorldTransform,
232                                           RayResultCallback& resultCallback)
233 {
234         btSphereShape pointShape(btScalar(0.0));
235         pointShape.setMargin(0.f);
236         const btConvexShape* castShape = &pointShape;
237
238         if (collisionShape->isConvex())
239         {
240 //              BT_PROFILE("rayTestConvex");
241                 btConvexCast::CastResult castResult;
242                 castResult.m_fraction = resultCallback.m_closestHitFraction;
243
244                 btConvexShape* convexShape = (btConvexShape*) collisionShape;
245                 btVoronoiSimplexSolver  simplexSolver;
246 #define USE_SUBSIMPLEX_CONVEX_CAST 1
247 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
248                 btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
249 #else
250                 //btGjkConvexCast       convexCaster(castShape,convexShape,&simplexSolver);
251                 //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
252 #endif //#USE_SUBSIMPLEX_CONVEX_CAST
253
254                 if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
255                 {
256                         //add hit
257                         if (castResult.m_normal.length2() > btScalar(0.0001))
258                         {
259                                 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
260                                 {
261 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
262                                         //rotate normal into worldspace
263                                         castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
264 #endif //USE_SUBSIMPLEX_CONVEX_CAST
265
266                                         castResult.m_normal.normalize();
267                                         btCollisionWorld::LocalRayResult localRayResult
268                                                 (
269                                                         collisionObject,
270                                                         0,
271                                                         castResult.m_normal,
272                                                         castResult.m_fraction
273                                                 );
274
275                                         bool normalInWorldSpace = true;
276                                         resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
277
278                                 }
279                         }
280                 }
281         } else {
282                 if (collisionShape->isConcave())
283                 {
284 //                      BT_PROFILE("rayTestConcave");
285                         if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
286                         {
287                                 ///optimized version for btBvhTriangleMeshShape
288                                 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
289                                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
290                                 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
291                                 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
292
293                                 //ConvexCast::CastResult
294                                 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
295                                 {
296                                         btCollisionWorld::RayResultCallback* m_resultCallback;
297                                         btCollisionObject*      m_collisionObject;
298                                         btTriangleMeshShape*    m_triangleMesh;
299
300                                         BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
301                                                 btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh):
302                   //@BP Mod
303                                                 btTriangleRaycastCallback(from,to, resultCallback->m_flags),
304                                                         m_resultCallback(resultCallback),
305                                                         m_collisionObject(collisionObject),
306                                                         m_triangleMesh(triangleMesh)
307                                                 {
308                                                 }
309
310
311                                         virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
312                                         {
313                                                 btCollisionWorld::LocalShapeInfo        shapeInfo;
314                                                 shapeInfo.m_shapePart = partId;
315                                                 shapeInfo.m_triangleIndex = triangleIndex;
316
317                                                 btCollisionWorld::LocalRayResult rayResult
318                                                 (m_collisionObject,
319                                                         &shapeInfo,
320                                                         hitNormalLocal,
321                                                         hitFraction);
322
323                                                 bool    normalInWorldSpace = false;
324                                                 return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
325                                         }
326
327                                 };
328
329                                 BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
330                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
331                                 triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
332                         } else
333                         {
334                                 //generic (slower) case
335                                 btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
336
337                                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
338
339                                 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
340                                 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
341
342                                 //ConvexCast::CastResult
343
344                                 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
345                                 {
346                                         btCollisionWorld::RayResultCallback* m_resultCallback;
347                                         btCollisionObject*      m_collisionObject;
348                                         btConcaveShape* m_triangleMesh;
349
350                                         BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
351                                                 btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh):
352                   //@BP Mod
353                   btTriangleRaycastCallback(from,to, resultCallback->m_flags),
354                                                         m_resultCallback(resultCallback),
355                                                         m_collisionObject(collisionObject),
356                                                         m_triangleMesh(triangleMesh)
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                                                 btCollisionWorld::LocalRayResult rayResult
368                                                 (m_collisionObject,
369                                                         &shapeInfo,
370                                                         hitNormalLocal,
371                                                         hitFraction);
372
373                                                 bool    normalInWorldSpace = false;
374                                                 return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
375
376
377                                         }
378
379                                 };
380
381
382                                 BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape);
383                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
384
385                                 btVector3 rayAabbMinLocal = rayFromLocal;
386                                 rayAabbMinLocal.setMin(rayToLocal);
387                                 btVector3 rayAabbMaxLocal = rayFromLocal;
388                                 rayAabbMaxLocal.setMax(rayToLocal);
389
390                                 concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
391                         }
392                 } else {
393 //                      BT_PROFILE("rayTestCompound");
394                         ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
395                         if (collisionShape->isCompound())
396                         {
397                                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
398                                 int i=0;
399                                 for (i=0;i<compoundShape->getNumChildShapes();i++)
400                                 {
401                                         btTransform childTrans = compoundShape->getChildTransform(i);
402                                         const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
403                                         btTransform childWorldTrans = colObjWorldTransform * childTrans;
404                                         // replace collision shape so that callback can determine the triangle
405                                         btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
406                                         collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
407                                         rayTestSingle(rayFromTrans,rayToTrans,
408                                                 collisionObject,
409                                                 childCollisionShape,
410                                                 childWorldTrans,
411                                                 resultCallback);
412                                         // restore
413                                         collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
414                                 }
415                         } else {
416                                 if (collisionShape->isSoftBody()) {
417                                         btSoftBody* softBody = static_cast<btSoftBody*>(collisionObject);
418                                         btSoftBody::sRayCast softResult;
419                                         if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) 
420                                         {
421                                                 btCollisionWorld::LocalShapeInfo shapeInfo;
422                                                 shapeInfo.m_shapePart = 0;
423                                                 shapeInfo.m_triangleIndex = softResult.index;
424                                                 // get the normal
425                                                 btVector3 normal = softBody->m_faces[softResult.index].m_normal;
426                                                 btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
427                                                 if (normal.dot(rayDir) > 0) {
428                                                         // normal always point toward origin of the ray
429                                                         normal = -normal;
430                                                 }
431                                                 btCollisionWorld::LocalRayResult rayResult
432                                                         (collisionObject,
433                                                          &shapeInfo,
434                                                          normal,
435                                                          softResult.fraction);
436                                                 bool    normalInWorldSpace = true;
437                                                 resultCallback.addSingleResult(rayResult,normalInWorldSpace);
438                                         }
439                                 }
440                         }
441                 }
442         }
443 }
444
445 void    btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
446                                           btCollisionObject* collisionObject,
447                                           const btCollisionShape* collisionShape,
448                                           const btTransform& colObjWorldTransform,
449                                           ConvexResultCallback& resultCallback, btScalar allowedPenetration)
450 {
451         if (collisionShape->isConvex())
452         {
453                 //BT_PROFILE("convexSweepConvex");
454                 btConvexCast::CastResult castResult;
455                 castResult.m_allowedPenetration = allowedPenetration;
456                 castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
457
458                 btConvexShape* convexShape = (btConvexShape*) collisionShape;
459                 btVoronoiSimplexSolver  simplexSolver;
460                 btGjkEpaPenetrationDepthSolver  gjkEpaPenetrationSolver;
461                 
462                 btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
463                 //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
464                 //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
465
466                 btConvexCast* castPtr = &convexCaster1;
467         
468         
469                 
470                 if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
471                 {
472                         //add hit
473                         if (castResult.m_normal.length2() > btScalar(0.0001))
474                         {
475                                 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
476                                 {
477                                         castResult.m_normal.normalize();
478                                         btCollisionWorld::LocalConvexResult localConvexResult
479                                                                 (
480                                                                         collisionObject,
481                                                                         0,
482                                                                         castResult.m_normal,
483                                                                         castResult.m_hitPoint,
484                                                                         castResult.m_fraction
485                                                                 );
486
487                                         bool normalInWorldSpace = true;
488                                         resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
489
490                                 }
491                         }
492                 }
493         } else {
494                 if (collisionShape->isConcave())
495                 {
496                         if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
497                         {
498                                 //BT_PROFILE("convexSweepbtBvhTriangleMesh");
499                                 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
500                                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
501                                 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
502                                 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
503                                 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
504                                 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
505
506                                 //ConvexCast::CastResult
507                                 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
508                                 {
509                                         btCollisionWorld::ConvexResultCallback* m_resultCallback;
510                                         btCollisionObject*      m_collisionObject;
511                                         btTriangleMeshShape*    m_triangleMesh;
512
513                                         BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
514                                                 btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
515                                                 btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
516                                                         m_resultCallback(resultCallback),
517                                                         m_collisionObject(collisionObject),
518                                                         m_triangleMesh(triangleMesh)
519                                                 {
520                                                 }
521
522
523                                         virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
524                                         {
525                                                 btCollisionWorld::LocalShapeInfo        shapeInfo;
526                                                 shapeInfo.m_shapePart = partId;
527                                                 shapeInfo.m_triangleIndex = triangleIndex;
528                                                 if (hitFraction <= m_resultCallback->m_closestHitFraction)
529                                                 {
530
531                                                         btCollisionWorld::LocalConvexResult convexResult
532                                                         (m_collisionObject,
533                                                                 &shapeInfo,
534                                                                 hitNormalLocal,
535                                                                 hitPointLocal,
536                                                                 hitFraction);
537
538                                                         bool    normalInWorldSpace = true;
539
540
541                                                         return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
542                                                 }
543                                                 return hitFraction;
544                                         }
545
546                                 };
547
548                                 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
549                                 tccb.m_hitFraction = resultCallback.m_closestHitFraction;
550                                 btVector3 boxMinLocal, boxMaxLocal;
551                                 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
552                                 triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
553                         } else
554                         {
555                                 //BT_PROFILE("convexSweepConcave");
556                                 btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
557                                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
558                                 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
559                                 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
560                                 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
561                                 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
562
563                                 //ConvexCast::CastResult
564                                 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
565                                 {
566                                         btCollisionWorld::ConvexResultCallback* m_resultCallback;
567                                         btCollisionObject*      m_collisionObject;
568                                         btConcaveShape* m_triangleMesh;
569
570                                         BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
571                                                 btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*      triangleMesh, const btTransform& triangleToWorld):
572                                                 btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
573                                                         m_resultCallback(resultCallback),
574                                                         m_collisionObject(collisionObject),
575                                                         m_triangleMesh(triangleMesh)
576                                                 {
577                                                 }
578
579
580                                         virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
581                                         {
582                                                 btCollisionWorld::LocalShapeInfo        shapeInfo;
583                                                 shapeInfo.m_shapePart = partId;
584                                                 shapeInfo.m_triangleIndex = triangleIndex;
585                                                 if (hitFraction <= m_resultCallback->m_closestHitFraction)
586                                                 {
587
588                                                         btCollisionWorld::LocalConvexResult convexResult
589                                                         (m_collisionObject,
590                                                                 &shapeInfo,
591                                                                 hitNormalLocal,
592                                                                 hitPointLocal,
593                                                                 hitFraction);
594
595                                                         bool    normalInWorldSpace = false;
596
597                                                         return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
598                                                 }
599                                                 return hitFraction;
600                                         }
601
602                                 };
603
604                                 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
605                                 tccb.m_hitFraction = resultCallback.m_closestHitFraction;
606                                 btVector3 boxMinLocal, boxMaxLocal;
607                                 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
608
609                                 btVector3 rayAabbMinLocal = convexFromLocal;
610                                 rayAabbMinLocal.setMin(convexToLocal);
611                                 btVector3 rayAabbMaxLocal = convexFromLocal;
612                                 rayAabbMaxLocal.setMax(convexToLocal);
613                                 rayAabbMinLocal += boxMinLocal;
614                                 rayAabbMaxLocal += boxMaxLocal;
615                                 concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
616                         }
617                 } else {
618                         ///@todo : use AABB tree or other BVH acceleration structure!
619                         if (collisionShape->isCompound())
620                         {
621                                 BT_PROFILE("convexSweepCompound");
622                                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
623                                 int i=0;
624                                 for (i=0;i<compoundShape->getNumChildShapes();i++)
625                                 {
626                                         btTransform childTrans = compoundShape->getChildTransform(i);
627                                         const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
628                                         btTransform childWorldTrans = colObjWorldTransform * childTrans;
629                                         // replace collision shape so that callback can determine the triangle
630                                         btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
631                                         collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
632                                         objectQuerySingle(castShape, convexFromTrans,convexToTrans,
633                                                 collisionObject,
634                                                 childCollisionShape,
635                                                 childWorldTrans,
636                                                 resultCallback, allowedPenetration);
637                                         // restore
638                                         collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
639                                 }
640                         }
641                 }
642         }
643 }
644
645
646 struct btSingleRayCallback : public btBroadphaseRayCallback
647 {
648
649         btVector3       m_rayFromWorld;
650         btVector3       m_rayToWorld;
651         btTransform     m_rayFromTrans;
652         btTransform     m_rayToTrans;
653         btVector3       m_hitNormal;
654
655         const btCollisionWorld* m_world;
656         btCollisionWorld::RayResultCallback&    m_resultCallback;
657
658         btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
659         :m_rayFromWorld(rayFromWorld),
660         m_rayToWorld(rayToWorld),
661         m_world(world),
662         m_resultCallback(resultCallback)
663         {
664                 m_rayFromTrans.setIdentity();
665                 m_rayFromTrans.setOrigin(m_rayFromWorld);
666                 m_rayToTrans.setIdentity();
667                 m_rayToTrans.setOrigin(m_rayToWorld);
668
669                 btVector3 rayDir = (rayToWorld-rayFromWorld);
670
671                 rayDir.normalize ();
672                 ///what about division by zero? --> just set rayDirection[i] to INF/1e30
673                 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
674                 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
675                 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
676                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
677                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
678                 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
679
680                 m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
681
682         }
683
684         
685
686         virtual bool    process(const btBroadphaseProxy* proxy)
687         {
688                 ///terminate further ray tests, once the closestHitFraction reached zero
689                 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
690                         return false;
691
692                 btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
693
694                 //only perform raycast if filterMask matches
695                 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
696                 {
697                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
698                         //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
699 #if 0
700 #ifdef RECALCULATE_AABB
701                         btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
702                         collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
703 #else
704                         //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
705                         const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
706                         const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
707 #endif
708 #endif
709                         //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
710                         //culling already done by broadphase
711                         //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
712                         {
713                                 m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
714                                         collisionObject,
715                                                 collisionObject->getCollisionShape(),
716                                                 collisionObject->getWorldTransform(),
717                                                 m_resultCallback);
718                         }
719                 }
720                 return true;
721         }
722 };
723
724 void    btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
725 {
726         BT_PROFILE("rayTest");
727         /// use the broadphase to accelerate the search for objects, based on their aabb
728         /// and for each object with ray-aabb overlap, perform an exact ray test
729         btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
730
731 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
732         m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
733 #else
734         for (int i=0;i<this->getNumCollisionObjects();i++)
735         {
736                 rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
737         }       
738 #endif //USE_BRUTEFORCE_RAYBROADPHASE
739
740 }
741
742
743 struct btSingleSweepCallback : public btBroadphaseRayCallback
744 {
745
746         btTransform     m_convexFromTrans;
747         btTransform     m_convexToTrans;
748         btVector3       m_hitNormal;
749         const btCollisionWorld* m_world;
750         btCollisionWorld::ConvexResultCallback& m_resultCallback;
751         btScalar        m_allowedCcdPenetration;
752         const btConvexShape* m_castShape;
753
754
755         btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
756                 :m_convexFromTrans(convexFromTrans),
757                 m_convexToTrans(convexToTrans),
758                 m_world(world),
759                 m_resultCallback(resultCallback),
760                 m_allowedCcdPenetration(allowedPenetration),
761                 m_castShape(castShape)
762         {
763                 btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
764                 btVector3 rayDir = unnormalizedRayDir.normalized();
765                 ///what about division by zero? --> just set rayDirection[i] to INF/1e30
766                 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
767                 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
768                 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
769                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
770                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
771                 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
772
773                 m_lambda_max = rayDir.dot(unnormalizedRayDir);
774
775         }
776
777         virtual bool    process(const btBroadphaseProxy* proxy)
778         {
779                 ///terminate further convex sweep tests, once the closestHitFraction reached zero
780                 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
781                         return false;
782
783                 btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
784
785                 //only perform raycast if filterMask matches
786                 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
787                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
788                         m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
789                                         collisionObject,
790                                                 collisionObject->getCollisionShape(),
791                                                 collisionObject->getWorldTransform(),
792                                                 m_resultCallback,
793                                                 m_allowedCcdPenetration);
794                 }
795                 
796                 return true;
797         }
798 };
799
800
801
802 void    btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
803 {
804
805         BT_PROFILE("convexSweepTest");
806         /// use the broadphase to accelerate the search for objects, based on their aabb
807         /// and for each object with ray-aabb overlap, perform an exact ray test
808         /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
809
810         
811
812         btTransform     convexFromTrans,convexToTrans;
813         convexFromTrans = convexFromWorld;
814         convexToTrans = convexToWorld;
815         btVector3 castShapeAabbMin, castShapeAabbMax;
816         /* Compute AABB that encompasses angular movement */
817         {
818                 btVector3 linVel, angVel;
819                 btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
820                 btVector3 zeroLinVel;
821                 zeroLinVel.setValue(0,0,0);
822                 btTransform R;
823                 R.setIdentity ();
824                 R.setRotation (convexFromTrans.getRotation());
825                 castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
826         }
827
828 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
829
830         btSingleSweepCallback   convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
831
832         m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
833
834 #else
835         /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
836         // do a ray-shape query using convexCaster (CCD)
837         int i;
838         for (i=0;i<m_collisionObjects.size();i++)
839         {
840                 btCollisionObject*      collisionObject= m_collisionObjects[i];
841                 //only perform raycast if filterMask matches
842                 if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
843                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
844                         btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
845                         collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
846                         AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
847                         btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
848                         btVector3 hitNormal;
849                         if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
850                         {
851                                 objectQuerySingle(castShape, convexFromTrans,convexToTrans,
852                                         collisionObject,
853                                                 collisionObject->getCollisionShape(),
854                                                 collisionObject->getWorldTransform(),
855                                                 resultCallback,
856                                                 allowedCcdPenetration);
857                         }
858                 }
859         }
860 #endif //USE_BRUTEFORCE_RAYBROADPHASE
861 }