Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @ 12177

Last change on this file since 12177 was 12177, checked in by siramesh, 5 years ago

Super Orxo Bros Final (Sidharth Ramesh, Nisa Balta, Jeff Ren)

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