Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @ 8351

Last change on this file since 8351 was 8351, checked in by rgrieder, 13 years ago

Merged kicklib2 branch back to trunk (includes former branches ois_update, mac_osx and kicklib).

Notes for updating

Linux:
You don't need an extra package for CEGUILua and Tolua, it's already shipped with CEGUI.
However you do need to make sure that the OgreRenderer is installed too with CEGUI 0.7 (may be a separate package).
Also, Orxonox now recognises if you install the CgProgramManager (a separate package available on newer Ubuntu on Debian systems).

Windows:
Download the new dependency packages versioned 6.0 and use these. If you have problems with that or if you don't like the in game console problem mentioned below, you can download the new 4.3 version of the packages (only available for Visual Studio 2005/2008).

Key new features:

  • *Support for Mac OS X*
  • Visual Studio 2010 support
  • Bullet library update to 2.77
  • OIS library update to 1.3
  • Support for CEGUI 0.7 —> Support for Arch Linux and even SuSE
  • Improved install target
  • Compiles now with GCC 4.6
  • Ogre Cg Shader plugin activated for Linux if available
  • And of course lots of bug fixes

There are also some regressions:

  • No support for CEGUI 0.5, Ogre 1.4 and boost 1.35 - 1.39 any more
  • In game console is not working in main menu for CEGUI 0.7
  • Tolua (just the C lib, not the application) and CEGUILua libraries are no longer in our repository. —> You will need to get these as well when compiling Orxonox
  • And of course lots of new bugs we don't yet know about
  • Property svn:eol-style set to native
