Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @ 1963

Last change on this file since 1963 was 1963, checked in by rgrieder, 15 years ago

Added Bullet physics engine.

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