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