File size: 50.6 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#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
30#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
31#include "LinearMath/btAabbUtil2.h"
32#include "LinearMath/btQuickprof.h"
33#include "LinearMath/btStackAlloc.h"
34#include "LinearMath/btSerializer.h"
35
36//#define USE_BRUTEFORCE_RAYBROADPHASE 1
37//RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation'  or 'updateAabbs' before using a rayTest
38//#define RECALCULATE_AABB_RAYCAST 1
39
40//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
41#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
42#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
43#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
44
45
46///for debug drawing
47
48//for debug rendering
49#include "BulletCollision/CollisionShapes/btBoxShape.h"
50#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
51#include "BulletCollision/CollisionShapes/btCompoundShape.h"
52#include "BulletCollision/CollisionShapes/btConeShape.h"
53#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
54#include "BulletCollision/CollisionShapes/btCylinderShape.h"
55#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
56#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
57#include "BulletCollision/CollisionShapes/btSphereShape.h"
58#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
59#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
60#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
61
62
63
64btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
65:m_dispatcher1(dispatcher),
66m_broadphasePairCache(pairCache),
67m_debugDrawer(0),
68m_forceUpdateAllAabbs(true)
69{
70        m_stackAlloc = collisionConfiguration->getStackAllocator();
71        m_dispatchInfo.m_stackAllocator = m_stackAlloc;
72}
73
74
75btCollisionWorld::~btCollisionWorld()
76{
77
78        //clean up remaining objects
79        int i;
80        for (i=0;i<m_collisionObjects.size();i++)
81        {
82                btCollisionObject* collisionObject= m_collisionObjects[i];
83
84                btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
85                if (bp)
86                {
87                        //
88                        // only clear the cached algorithms
89                        //
90                        getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
91                        getBroadphase()->destroyProxy(bp,m_dispatcher1);
92                        collisionObject->setBroadphaseHandle(0);
93                }
94        }
95
96
97}
98
99
100
101
102
103
104
105
106
107
108void    btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
109{
110
111        btAssert(collisionObject);
112
113        //check that the object isn't already added
114        btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
115
116        m_collisionObjects.push_back(collisionObject);
117
118        //calculate new AABB
119        btTransform trans = collisionObject->getWorldTransform();
120
121        btVector3       minAabb;
122        btVector3       maxAabb;
123        collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
124
125        int type = collisionObject->getCollisionShape()->getShapeType();
126        collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
127                minAabb,
128                maxAabb,
129                type,
130                collisionObject,
131                collisionFilterGroup,
132                collisionFilterMask,
133                m_dispatcher1,0
134                ))      ;
135
136
137
138
139
140}
141
142
143
144void    btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
145{
146        btVector3 minAabb,maxAabb;
147        colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
148        //need to increase the aabb for contact thresholds
149        btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
150        minAabb -= contactThreshold;
151        maxAabb += contactThreshold;
152
153        btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
154
155        //moving objects should be moderately sized, probably something wrong if not
156        if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
157        {
158                bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
159        } else
160        {
161                //something went wrong, investigate
162                //this assert is unwanted in 3D modelers (danger of loosing work)
163                colObj->setActivationState(DISABLE_SIMULATION);
164
165                static bool reportMe = true;
166                if (reportMe && m_debugDrawer)
167                {
168                        reportMe = false;
169                        m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
170                        m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
171                        m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
172                        m_debugDrawer->reportErrorWarning("Thanks.\n");
173                }
174        }
175}
176
177void    btCollisionWorld::updateAabbs()
178{
179        BT_PROFILE("updateAabbs");
180
181        btTransform predictedTrans;
182        for ( int i=0;i<m_collisionObjects.size();i++)
183        {
184                btCollisionObject* colObj = m_collisionObjects[i];
185
186                //only update aabb of active objects
187                if (m_forceUpdateAllAabbs || colObj->isActive())
188                {
189                        updateSingleAabb(colObj);
190                }
191        }
192}
193
194
195
196void    btCollisionWorld::performDiscreteCollisionDetection()
197{
198        BT_PROFILE("performDiscreteCollisionDetection");
199
200        btDispatcherInfo& dispatchInfo = getDispatchInfo();
201
202        updateAabbs();
203
204        {
205                BT_PROFILE("calculateOverlappingPairs");
206                m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
207        }
208
209
210        btDispatcher* dispatcher = getDispatcher();
211        {
212                BT_PROFILE("dispatchAllCollisionPairs");
213                if (dispatcher)
214                        dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
215        }
216
217}
218
219
220
221void    btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
222{
223
224
225        //bool removeFromBroadphase = false;
226
227        {
228
229                btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
230                if (bp)
231                {
232                        //
233                        // only clear the cached algorithms
234                        //
235                        getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
236                        getBroadphase()->destroyProxy(bp,m_dispatcher1);
237                        collisionObject->setBroadphaseHandle(0);
238                }
239        }
240
241
242        //swapremove
243        m_collisionObjects.remove(collisionObject);
244
245}
246
247
248
249void    btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
250                                                                                btCollisionObject* collisionObject,
251                                                                                const btCollisionShape* collisionShape,
252                                                                                const btTransform& colObjWorldTransform,
253                                                                                RayResultCallback& resultCallback)
254{
255        btSphereShape pointShape(btScalar(0.0));
256        pointShape.setMargin(0.f);
257        const btConvexShape* castShape = &pointShape;
258
259        if (collisionShape->isConvex())
260        {
261                //              BT_PROFILE("rayTestConvex");
262                btConvexCast::CastResult castResult;
263                castResult.m_fraction = resultCallback.m_closestHitFraction;
264
265                btConvexShape* convexShape = (btConvexShape*) collisionShape;
266                btVoronoiSimplexSolver  simplexSolver;
267#define USE_SUBSIMPLEX_CONVEX_CAST 1
268#ifdef USE_SUBSIMPLEX_CONVEX_CAST
269                btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
270#else
271                //btGjkConvexCast       convexCaster(castShape,convexShape,&simplexSolver);
272                //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
273#endif //#USE_SUBSIMPLEX_CONVEX_CAST
274
275                if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
276                {
277                        //add hit
278                        if (castResult.m_normal.length2() > btScalar(0.0001))
279                        {
280                                if (castResult.m_fraction < resultCallback.m_closestHitFraction)
281                                {
282#ifdef USE_SUBSIMPLEX_CONVEX_CAST
283                                        //rotate normal into worldspace
284                                        castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
285#endif //USE_SUBSIMPLEX_CONVEX_CAST
286
287                                        castResult.m_normal.normalize();
288                                        btCollisionWorld::LocalRayResult localRayResult
289                                                (
290                                                collisionObject,
291                                                0,
292                                                castResult.m_normal,
293                                                castResult.m_fraction
294                                                );
295
296                                        bool normalInWorldSpace = true;
297                                        resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
298
299                                }
300                        }
301                }
302        } else {
303                if (collisionShape->isConcave())
304                {
305                        //                      BT_PROFILE("rayTestConcave");
306                        if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
307                        {
308                                ///optimized version for btBvhTriangleMeshShape
309                                btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
310                                btTransform worldTocollisionObject = colObjWorldTransform.inverse();
311                                btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
312                                btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
313
314                                //ConvexCast::CastResult
315                                struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
316                                {
317                                        btCollisionWorld::RayResultCallback* m_resultCallback;
318                                        btCollisionObject*      m_collisionObject;
319                                        btTriangleMeshShape*    m_triangleMesh;
320
321                                        btTransform m_colObjWorldTransform;
322
323                                        BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
324                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh,const btTransform& colObjWorldTransform):
325                                        //@BP Mod
326                                        btTriangleRaycastCallback(from,to, resultCallback->m_flags),
327                                                m_resultCallback(resultCallback),
328                                                m_collisionObject(collisionObject),
329                                                m_triangleMesh(triangleMesh),
330                                                m_colObjWorldTransform(colObjWorldTransform)
331                                        {
332                                        }
333
334
335                                        virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
336                                        {
337                                                btCollisionWorld::LocalShapeInfo        shapeInfo;
338                                                shapeInfo.m_shapePart = partId;
339                                                shapeInfo.m_triangleIndex = triangleIndex;
340
341                                                btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
342
343                                                btCollisionWorld::LocalRayResult rayResult
344                                                        (m_collisionObject,
345                                                        &shapeInfo,
346                                                        hitNormalWorld,
347                                                        hitFraction);
348
349                                                bool    normalInWorldSpace = true;
350                                                return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
351                                        }
352
353                                };
354
355                                BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform);
356                                rcb.m_hitFraction = resultCallback.m_closestHitFraction;
357                                triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
358                        } else
359                        {
360                                //generic (slower) case
361                                btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
362
363                                btTransform worldTocollisionObject = colObjWorldTransform.inverse();
364
365                                btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
366                                btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
367
368                                //ConvexCast::CastResult
369
370                                struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
371                                {
372                                        btCollisionWorld::RayResultCallback* m_resultCallback;
373                                        btCollisionObject*      m_collisionObject;
374                                        btConcaveShape* m_triangleMesh;
375
376                                        btTransform m_colObjWorldTransform;
377
378                                        BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
379                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
380                                        //@BP Mod
381                                        btTriangleRaycastCallback(from,to, resultCallback->m_flags),
382                                                m_resultCallback(resultCallback),
383                                                m_collisionObject(collisionObject),
384                                                m_triangleMesh(triangleMesh),
385                                                m_colObjWorldTransform(colObjWorldTransform)
386                                        {
387                                        }
388
389
390                                        virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
391                                        {
392                                                btCollisionWorld::LocalShapeInfo        shapeInfo;
393                                                shapeInfo.m_shapePart = partId;
394                                                shapeInfo.m_triangleIndex = triangleIndex;
395
396                                                btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
397
398                                                btCollisionWorld::LocalRayResult rayResult
399                                                        (m_collisionObject,
400                                                        &shapeInfo,
401                                                        hitNormalWorld,
402                                                        hitFraction);
403
404                                                bool    normalInWorldSpace = true;
405                                                return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
406                                        }
407
408                                };
409
410
411                                BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
412                                rcb.m_hitFraction = resultCallback.m_closestHitFraction;
413
414                                btVector3 rayAabbMinLocal = rayFromLocal;
415                                rayAabbMinLocal.setMin(rayToLocal);
416                                btVector3 rayAabbMaxLocal = rayFromLocal;
417                                rayAabbMaxLocal.setMax(rayToLocal);
418
419                                concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
420                        }
421                } else {
422                        //                      BT_PROFILE("rayTestCompound");
423                        ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
424                        if (collisionShape->isCompound())
425                        {
426                                const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
427                                int i=0;
428                                for (i=0;i<compoundShape->getNumChildShapes();i++)
429                                {
430                                        btTransform childTrans = compoundShape->getChildTransform(i);
431                                        const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
432                                        btTransform childWorldTrans = colObjWorldTransform * childTrans;
433                                        // replace collision shape so that callback can determine the triangle
434                                        btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
435                                        collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
436                    struct LocalInfoAdder2 : public RayResultCallback {
437                                                RayResultCallback* m_userCallback;
438                                                int m_i;
439                        LocalInfoAdder2 (int i, RayResultCallback *user)
440                                                        : m_userCallback(user),
441                                                        m_i(i)
442                                                { 
443                                                        m_closestHitFraction = m_userCallback->m_closestHitFraction;
444                                                }
445                                                virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
446                            {
447                                    btCollisionWorld::LocalShapeInfo    shapeInfo;
448                                    shapeInfo.m_shapePart = -1;
449                                    shapeInfo.m_triangleIndex = m_i;
450                                    if (r.m_localShapeInfo == NULL)
451                                        r.m_localShapeInfo = &shapeInfo;
452
453                                                                        const btScalar result = m_userCallback->addSingleResult(r, b);
454                                                                        m_closestHitFraction = m_userCallback->m_closestHitFraction;
455                                                                        return result;
456                            }
457                    };
458
459                    LocalInfoAdder2 my_cb(i, &resultCallback);
460
461                                        rayTestSingle(rayFromTrans,rayToTrans,
462                                                collisionObject,
463                                                childCollisionShape,
464                                                childWorldTrans,
465                                                my_cb);
466                                        // restore
467                                        collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
468                                }
469                        }
470                }
471        }
472}
473
474void    btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
475                                                                                        btCollisionObject* collisionObject,
476                                                                                        const btCollisionShape* collisionShape,
477                                                                                        const btTransform& colObjWorldTransform,
478                                                                                        ConvexResultCallback& resultCallback, btScalar allowedPenetration)
479{
480        if (collisionShape->isConvex())
481        {
482                //BT_PROFILE("convexSweepConvex");
483                btConvexCast::CastResult castResult;
484                castResult.m_allowedPenetration = allowedPenetration;
485                castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
486
487                btConvexShape* convexShape = (btConvexShape*) collisionShape;
488                btVoronoiSimplexSolver  simplexSolver;
489                btGjkEpaPenetrationDepthSolver  gjkEpaPenetrationSolver;
490
491                btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
492                //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
493                //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
494
495                btConvexCast* castPtr = &convexCaster1;
496
497
498
499                if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
500                {
501                        //add hit
502                        if (castResult.m_normal.length2() > btScalar(0.0001))
503                        {
504                                if (castResult.m_fraction < resultCallback.m_closestHitFraction)
505                                {
506                                        castResult.m_normal.normalize();
507                                        btCollisionWorld::LocalConvexResult localConvexResult
508                                                (
509                                                collisionObject,
510                                                0,
511                                                castResult.m_normal,
512                                                castResult.m_hitPoint,
513                                                castResult.m_fraction
514                                                );
515
516                                        bool normalInWorldSpace = true;
517                                        resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
518
519                                }
520                        }
521                }
522        } else {
523                if (collisionShape->isConcave())
524                {
525                        if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
526                        {
527                                //BT_PROFILE("convexSweepbtBvhTriangleMesh");
528                                btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
529                                btTransform worldTocollisionObject = colObjWorldTransform.inverse();
530                                btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
531                                btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
532                                // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
533                                btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
534
535                                //ConvexCast::CastResult
536                                struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
537                                {
538                                        btCollisionWorld::ConvexResultCallback* m_resultCallback;
539                                        btCollisionObject*      m_collisionObject;
540                                        btTriangleMeshShape*    m_triangleMesh;
541
542                                        BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
543                                                btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
544                                        btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
545                                                m_resultCallback(resultCallback),
546                                                m_collisionObject(collisionObject),
547                                                m_triangleMesh(triangleMesh)
548                                        {
549                                        }
550
551
552                                        virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
553                                        {
554                                                btCollisionWorld::LocalShapeInfo        shapeInfo;
555                                                shapeInfo.m_shapePart = partId;
556                                                shapeInfo.m_triangleIndex = triangleIndex;
557                                                if (hitFraction <= m_resultCallback->m_closestHitFraction)
558                                                {
559
560                                                        btCollisionWorld::LocalConvexResult convexResult
561                                                                (m_collisionObject,
562                                                                &shapeInfo,
563                                                                hitNormalLocal,
564                                                                hitPointLocal,
565                                                                hitFraction);
566
567                                                        bool    normalInWorldSpace = true;
568
569
570                                                        return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
571                                                }
572                                                return hitFraction;
573                                        }
574
575                                };
576
577                                BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
578                                tccb.m_hitFraction = resultCallback.m_closestHitFraction;
579                                btVector3 boxMinLocal, boxMaxLocal;
580                                castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
581                                triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
582                        } else
583                        {
584                                //BT_PROFILE("convexSweepConcave");
585                                btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
586                                btTransform worldTocollisionObject = colObjWorldTransform.inverse();
587                                btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
588                                btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
589                                // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
590                                btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
591
592                                //ConvexCast::CastResult
593                                struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
594                                {
595                                        btCollisionWorld::ConvexResultCallback* m_resultCallback;
596                                        btCollisionObject*      m_collisionObject;
597                                        btConcaveShape* m_triangleMesh;
598
599                                        BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
600                                                btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*      triangleMesh, const btTransform& triangleToWorld):
601                                        btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
602                                                m_resultCallback(resultCallback),
603                                                m_collisionObject(collisionObject),
604                                                m_triangleMesh(triangleMesh)
605                                        {
606                                        }
607
608
609                                        virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
610                                        {
611                                                btCollisionWorld::LocalShapeInfo        shapeInfo;
612                                                shapeInfo.m_shapePart = partId;
613                                                shapeInfo.m_triangleIndex = triangleIndex;
614                                                if (hitFraction <= m_resultCallback->m_closestHitFraction)
615                                                {
616
617                                                        btCollisionWorld::LocalConvexResult convexResult
618                                                                (m_collisionObject,
619                                                                &shapeInfo,
620                                                                hitNormalLocal,
621                                                                hitPointLocal,
622                                                                hitFraction);
623
624                                                        bool    normalInWorldSpace = false;
625
626                                                        return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
627                                                }
628                                                return hitFraction;
629                                        }
630
631                                };
632
633                                BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
634                                tccb.m_hitFraction = resultCallback.m_closestHitFraction;
635                                btVector3 boxMinLocal, boxMaxLocal;
636                                castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
637
638                                btVector3 rayAabbMinLocal = convexFromLocal;
639                                rayAabbMinLocal.setMin(convexToLocal);
640                                btVector3 rayAabbMaxLocal = convexFromLocal;
641                                rayAabbMaxLocal.setMax(convexToLocal);
642                                rayAabbMinLocal += boxMinLocal;
643                                rayAabbMaxLocal += boxMaxLocal;
644                                concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
645                        }
646                } else {
647                        ///@todo : use AABB tree or other BVH acceleration structure!
648                        if (collisionShape->isCompound())
649                        {
650                                BT_PROFILE("convexSweepCompound");
651                                const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
652                                int i=0;
653                                for (i=0;i<compoundShape->getNumChildShapes();i++)
654                                {
655                                        btTransform childTrans = compoundShape->getChildTransform(i);
656                                        const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
657                                        btTransform childWorldTrans = colObjWorldTransform * childTrans;
658                                        // replace collision shape so that callback can determine the triangle
659                                        btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
660                                        collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
661                    struct      LocalInfoAdder : public ConvexResultCallback {
662                            ConvexResultCallback* m_userCallback;
663                                                        int m_i;
664
665                            LocalInfoAdder (int i, ConvexResultCallback *user)
666                                                                : m_userCallback(user), m_i(i)
667                                                        {
668                                                                m_closestHitFraction = m_userCallback->m_closestHitFraction;
669                                                        }
670                            virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult&      r,      bool b)
671                            {
672                                    btCollisionWorld::LocalShapeInfo    shapeInfo;
673                                    shapeInfo.m_shapePart = -1;
674                                    shapeInfo.m_triangleIndex = m_i;
675                                    if (r.m_localShapeInfo == NULL)
676                                        r.m_localShapeInfo = &shapeInfo;
677                                                                        const btScalar result = m_userCallback->addSingleResult(r, b);
678                                                                        m_closestHitFraction = m_userCallback->m_closestHitFraction;
679                                                                        return result;
680                                   
681                            }
682                    };
683
684                    LocalInfoAdder my_cb(i, &resultCallback);
685                                       
686
687                                        objectQuerySingle(castShape, convexFromTrans,convexToTrans,
688                                                collisionObject,
689                                                childCollisionShape,
690                                                childWorldTrans,
691                                                my_cb, allowedPenetration);
692                                        // restore
693                                        collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
694                                }
695                        }
696                }
697        }
698}
699
700
701struct btSingleRayCallback : public btBroadphaseRayCallback
702{
703
704        btVector3       m_rayFromWorld;
705        btVector3       m_rayToWorld;
706        btTransform     m_rayFromTrans;
707        btTransform     m_rayToTrans;
708        btVector3       m_hitNormal;
709
710        const btCollisionWorld* m_world;
711        btCollisionWorld::RayResultCallback&    m_resultCallback;
712
713        btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
714                :m_rayFromWorld(rayFromWorld),
715                m_rayToWorld(rayToWorld),
716                m_world(world),
717                m_resultCallback(resultCallback)
718        {
719                m_rayFromTrans.setIdentity();
720                m_rayFromTrans.setOrigin(m_rayFromWorld);
721                m_rayToTrans.setIdentity();
722                m_rayToTrans.setOrigin(m_rayToWorld);
723
724                btVector3 rayDir = (rayToWorld-rayFromWorld);
725
726                rayDir.normalize ();
727                ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
728                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
729                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
730                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
731                m_signs[0] = m_rayDirectionInverse[0] < 0.0;
732                m_signs[1] = m_rayDirectionInverse[1] < 0.0;
733                m_signs[2] = m_rayDirectionInverse[2] < 0.0;
734
735                m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
736
737        }
738
739
740
741        virtual bool    process(const btBroadphaseProxy* proxy)
742        {
743                ///terminate further ray tests, once the closestHitFraction reached zero
744                if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
745                        return false;
746
747                btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
748
749                //only perform raycast if filterMask matches
750                if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
751                {
752                        //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
753                        //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
754#if 0
755#ifdef RECALCULATE_AABB
756                        btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
757                        collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
758#else
759                        //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
760                        const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
761                        const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
762#endif
763#endif
764                        //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
765                        //culling already done by broadphase
766                        //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
767                        {
768                                m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
769                                        collisionObject,
770                                        collisionObject->getCollisionShape(),
771                                        collisionObject->getWorldTransform(),
772                                        m_resultCallback);
773                        }
774                }
775                return true;
776        }
777};
778
779void    btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
780{
781        //BT_PROFILE("rayTest");
782        /// use the broadphase to accelerate the search for objects, based on their aabb
783        /// and for each object with ray-aabb overlap, perform an exact ray test
784        btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
785
786#ifndef USE_BRUTEFORCE_RAYBROADPHASE
787        m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
788#else
789        for (int i=0;i<this->getNumCollisionObjects();i++)
790        {
791                rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
792        }       
793#endif //USE_BRUTEFORCE_RAYBROADPHASE
794
795}
796
797
798struct btSingleSweepCallback : public btBroadphaseRayCallback
799{
800
801        btTransform     m_convexFromTrans;
802        btTransform     m_convexToTrans;
803        btVector3       m_hitNormal;
804        const btCollisionWorld* m_world;
805        btCollisionWorld::ConvexResultCallback& m_resultCallback;
806        btScalar        m_allowedCcdPenetration;
807        const btConvexShape* m_castShape;
808
809
810        btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
811                :m_convexFromTrans(convexFromTrans),
812                m_convexToTrans(convexToTrans),
813                m_world(world),
814                m_resultCallback(resultCallback),
815                m_allowedCcdPenetration(allowedPenetration),
816                m_castShape(castShape)
817        {
818                btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
819                btVector3 rayDir = unnormalizedRayDir.normalized();
820                ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
821                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
822                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
823                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
824                m_signs[0] = m_rayDirectionInverse[0] < 0.0;
825                m_signs[1] = m_rayDirectionInverse[1] < 0.0;
826                m_signs[2] = m_rayDirectionInverse[2] < 0.0;
827
828                m_lambda_max = rayDir.dot(unnormalizedRayDir);
829
830        }
831
832        virtual bool    process(const btBroadphaseProxy* proxy)
833        {
834                ///terminate further convex sweep tests, once the closestHitFraction reached zero
835                if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
836                        return false;
837
838                btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
839
840                //only perform raycast if filterMask matches
841                if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
842                        //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
843                        m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
844                                collisionObject,
845                                collisionObject->getCollisionShape(),
846                                collisionObject->getWorldTransform(),
847                                m_resultCallback,
848                                m_allowedCcdPenetration);
849                }
850
851                return true;
852        }
853};
854
855
856
857void    btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
858{
859
860        BT_PROFILE("convexSweepTest");
861        /// use the broadphase to accelerate the search for objects, based on their aabb
862        /// and for each object with ray-aabb overlap, perform an exact ray test
863        /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
864
865
866
867        btTransform     convexFromTrans,convexToTrans;
868        convexFromTrans = convexFromWorld;
869        convexToTrans = convexToWorld;
870        btVector3 castShapeAabbMin, castShapeAabbMax;
871        /* Compute AABB that encompasses angular movement */
872        {
873                btVector3 linVel, angVel;
874                btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
875                btVector3 zeroLinVel;
876                zeroLinVel.setValue(0,0,0);
877                btTransform R;
878                R.setIdentity ();
879                R.setRotation (convexFromTrans.getRotation());
880                castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
881        }
882
883#ifndef USE_BRUTEFORCE_RAYBROADPHASE
884
885        btSingleSweepCallback   convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
886
887        m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
888
889#else
890        /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
891        // do a ray-shape query using convexCaster (CCD)
892        int i;
893        for (i=0;i<m_collisionObjects.size();i++)
894        {
895                btCollisionObject*      collisionObject= m_collisionObjects[i];
896                //only perform raycast if filterMask matches
897                if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
898                        //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
899                        btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
900                        collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
901                        AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
902                        btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
903                        btVector3 hitNormal;
904                        if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
905                        {
906                                objectQuerySingle(castShape, convexFromTrans,convexToTrans,
907                                        collisionObject,
908                                        collisionObject->getCollisionShape(),
909                                        collisionObject->getWorldTransform(),
910                                        resultCallback,
911                                        allowedCcdPenetration);
912                        }
913                }
914        }
915#endif //USE_BRUTEFORCE_RAYBROADPHASE
916}
917
918
919
920struct btBridgedManifoldResult : public btManifoldResult
921{
922
923        btCollisionWorld::ContactResultCallback&        m_resultCallback;
924
925        btBridgedManifoldResult( btCollisionObject* obj0,btCollisionObject* obj1,btCollisionWorld::ContactResultCallback& resultCallback )
926                :btManifoldResult(obj0,obj1),
927                m_resultCallback(resultCallback)
928        {
929        }
930
931        virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
932        {
933                bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
934                btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
935                btVector3 localA;
936                btVector3 localB;
937                if (isSwapped)
938                {
939                        localA = m_rootTransB.invXform(pointA );
940                        localB = m_rootTransA.invXform(pointInWorld);
941                } else
942                {
943                        localA = m_rootTransA.invXform(pointA );
944                        localB = m_rootTransB.invXform(pointInWorld);
945                }
946               
947                btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
948                newPt.m_positionWorldOnA = pointA;
949                newPt.m_positionWorldOnB = pointInWorld;
950               
951           //BP mod, store contact triangles.
952                if (isSwapped)
953                {
954                        newPt.m_partId0 = m_partId1;
955                        newPt.m_partId1 = m_partId0;
956                        newPt.m_index0  = m_index1;
957                        newPt.m_index1  = m_index0;
958                } else
959                {
960                        newPt.m_partId0 = m_partId0;
961                        newPt.m_partId1 = m_partId1;
962                        newPt.m_index0  = m_index0;
963                        newPt.m_index1  = m_index1;
964                }
965
966                //experimental feature info, for per-triangle material etc.
967                btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
968                btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
969                m_resultCallback.addSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
970
971        }
972       
973};
974
975
976
977struct btSingleContactCallback : public btBroadphaseAabbCallback
978{
979
980        btCollisionObject* m_collisionObject;
981        btCollisionWorld*       m_world;
982        btCollisionWorld::ContactResultCallback&        m_resultCallback;
983       
984       
985        btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
986                :m_collisionObject(collisionObject),
987                m_world(world),
988                m_resultCallback(resultCallback)
989        {
990        }
991
992        virtual bool    process(const btBroadphaseProxy* proxy)
993        {
994                btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
995                if (collisionObject == m_collisionObject)
996                        return true;
997
998                //only perform raycast if filterMask matches
999                if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
1000                {
1001                        btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(m_collisionObject,collisionObject);
1002                        if (algorithm)
1003                        {
1004                                btBridgedManifoldResult contactPointResult(m_collisionObject,collisionObject, m_resultCallback);
1005                                //discrete collision detection query
1006                                algorithm->processCollision(m_collisionObject,collisionObject, m_world->getDispatchInfo(),&contactPointResult);
1007
1008                                algorithm->~btCollisionAlgorithm();
1009                                m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
1010                        }
1011                }
1012                return true;
1013        }
1014};
1015
1016
1017///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
1018///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
1019void    btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
1020{
1021        btVector3 aabbMin,aabbMax;
1022        colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
1023        btSingleContactCallback contactCB(colObj,this,resultCallback);
1024       
1025        m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
1026}
1027
1028
1029///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
1030///it reports one or more contact points (including the one with deepest penetration)
1031void    btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
1032{
1033        btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(colObjA,colObjB);
1034        if (algorithm)
1035        {
1036                btBridgedManifoldResult contactPointResult(colObjA,colObjB, resultCallback);
1037                //discrete collision detection query
1038                algorithm->processCollision(colObjA,colObjB, getDispatchInfo(),&contactPointResult);
1039
1040                algorithm->~btCollisionAlgorithm();
1041                getDispatcher()->freeCollisionAlgorithm(algorithm);
1042        }
1043
1044}
1045
1046
1047
1048
1049class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
1050{
1051        btIDebugDraw*   m_debugDrawer;
1052        btVector3       m_color;
1053        btTransform     m_worldTrans;
1054
1055public:
1056
1057        DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
1058          m_debugDrawer(debugDrawer),
1059                  m_color(color),
1060                  m_worldTrans(worldTrans)
1061          {
1062          }
1063
1064          virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
1065          {
1066                  processTriangle(triangle,partId,triangleIndex);
1067          }
1068
1069          virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
1070          {
1071                  (void)partId;
1072                  (void)triangleIndex;
1073
1074                  btVector3 wv0,wv1,wv2;
1075                  wv0 = m_worldTrans*triangle[0];
1076                  wv1 = m_worldTrans*triangle[1];
1077                  wv2 = m_worldTrans*triangle[2];
1078                  btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
1079
1080                  btVector3 normal = (wv1-wv0).cross(wv2-wv0);
1081                  normal.normalize();
1082                  btVector3 normalColor(1,1,0);
1083                  m_debugDrawer->drawLine(center,center+normal,normalColor);
1084
1085
1086
1087                 
1088                  m_debugDrawer->drawLine(wv0,wv1,m_color);
1089                  m_debugDrawer->drawLine(wv1,wv2,m_color);
1090                  m_debugDrawer->drawLine(wv2,wv0,m_color);
1091          }
1092};
1093
1094
1095void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
1096{
1097        // Draw a small simplex at the center of the object
1098        getDebugDrawer()->drawTransform(worldTransform,1);
1099
1100        if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
1101        {
1102                const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
1103                for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
1104                {
1105                        btTransform childTrans = compoundShape->getChildTransform(i);
1106                        const btCollisionShape* colShape = compoundShape->getChildShape(i);
1107                        debugDrawObject(worldTransform*childTrans,colShape,color);
1108                }
1109
1110        } else
1111        {
1112                switch (shape->getShapeType())
1113                {
1114
1115                case BOX_SHAPE_PROXYTYPE:
1116                        {
1117                                const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
1118                                btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
1119                                getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
1120                                break;
1121                        }
1122
1123                case SPHERE_SHAPE_PROXYTYPE:
1124                        {
1125                                const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
1126                                btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
1127
1128                                getDebugDrawer()->drawSphere(radius, worldTransform, color);
1129                                break;
1130                        }
1131                case MULTI_SPHERE_SHAPE_PROXYTYPE:
1132                        {
1133                                const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
1134
1135                                btTransform childTransform;
1136                                childTransform.setIdentity();
1137
1138                                for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
1139                                {
1140                                        childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
1141                                        getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
1142                                }
1143
1144                                break;
1145                        }
1146                case CAPSULE_SHAPE_PROXYTYPE:
1147                        {
1148                                const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
1149
1150                                btScalar radius = capsuleShape->getRadius();
1151                                btScalar halfHeight = capsuleShape->getHalfHeight();
1152
1153                                int upAxis = capsuleShape->getUpAxis();
1154
1155
1156                                btVector3 capStart(0.f,0.f,0.f);
1157                                capStart[upAxis] = -halfHeight;
1158
1159                                btVector3 capEnd(0.f,0.f,0.f);
1160                                capEnd[upAxis] = halfHeight;
1161
1162                                // Draw the ends
1163                                {
1164
1165                                        btTransform childTransform = worldTransform;
1166                                        childTransform.getOrigin() = worldTransform * capStart;
1167                                        getDebugDrawer()->drawSphere(radius, childTransform, color);
1168                                }
1169
1170                                {
1171                                        btTransform childTransform = worldTransform;
1172                                        childTransform.getOrigin() = worldTransform * capEnd;
1173                                        getDebugDrawer()->drawSphere(radius, childTransform, color);
1174                                }
1175
1176                                // Draw some additional lines
1177                                btVector3 start = worldTransform.getOrigin();
1178
1179
1180                                capStart[(upAxis+1)%3] = radius;
1181                                capEnd[(upAxis+1)%3] = radius;
1182                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
1183                                capStart[(upAxis+1)%3] = -radius;
1184                                capEnd[(upAxis+1)%3] = -radius;
1185                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
1186
1187                                capStart[(upAxis+1)%3] = 0.f;
1188                                capEnd[(upAxis+1)%3] = 0.f;
1189
1190                                capStart[(upAxis+2)%3] = radius;
1191                                capEnd[(upAxis+2)%3] = radius;
1192                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
1193                                capStart[(upAxis+2)%3] = -radius;
1194                                capEnd[(upAxis+2)%3] = -radius;
1195                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
1196
1197
1198                                break;
1199                        }
1200                case CONE_SHAPE_PROXYTYPE:
1201                        {
1202                                const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
1203                                btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
1204                                btScalar height = coneShape->getHeight();//+coneShape->getMargin();
1205                                btVector3 start = worldTransform.getOrigin();
1206
1207                                int upAxis= coneShape->getConeUpIndex();
1208
1209
1210                                btVector3       offsetHeight(0,0,0);
1211                                offsetHeight[upAxis] = height * btScalar(0.5);
1212                                btVector3       offsetRadius(0,0,0);
1213                                offsetRadius[(upAxis+1)%3] = radius;
1214                                btVector3       offset2Radius(0,0,0);
1215                                offset2Radius[(upAxis+2)%3] = radius;
1216
1217                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
1218                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
1219                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
1220                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
1221
1222                                // Drawing the base of the cone
1223                                btVector3 yaxis(0,0,0);
1224                                yaxis[upAxis] = btScalar(1.0);
1225                                btVector3 xaxis(0,0,0);
1226                                xaxis[(upAxis+1)%3] = btScalar(1.0);
1227                                getDebugDrawer()->drawArc(start-worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,10.0);
1228                break;
1229
1230                        }
1231                case CYLINDER_SHAPE_PROXYTYPE:
1232                        {
1233                                const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
1234                                int upAxis = cylinder->getUpAxis();
1235                                btScalar radius = cylinder->getRadius();
1236                                btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
1237                                btVector3 start = worldTransform.getOrigin();
1238                                btVector3       offsetHeight(0,0,0);
1239                                offsetHeight[upAxis] = halfHeight;
1240                                btVector3       offsetRadius(0,0,0);
1241                                offsetRadius[(upAxis+1)%3] = radius;
1242                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
1243                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
1244
1245                                // Drawing top and bottom caps of the cylinder
1246                                btVector3 yaxis(0,0,0);
1247                                yaxis[upAxis] = btScalar(1.0);
1248                                btVector3 xaxis(0,0,0);
1249                                xaxis[(upAxis+1)%3] = btScalar(1.0);
1250                                getDebugDrawer()->drawArc(start-worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
1251                                getDebugDrawer()->drawArc(start+worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
1252                                break;
1253                        }
1254
1255                case STATIC_PLANE_PROXYTYPE:
1256                        {
1257                                const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
1258                                btScalar planeConst = staticPlaneShape->getPlaneConstant();
1259                                const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
1260                                btVector3 planeOrigin = planeNormal * planeConst;
1261                                btVector3 vec0,vec1;
1262                                btPlaneSpace1(planeNormal,vec0,vec1);
1263                                btScalar vecLen = 100.f;
1264                                btVector3 pt0 = planeOrigin + vec0*vecLen;
1265                                btVector3 pt1 = planeOrigin - vec0*vecLen;
1266                                btVector3 pt2 = planeOrigin + vec1*vecLen;
1267                                btVector3 pt3 = planeOrigin - vec1*vecLen;
1268                                getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
1269                                getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
1270                                break;
1271
1272                        }
1273                default:
1274                        {
1275
1276                                if (shape->isConcave())
1277                                {
1278                                        btConcaveShape* concaveMesh = (btConcaveShape*) shape;
1279
1280                                        ///@todo pass camera, for some culling? no -> we are not a graphics lib
1281                                        btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
1282                                        btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
1283
1284                                        DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1285                                        concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
1286
1287                                }
1288
1289                                if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
1290                                {
1291                                        btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
1292                                        //todo: pass camera for some culling                   
1293                                        btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
1294                                        btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
1295                                        //DebugDrawcallback drawCallback;
1296                                        DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1297                                        convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
1298                                }
1299
1300
1301                                /// for polyhedral shapes
1302                                if (shape->isPolyhedral())
1303                                {
1304                                        btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
1305
1306                                        int i;
1307                                        for (i=0;i<polyshape->getNumEdges();i++)
1308                                        {
1309                                                btVector3 a,b;
1310                                                polyshape->getEdge(i,a,b);
1311                                                btVector3 wa = worldTransform * a;
1312                                                btVector3 wb = worldTransform * b;
1313                                                getDebugDrawer()->drawLine(wa,wb,color);
1314
1315                                        }
1316
1317
1318                                }
1319                        }
1320                }
1321        }
1322}
1323
1324
1325void    btCollisionWorld::debugDrawWorld()
1326{
1327        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
1328        {
1329                int numManifolds = getDispatcher()->getNumManifolds();
1330                btVector3 color(0,0,0);
1331                for (int i=0;i<numManifolds;i++)
1332                {
1333                        btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
1334                        //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
1335                        //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
1336
1337                        int numContacts = contactManifold->getNumContacts();
1338                        for (int j=0;j<numContacts;j++)
1339                        {
1340                                btManifoldPoint& cp = contactManifold->getContactPoint(j);
1341                                getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
1342                        }
1343                }
1344        }
1345
1346        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
1347        {
1348                int i;
1349
1350                for (  i=0;i<m_collisionObjects.size();i++)
1351                {
1352                        btCollisionObject* colObj = m_collisionObjects[i];
1353                        if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
1354                        {
1355                                if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
1356                                {
1357                                        btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.));
1358                                        switch(colObj->getActivationState())
1359                                        {
1360                                        case  ACTIVE_TAG:
1361                                                color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break;
1362                                        case ISLAND_SLEEPING:
1363                                                color =  btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break;
1364                                        case WANTS_DEACTIVATION:
1365                                                color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break;
1366                                        case DISABLE_DEACTIVATION:
1367                                                color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break;
1368                                        case DISABLE_SIMULATION:
1369                                                color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break;
1370                                        default:
1371                                                {
1372                                                        color = btVector3(btScalar(1),btScalar(0.),btScalar(0.));
1373                                                }
1374                                        };
1375
1376                                        debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
1377                                }
1378                                if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
1379                                {
1380                                        btVector3 minAabb,maxAabb;
1381                                        btVector3 colorvec(1,0,0);
1382                                        colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
1383                                        m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
1384                                }
1385                        }
1386
1387                }
1388        }
1389}
1390
1391
1392void    btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
1393{
1394        int i;
1395        //serialize all collision objects
1396        for (i=0;i<m_collisionObjects.size();i++)
1397        {
1398                btCollisionObject* colObj = m_collisionObjects[i];
1399                if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
1400                {
1401                        colObj->serializeSingleObject(serializer);
1402                }
1403        }
1404
1405        ///keep track of shapes already serialized
1406        btHashMap<btHashPtr,btCollisionShape*>  serializedShapes;
1407
1408        for (i=0;i<m_collisionObjects.size();i++)
1409        {
1410                btCollisionObject* colObj = m_collisionObjects[i];
1411                btCollisionShape* shape = colObj->getCollisionShape();
1412
1413                if (!serializedShapes.find(shape))
1414                {
1415                        serializedShapes.insert(shape,shape);
1416                        shape->serializeSingleShape(serializer);
1417                }
1418        }
1419
1420}
1421
1422
1423void    btCollisionWorld::serialize(btSerializer* serializer)
1424{
1425
1426        serializer->startSerialization();
1427       
1428        serializeCollisionObjects(serializer);
1429       
1430        serializer->finishSerialization();
1431}
1432
Note: See TracBrowser for help on using the repository browser.