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