← Back to branch summary

~siretart/ubuntu/utopic/blender/libav10

« back to all changes in this revision

Viewing changes to extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

  • Committer: Package Import Robot
  • Author(s): Matteo F. Vescovi
  • Date: 2012-07-23 08:54:18 UTC
  • mfrom: (14.2.16 sid)
  • mto: (14.2.19 sid)
  • mto: This revision was merged to the branch mainline in revision 42.
  • Revision ID: package-import@ubuntu.com-20120723085418-9foz30v6afaf5ffs
Tags: 2.63a-2
http://bugs.debian.org/658075
* debian/: Cycles support added (Closes: #658075)
  For now, this top feature has been enabled only
  on [any-amd64 any-i386] architectures because
  of OpenImageIO failing on all others
* debian/: scripts installation path changed
  from /usr/lib to /usr/share:
  + debian/patches/: patchset re-worked for path changing
  + debian/control: "Breaks" field added on yafaray-exporter

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
26
26
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
27
27
#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
28
28
#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
29
 
 
 
29
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
30
30
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
 
31
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
31
32
#include "LinearMath/btAabbUtil2.h"
32
33
#include "LinearMath/btQuickprof.h"
33
34
#include "LinearMath/btStackAlloc.h"
34
 
#include "BulletSoftBody/btSoftBody.h"
 
35
#include "LinearMath/btSerializer.h"
 
36
 
 
37
//#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
 
38
 
35
39
 
36
40
//#define USE_BRUTEFORCE_RAYBROADPHASE 1
37
41
//RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation'  or 'updateAabbs' before using a rayTest
43
47
#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
44
48
 
45
49
 
 
50
///for debug drawing
 
51
 
 
52
//for debug rendering
 
53
#include "BulletCollision/CollisionShapes/btBoxShape.h"
 
54
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
 
55
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
 
56
#include "BulletCollision/CollisionShapes/btConeShape.h"
 
57
#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
 
58
#include "BulletCollision/CollisionShapes/btCylinderShape.h"
 
59
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
 
60
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
 
61
#include "BulletCollision/CollisionShapes/btSphereShape.h"
 
62
#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
 
63
#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
 
64
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
 
65
 
 
66
 
 
67
 
46
68
btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
47
69
:m_dispatcher1(dispatcher),
48
70
m_broadphasePairCache(pairCache),
49
 
m_debugDrawer(0)
 
71
m_debugDrawer(0),
 
72
m_forceUpdateAllAabbs(true)
50
73
{
51
74
        m_stackAlloc = collisionConfiguration->getStackAllocator();
52
75
        m_dispatchInfo.m_stackAllocator = m_stackAlloc;
89
112
void    btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
90
113
{
91
114
 
 
115
        btAssert(collisionObject);
 
116
 
92
117
        //check that the object isn't already added
93
 
                btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
94
 
 
95
 
                m_collisionObjects.push_back(collisionObject);
96
 
 
97
 
                //calculate new AABB
98
 
                btTransform trans = collisionObject->getWorldTransform();
99
 
 
100
 
                btVector3       minAabb;
101
 
                btVector3       maxAabb;
102
 
                collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
103
 
 
104
 
                int type = collisionObject->getCollisionShape()->getShapeType();
105
 
                collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
106
 
                        minAabb,
107
 
                        maxAabb,
108
 
                        type,
109
 
                        collisionObject,
110
 
                        collisionFilterGroup,
111
 
                        collisionFilterMask,
112
 
                        m_dispatcher1,0
113
 
                        ))      ;
 
118
        btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
 
119
 
 
120
        m_collisionObjects.push_back(collisionObject);
 
121
 
 
122
        //calculate new AABB
 
123
        btTransform trans = collisionObject->getWorldTransform();
 
124
 
 
125
        btVector3       minAabb;
 
126
        btVector3       maxAabb;
 
127
        collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
 
128
 
 
129
        int type = collisionObject->getCollisionShape()->getShapeType();
 
130
        collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
 
131
                minAabb,
 
132
                maxAabb,
 
133
                type,
 
134
                collisionObject,
 
135
                collisionFilterGroup,
 
136
                collisionFilterMask,
 
137
                m_dispatcher1,0
 
138
                ))      ;
114
139
 
115
140
 
116
141
 
129
154
        minAabb -= contactThreshold;
130
155
        maxAabb += contactThreshold;
131
156
 
 
157
        if(getDispatchInfo().m_convexMaxDistanceUseCPT)
 
158
        {
 
159
                btVector3 minAabb2,maxAabb2;
 
160
                colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
 
161
                minAabb2 -= contactThreshold;
 
162
                maxAabb2 += contactThreshold;
 
163
                minAabb.setMin(minAabb2);
 
164
                maxAabb.setMax(maxAabb2);
 
165
        }
 
166
 
132
167
        btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
133
168
 
134
169
        //moving objects should be moderately sized, probably something wrong if not
163
198
                btCollisionObject* colObj = m_collisionObjects[i];
164
199
 
165
200
                //only update aabb of active objects
166
 
                if (colObj->isActive())
 
201
                if (m_forceUpdateAllAabbs || colObj->isActive())
167
202
                {
168
203
                        updateSingleAabb(colObj);
169
204
                }
226
261
 
227
262
 
228
263
void    btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
229
 
                                          btCollisionObject* collisionObject,
230
 
                                          const btCollisionShape* collisionShape,
231
 
                                          const btTransform& colObjWorldTransform,
232
 
                                          RayResultCallback& resultCallback)
 
264
                                                                                btCollisionObject* collisionObject,
 
265
                                                                                const btCollisionShape* collisionShape,
 
266
                                                                                const btTransform& colObjWorldTransform,
 
267
                                                                                RayResultCallback& resultCallback)
233
268
{
234
269
        btSphereShape pointShape(btScalar(0.0));
235
270
        pointShape.setMargin(0.f);
237
272
 
238
273
        if (collisionShape->isConvex())
239
274
        {
240
 
//              BT_PROFILE("rayTestConvex");
 
275
                //              BT_PROFILE("rayTestConvex");
241
276
                btConvexCast::CastResult castResult;
242
277
                castResult.m_fraction = resultCallback.m_closestHitFraction;
243
278
 
266
301
                                        castResult.m_normal.normalize();
267
302
                                        btCollisionWorld::LocalRayResult localRayResult
268
303
                                                (
269
 
                                                        collisionObject,
270
 
                                                        0,
271
 
                                                        castResult.m_normal,
272
 
                                                        castResult.m_fraction
 
304
                                                collisionObject,
 
305
                                                0,
 
306
                                                castResult.m_normal,
 
307
                                                castResult.m_fraction
273
308
                                                );
274
309
 
275
310
                                        bool normalInWorldSpace = true;
281
316
        } else {
282
317
                if (collisionShape->isConcave())
283
318
                {
284
 
//                      BT_PROFILE("rayTestConcave");
 
319
                        //                      BT_PROFILE("rayTestConcave");
285
320
                        if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
286
321
                        {
287
322
                                ///optimized version for btBvhTriangleMeshShape
297
332
                                        btCollisionObject*      m_collisionObject;
298
333
                                        btTriangleMeshShape*    m_triangleMesh;
299
334
 
 
335
                                        btTransform m_colObjWorldTransform;
 
336
 
300
337
                                        BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
301
 
                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh):
302
 
                  //@BP Mod
303
 
                                                btTriangleRaycastCallback(from,to, resultCallback->m_flags),
304
 
                                                        m_resultCallback(resultCallback),
305
 
                                                        m_collisionObject(collisionObject),
306
 
                                                        m_triangleMesh(triangleMesh)
307
 
                                                {
308
 
                                                }
 
338
                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh,const btTransform& colObjWorldTransform):
 
339
                                        //@BP Mod
 
340
                                        btTriangleRaycastCallback(from,to, resultCallback->m_flags),
 
341
                                                m_resultCallback(resultCallback),
 
342
                                                m_collisionObject(collisionObject),
 
343
                                                m_triangleMesh(triangleMesh),
 
344
                                                m_colObjWorldTransform(colObjWorldTransform)
 
345
                                        {
 
346
                                        }
309
347
 
310
348
 
311
349
                                        virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
314
352
                                                shapeInfo.m_shapePart = partId;
315
353
                                                shapeInfo.m_triangleIndex = triangleIndex;
316
354
 
 
355
                                                btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
 
356
 
317
357
                                                btCollisionWorld::LocalRayResult rayResult
318
 
                                                (m_collisionObject,
 
358
                                                        (m_collisionObject,
319
359
                                                        &shapeInfo,
320
 
                                                        hitNormalLocal,
 
360
                                                        hitNormalWorld,
321
361
                                                        hitFraction);
322
362
 
323
 
                                                bool    normalInWorldSpace = false;
 
363
                                                bool    normalInWorldSpace = true;
324
364
                                                return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
325
365
                                        }
326
366
 
327
367
                                };
328
368
 
329
 
                                BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
 
369
                                BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform);
330
370
                                rcb.m_hitFraction = resultCallback.m_closestHitFraction;
331
371
                                triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
332
372
                        } else
347
387
                                        btCollisionObject*      m_collisionObject;
348
388
                                        btConcaveShape* m_triangleMesh;
349
389
 
 
390
                                        btTransform m_colObjWorldTransform;
 
391
 
350
392
                                        BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
351
 
                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh):
352
 
                  //@BP Mod
353
 
                  btTriangleRaycastCallback(from,to, resultCallback->m_flags),
354
 
                                                        m_resultCallback(resultCallback),
355
 
                                                        m_collisionObject(collisionObject),
356
 
                                                        m_triangleMesh(triangleMesh)
357
 
                                                {
358
 
                                                }
 
393
                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
 
394
                                        //@BP Mod
 
395
                                        btTriangleRaycastCallback(from,to, resultCallback->m_flags),
 
396
                                                m_resultCallback(resultCallback),
 
397
                                                m_collisionObject(collisionObject),
 
398
                                                m_triangleMesh(triangleMesh),
 
399
                                                m_colObjWorldTransform(colObjWorldTransform)
 
400
                                        {
 
401
                                        }
359
402
 
360
403
 
361
404
                                        virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
364
407
                                                shapeInfo.m_shapePart = partId;
365
408
                                                shapeInfo.m_triangleIndex = triangleIndex;
366
409
 
 
410
                                                btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
 
411
 
367
412
                                                btCollisionWorld::LocalRayResult rayResult
368
 
                                                (m_collisionObject,
 
413
                                                        (m_collisionObject,
369
414
                                                        &shapeInfo,
370
 
                                                        hitNormalLocal,
 
415
                                                        hitNormalWorld,
371
416
                                                        hitFraction);
372
417
 
373
 
                                                bool    normalInWorldSpace = false;
 
418
                                                bool    normalInWorldSpace = true;
374
419
                                                return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
375
 
 
376
 
 
377
420
                                        }
378
421
 
379
422
                                };
380
423
 
381
424
 
382
 
                                BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape);
 
425
                                BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
383
426
                                rcb.m_hitFraction = resultCallback.m_closestHitFraction;
384
427
 
385
428
                                btVector3 rayAabbMinLocal = rayFromLocal;
390
433
                                concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
391
434
                        }
392
435
                } else {
393
 
//                      BT_PROFILE("rayTestCompound");
394
 
                        ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
 
436
                        //                      BT_PROFILE("rayTestCompound");
395
437
                        if (collisionShape->isCompound())
396
438
                        {
 
439
                                struct LocalInfoAdder2 : public RayResultCallback
 
440
                                {
 
441
                                        RayResultCallback* m_userCallback;
 
442
                                        int m_i;
 
443
                                        
 
444
                                        LocalInfoAdder2 (int i, RayResultCallback *user)
 
445
                                                : m_userCallback(user), m_i(i)
 
446
                                        { 
 
447
                                                m_closestHitFraction = m_userCallback->m_closestHitFraction;
 
448
                                        }
 
449
                                        virtual bool needsCollision(btBroadphaseProxy* p) const
 
450
                                        {
 
451
                                                return m_userCallback->needsCollision(p);
 
452
                                        }
 
453
 
 
454
                                        virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
 
455
                                        {
 
456
                                                btCollisionWorld::LocalShapeInfo shapeInfo;
 
457
                                                shapeInfo.m_shapePart = -1;
 
458
                                                shapeInfo.m_triangleIndex = m_i;
 
459
                                                if (r.m_localShapeInfo == NULL)
 
460
                                                        r.m_localShapeInfo = &shapeInfo;
 
461
 
 
462
                                                const btScalar result = m_userCallback->addSingleResult(r, b);
 
463
                                                m_closestHitFraction = m_userCallback->m_closestHitFraction;
 
464
                                                return result;
 
465
                                        }
 
466
                                };
 
467
                                
 
468
                                struct RayTester : btDbvt::ICollide
 
469
                                {
 
470
                                        btCollisionObject* m_collisionObject;
 
471
                                        const btCompoundShape* m_compoundShape;
 
472
                                        const btTransform& m_colObjWorldTransform;
 
473
                                        const btTransform& m_rayFromTrans;
 
474
                                        const btTransform& m_rayToTrans;
 
475
                                        RayResultCallback& m_resultCallback;
 
476
                                        
 
477
                                        RayTester(btCollisionObject* collisionObject,
 
478
                                                        const btCompoundShape* compoundShape,
 
479
                                                        const btTransform& colObjWorldTransform,
 
480
                                                        const btTransform& rayFromTrans,
 
481
                                                        const btTransform& rayToTrans,
 
482
                                                        RayResultCallback& resultCallback):
 
483
                                                m_collisionObject(collisionObject),
 
484
                                                m_compoundShape(compoundShape),
 
485
                                                m_colObjWorldTransform(colObjWorldTransform),
 
486
                                                m_rayFromTrans(rayFromTrans),
 
487
                                                m_rayToTrans(rayToTrans),
 
488
                                                m_resultCallback(resultCallback)
 
489
                                        {
 
490
                                                
 
491
                                        }
 
492
                                        
 
493
                                        void Process(int i)
 
494
                                        {
 
495
                                                const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
 
496
                                                const btTransform& childTrans = m_compoundShape->getChildTransform(i);
 
497
                                                btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
 
498
                                                
 
499
                                                // replace collision shape so that callback can determine the triangle
 
500
                                                btCollisionShape* saveCollisionShape = m_collisionObject->getCollisionShape();
 
501
                                                m_collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
 
502
 
 
503
                                                LocalInfoAdder2 my_cb(i, &m_resultCallback);
 
504
 
 
505
                                                rayTestSingle(
 
506
                                                        m_rayFromTrans,
 
507
                                                        m_rayToTrans,
 
508
                                                        m_collisionObject,
 
509
                                                        childCollisionShape,
 
510
                                                        childWorldTrans,
 
511
                                                        my_cb);
 
512
                                                
 
513
                                                // restore
 
514
                                                m_collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
 
515
                                        }
 
516
                                        
 
517
                                        void Process(const btDbvtNode* leaf)
 
518
                                        {
 
519
                                                Process(leaf->dataAsInt);
 
520
                                        }
 
521
                                };
 
522
                                
397
523
                                const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
398
 
                                int i=0;
399
 
                                for (i=0;i<compoundShape->getNumChildShapes();i++)
400
 
                                {
401
 
                                        btTransform childTrans = compoundShape->getChildTransform(i);
402
 
                                        const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
403
 
                                        btTransform childWorldTrans = colObjWorldTransform * childTrans;
404
 
                                        // replace collision shape so that callback can determine the triangle
405
 
                                        btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
406
 
                                        collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
407
 
                                        rayTestSingle(rayFromTrans,rayToTrans,
408
 
                                                collisionObject,
409
 
                                                childCollisionShape,
410
 
                                                childWorldTrans,
411
 
                                                resultCallback);
412
 
                                        // restore
413
 
                                        collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
 
524
                                const btDbvt* dbvt = compoundShape->getDynamicAabbTree();
 
525
 
 
526
 
 
527
                                RayTester rayCB(
 
528
                                        collisionObject,
 
529
                                        compoundShape,
 
530
                                        colObjWorldTransform,
 
531
                                        rayFromTrans,
 
532
                                        rayToTrans,
 
533
                                        resultCallback);
 
534
#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
 
535
                                if (dbvt)
 
536
                                {
 
537
                                        btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
 
538
                                        btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
 
539
                                        btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB);
 
540
                                }
 
541
                                else
 
542
#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
 
543
                                {
 
544
                                        for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
 
545
                                        {
 
546
                                                rayCB.Process(i);
 
547
                                        }       
414
548
                                }
415
549
                        }
416
550
                }
418
552
}
419
553
 
420
554
void    btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
421
 
                                          btCollisionObject* collisionObject,
422
 
                                          const btCollisionShape* collisionShape,
423
 
                                          const btTransform& colObjWorldTransform,
424
 
                                          ConvexResultCallback& resultCallback, btScalar allowedPenetration)
 
555
                                                                                        btCollisionObject* collisionObject,
 
556
                                                                                        const btCollisionShape* collisionShape,
 
557
                                                                                        const btTransform& colObjWorldTransform,
 
558
                                                                                        ConvexResultCallback& resultCallback, btScalar allowedPenetration)
425
559
{
426
560
        if (collisionShape->isConvex())
427
561
        {
433
567
                btConvexShape* convexShape = (btConvexShape*) collisionShape;
434
568
                btVoronoiSimplexSolver  simplexSolver;
435
569
                btGjkEpaPenetrationDepthSolver  gjkEpaPenetrationSolver;
436
 
                
 
570
 
437
571
                btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
438
572
                //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
439
573
                //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
440
574
 
441
575
                btConvexCast* castPtr = &convexCaster1;
442
 
        
443
 
        
444
 
                
 
576
 
 
577
 
 
578
 
445
579
                if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
446
580
                {
447
581
                        //add hit
451
585
                                {
452
586
                                        castResult.m_normal.normalize();
453
587
                                        btCollisionWorld::LocalConvexResult localConvexResult
454
 
                                                                (
455
 
                                                                        collisionObject,
456
 
                                                                        0,
457
 
                                                                        castResult.m_normal,
458
 
                                                                        castResult.m_hitPoint,
459
 
                                                                        castResult.m_fraction
460
 
                                                                );
 
588
                                                (
 
589
                                                collisionObject,
 
590
                                                0,
 
591
                                                castResult.m_normal,
 
592
                                                castResult.m_hitPoint,
 
593
                                                castResult.m_fraction
 
594
                                                );
461
595
 
462
596
                                        bool normalInWorldSpace = true;
463
597
                                        resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
487
621
 
488
622
                                        BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
489
623
                                                btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
490
 
                                                btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
491
 
                                                        m_resultCallback(resultCallback),
492
 
                                                        m_collisionObject(collisionObject),
493
 
                                                        m_triangleMesh(triangleMesh)
494
 
                                                {
495
 
                                                }
 
624
                                        btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
 
625
                                                m_resultCallback(resultCallback),
 
626
                                                m_collisionObject(collisionObject),
 
627
                                                m_triangleMesh(triangleMesh)
 
628
                                        {
 
629
                                        }
496
630
 
497
631
 
498
632
                                        virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
504
638
                                                {
505
639
 
506
640
                                                        btCollisionWorld::LocalConvexResult convexResult
507
 
                                                        (m_collisionObject,
 
641
                                                                (m_collisionObject,
508
642
                                                                &shapeInfo,
509
643
                                                                hitNormalLocal,
510
644
                                                                hitPointLocal,
522
656
 
523
657
                                BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
524
658
                                tccb.m_hitFraction = resultCallback.m_closestHitFraction;
 
659
                                tccb.m_allowedPenetration = allowedPenetration;
525
660
                                btVector3 boxMinLocal, boxMaxLocal;
526
661
                                castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
527
662
                                triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
544
679
 
545
680
                                        BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
546
681
                                                btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*      triangleMesh, const btTransform& triangleToWorld):
547
 
                                                btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
548
 
                                                        m_resultCallback(resultCallback),
549
 
                                                        m_collisionObject(collisionObject),
550
 
                                                        m_triangleMesh(triangleMesh)
551
 
                                                {
552
 
                                                }
 
682
                                        btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
 
683
                                                m_resultCallback(resultCallback),
 
684
                                                m_collisionObject(collisionObject),
 
685
                                                m_triangleMesh(triangleMesh)
 
686
                                        {
 
687
                                        }
553
688
 
554
689
 
555
690
                                        virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
561
696
                                                {
562
697
 
563
698
                                                        btCollisionWorld::LocalConvexResult convexResult
564
 
                                                        (m_collisionObject,
 
699
                                                                (m_collisionObject,
565
700
                                                                &shapeInfo,
566
701
                                                                hitNormalLocal,
567
702
                                                                hitPointLocal,
578
713
 
579
714
                                BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
580
715
                                tccb.m_hitFraction = resultCallback.m_closestHitFraction;
 
716
                                tccb.m_allowedPenetration = allowedPenetration;
581
717
                                btVector3 boxMinLocal, boxMaxLocal;
582
718
                                castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
583
719
 
604
740
                                        // replace collision shape so that callback can determine the triangle
605
741
                                        btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
606
742
                                        collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
 
743
                    struct      LocalInfoAdder : public ConvexResultCallback {
 
744
                            ConvexResultCallback* m_userCallback;
 
745
                                                        int m_i;
 
746
 
 
747
                            LocalInfoAdder (int i, ConvexResultCallback *user)
 
748
                                                                : m_userCallback(user), m_i(i)
 
749
                                                        {
 
750
                                                                m_closestHitFraction = m_userCallback->m_closestHitFraction;
 
751
                                                        }
 
752
                                                        virtual bool needsCollision(btBroadphaseProxy* p) const
 
753
                                                        {
 
754
                                                                return m_userCallback->needsCollision(p);
 
755
                                                        }
 
756
                            virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult&      r,      bool b)
 
757
                            {
 
758
                                    btCollisionWorld::LocalShapeInfo    shapeInfo;
 
759
                                    shapeInfo.m_shapePart = -1;
 
760
                                    shapeInfo.m_triangleIndex = m_i;
 
761
                                    if (r.m_localShapeInfo == NULL)
 
762
                                        r.m_localShapeInfo = &shapeInfo;
 
763
                                                                        const btScalar result = m_userCallback->addSingleResult(r, b);
 
764
                                                                        m_closestHitFraction = m_userCallback->m_closestHitFraction;
 
765
                                                                        return result;
 
766
                                    
 
767
                            }
 
768
                    };
 
769
 
 
770
                    LocalInfoAdder my_cb(i, &resultCallback);
 
771
                                        
 
772
 
607
773
                                        objectQuerySingle(castShape, convexFromTrans,convexToTrans,
608
774
                                                collisionObject,
609
775
                                                childCollisionShape,
610
776
                                                childWorldTrans,
611
 
                                                resultCallback, allowedPenetration);
 
777
                                                my_cb, allowedPenetration);
612
778
                                        // restore
613
779
                                        collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
614
780
                                }
631
797
        btCollisionWorld::RayResultCallback&    m_resultCallback;
632
798
 
633
799
        btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
634
 
        :m_rayFromWorld(rayFromWorld),
635
 
        m_rayToWorld(rayToWorld),
636
 
        m_world(world),
637
 
        m_resultCallback(resultCallback)
 
800
                :m_rayFromWorld(rayFromWorld),
 
801
                m_rayToWorld(rayToWorld),
 
802
                m_world(world),
 
803
                m_resultCallback(resultCallback)
638
804
        {
639
805
                m_rayFromTrans.setIdentity();
640
806
                m_rayFromTrans.setOrigin(m_rayFromWorld);
644
810
                btVector3 rayDir = (rayToWorld-rayFromWorld);
645
811
 
646
812
                rayDir.normalize ();
647
 
                ///what about division by zero? --> just set rayDirection[i] to INF/1e30
648
 
                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
649
 
                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
650
 
                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
 
813
                ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
 
814
                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
 
815
                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
 
816
                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
651
817
                m_signs[0] = m_rayDirectionInverse[0] < 0.0;
652
818
                m_signs[1] = m_rayDirectionInverse[1] < 0.0;
653
819
                m_signs[2] = m_rayDirectionInverse[2] < 0.0;
656
822
 
657
823
        }
658
824
 
659
 
        
 
825
 
660
826
 
661
827
        virtual bool    process(const btBroadphaseProxy* proxy)
662
828
        {
687
853
                        {
688
854
                                m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
689
855
                                        collisionObject,
690
 
                                                collisionObject->getCollisionShape(),
691
 
                                                collisionObject->getWorldTransform(),
692
 
                                                m_resultCallback);
 
856
                                        collisionObject->getCollisionShape(),
 
857
                                        collisionObject->getWorldTransform(),
 
858
                                        m_resultCallback);
693
859
                        }
694
860
                }
695
861
                return true;
698
864
 
699
865
void    btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
700
866
{
701
 
        BT_PROFILE("rayTest");
 
867
        //BT_PROFILE("rayTest");
702
868
        /// use the broadphase to accelerate the search for objects, based on their aabb
703
869
        /// and for each object with ray-aabb overlap, perform an exact ray test
704
870
        btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
737
903
        {
738
904
                btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
739
905
                btVector3 rayDir = unnormalizedRayDir.normalized();
740
 
                ///what about division by zero? --> just set rayDirection[i] to INF/1e30
741
 
                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
742
 
                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
743
 
                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
 
906
                ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
 
907
                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
 
908
                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
 
909
                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
744
910
                m_signs[0] = m_rayDirectionInverse[0] < 0.0;
745
911
                m_signs[1] = m_rayDirectionInverse[1] < 0.0;
746
912
                m_signs[2] = m_rayDirectionInverse[2] < 0.0;
761
927
                if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
762
928
                        //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
763
929
                        m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
764
 
                                        collisionObject,
765
 
                                                collisionObject->getCollisionShape(),
766
 
                                                collisionObject->getWorldTransform(),
767
 
                                                m_resultCallback,
768
 
                                                m_allowedCcdPenetration);
 
930
                                collisionObject,
 
931
                                collisionObject->getCollisionShape(),
 
932
                                collisionObject->getWorldTransform(),
 
933
                                m_resultCallback,
 
934
                                m_allowedCcdPenetration);
769
935
                }
770
 
                
 
936
 
771
937
                return true;
772
938
        }
773
939
};
782
948
        /// and for each object with ray-aabb overlap, perform an exact ray test
783
949
        /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
784
950
 
785
 
        
 
951
 
786
952
 
787
953
        btTransform     convexFromTrans,convexToTrans;
788
954
        convexFromTrans = convexFromWorld;
825
991
                        {
826
992
                                objectQuerySingle(castShape, convexFromTrans,convexToTrans,
827
993
                                        collisionObject,
828
 
                                                collisionObject->getCollisionShape(),
829
 
                                                collisionObject->getWorldTransform(),
830
 
                                                resultCallback,
831
 
                                                allowedCcdPenetration);
 
994
                                        collisionObject->getCollisionShape(),
 
995
                                        collisionObject->getWorldTransform(),
 
996
                                        resultCallback,
 
997
                                        allowedCcdPenetration);
832
998
                        }
833
999
                }
834
1000
        }
835
1001
#endif //USE_BRUTEFORCE_RAYBROADPHASE
836
1002
}
 
1003
 
 
1004
 
 
1005
 
 
1006
struct btBridgedManifoldResult : public btManifoldResult
 
1007
{
 
1008
 
 
1009
        btCollisionWorld::ContactResultCallback&        m_resultCallback;
 
1010
 
 
1011
        btBridgedManifoldResult( btCollisionObject* obj0,btCollisionObject* obj1,btCollisionWorld::ContactResultCallback& resultCallback )
 
1012
                :btManifoldResult(obj0,obj1),
 
1013
                m_resultCallback(resultCallback)
 
1014
        {
 
1015
        }
 
1016
 
 
1017
        virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
 
1018
        {
 
1019
                bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
 
1020
                btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
 
1021
                btVector3 localA;
 
1022
                btVector3 localB;
 
1023
                if (isSwapped)
 
1024
                {
 
1025
                        localA = m_rootTransB.invXform(pointA );
 
1026
                        localB = m_rootTransA.invXform(pointInWorld);
 
1027
                } else
 
1028
                {
 
1029
                        localA = m_rootTransA.invXform(pointA );
 
1030
                        localB = m_rootTransB.invXform(pointInWorld);
 
1031
                }
 
1032
                
 
1033
                btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
 
1034
                newPt.m_positionWorldOnA = pointA;
 
1035
                newPt.m_positionWorldOnB = pointInWorld;
 
1036
                
 
1037
           //BP mod, store contact triangles.
 
1038
                if (isSwapped)
 
1039
                {
 
1040
                        newPt.m_partId0 = m_partId1;
 
1041
                        newPt.m_partId1 = m_partId0;
 
1042
                        newPt.m_index0  = m_index1;
 
1043
                        newPt.m_index1  = m_index0;
 
1044
                } else
 
1045
                {
 
1046
                        newPt.m_partId0 = m_partId0;
 
1047
                        newPt.m_partId1 = m_partId1;
 
1048
                        newPt.m_index0  = m_index0;
 
1049
                        newPt.m_index1  = m_index1;
 
1050
                }
 
1051
 
 
1052
                //experimental feature info, for per-triangle material etc.
 
1053
                btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
 
1054
                btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
 
1055
                m_resultCallback.addSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
 
1056
 
 
1057
        }
 
1058
        
 
1059
};
 
1060
 
 
1061
 
 
1062
 
 
1063
struct btSingleContactCallback : public btBroadphaseAabbCallback
 
1064
{
 
1065
 
 
1066
        btCollisionObject* m_collisionObject;
 
1067
        btCollisionWorld*       m_world;
 
1068
        btCollisionWorld::ContactResultCallback&        m_resultCallback;
 
1069
        
 
1070
        
 
1071
        btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
 
1072
                :m_collisionObject(collisionObject),
 
1073
                m_world(world),
 
1074
                m_resultCallback(resultCallback)
 
1075
        {
 
1076
        }
 
1077
 
 
1078
        virtual bool    process(const btBroadphaseProxy* proxy)
 
1079
        {
 
1080
                btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
 
1081
                if (collisionObject == m_collisionObject)
 
1082
                        return true;
 
1083
 
 
1084
                //only perform raycast if filterMask matches
 
1085
                if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
 
1086
                {
 
1087
                        btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(m_collisionObject,collisionObject);
 
1088
                        if (algorithm)
 
1089
                        {
 
1090
                                btBridgedManifoldResult contactPointResult(m_collisionObject,collisionObject, m_resultCallback);
 
1091
                                //discrete collision detection query
 
1092
                                algorithm->processCollision(m_collisionObject,collisionObject, m_world->getDispatchInfo(),&contactPointResult);
 
1093
 
 
1094
                                algorithm->~btCollisionAlgorithm();
 
1095
                                m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
 
1096
                        }
 
1097
                }
 
1098
                return true;
 
1099
        }
 
1100
};
 
1101
 
 
1102
 
 
1103
///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
 
1104
///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
 
1105
void    btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
 
1106
{
 
1107
        btVector3 aabbMin,aabbMax;
 
1108
        colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
 
1109
        btSingleContactCallback contactCB(colObj,this,resultCallback);
 
1110
        
 
1111
        m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
 
1112
}
 
1113
 
 
1114
 
 
1115
///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
 
1116
///it reports one or more contact points (including the one with deepest penetration)
 
1117
void    btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
 
1118
{
 
1119
        btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(colObjA,colObjB);
 
1120
        if (algorithm)
 
1121
        {
 
1122
                btBridgedManifoldResult contactPointResult(colObjA,colObjB, resultCallback);
 
1123
                //discrete collision detection query
 
1124
                algorithm->processCollision(colObjA,colObjB, getDispatchInfo(),&contactPointResult);
 
1125
 
 
1126
                algorithm->~btCollisionAlgorithm();
 
1127
                getDispatcher()->freeCollisionAlgorithm(algorithm);
 
1128
        }
 
1129
 
 
1130
}
 
1131
 
 
1132
 
 
1133
 
 
1134
 
 
1135
class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
 
1136
{
 
1137
        btIDebugDraw*   m_debugDrawer;
 
1138
        btVector3       m_color;
 
1139
        btTransform     m_worldTrans;
 
1140
 
 
1141
public:
 
1142
 
 
1143
        DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
 
1144
          m_debugDrawer(debugDrawer),
 
1145
                  m_color(color),
 
1146
                  m_worldTrans(worldTrans)
 
1147
          {
 
1148
          }
 
1149
 
 
1150
          virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
 
1151
          {
 
1152
                  processTriangle(triangle,partId,triangleIndex);
 
1153
          }
 
1154
 
 
1155
          virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
 
1156
          {
 
1157
                  (void)partId;
 
1158
                  (void)triangleIndex;
 
1159
 
 
1160
                  btVector3 wv0,wv1,wv2;
 
1161
                  wv0 = m_worldTrans*triangle[0];
 
1162
                  wv1 = m_worldTrans*triangle[1];
 
1163
                  wv2 = m_worldTrans*triangle[2];
 
1164
                  btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
 
1165
 
 
1166
                  btVector3 normal = (wv1-wv0).cross(wv2-wv0);
 
1167
                  normal.normalize();
 
1168
                  btVector3 normalColor(1,1,0);
 
1169
                  m_debugDrawer->drawLine(center,center+normal,normalColor);
 
1170
 
 
1171
 
 
1172
 
 
1173
                 
 
1174
                  m_debugDrawer->drawLine(wv0,wv1,m_color);
 
1175
                  m_debugDrawer->drawLine(wv1,wv2,m_color);
 
1176
                  m_debugDrawer->drawLine(wv2,wv0,m_color);
 
1177
          }
 
1178
};
 
1179
 
 
1180
 
 
1181
void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
 
1182
{
 
1183
        // Draw a small simplex at the center of the object
 
1184
        getDebugDrawer()->drawTransform(worldTransform,1);
 
1185
 
 
1186
        if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
 
1187
        {
 
1188
                const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
 
1189
                for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
 
1190
                {
 
1191
                        btTransform childTrans = compoundShape->getChildTransform(i);
 
1192
                        const btCollisionShape* colShape = compoundShape->getChildShape(i);
 
1193
                        debugDrawObject(worldTransform*childTrans,colShape,color);
 
1194
                }
 
1195
 
 
1196
        } else
 
1197
        {
 
1198
                switch (shape->getShapeType())
 
1199
                {
 
1200
 
 
1201
                case BOX_SHAPE_PROXYTYPE:
 
1202
                        {
 
1203
                                const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
 
1204
                                btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
 
1205
                                getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
 
1206
                                break;
 
1207
                        }
 
1208
 
 
1209
                case SPHERE_SHAPE_PROXYTYPE:
 
1210
                        {
 
1211
                                const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
 
1212
                                btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
 
1213
 
 
1214
                                getDebugDrawer()->drawSphere(radius, worldTransform, color);
 
1215
                                break;
 
1216
                        }
 
1217
                case MULTI_SPHERE_SHAPE_PROXYTYPE:
 
1218
                        {
 
1219
                                const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
 
1220
 
 
1221
                                btTransform childTransform;
 
1222
                                childTransform.setIdentity();
 
1223
 
 
1224
                                for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
 
1225
                                {
 
1226
                                        childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
 
1227
                                        getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
 
1228
                                }
 
1229
 
 
1230
                                break;
 
1231
                        }
 
1232
                case CAPSULE_SHAPE_PROXYTYPE:
 
1233
                        {
 
1234
                                const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
 
1235
 
 
1236
                                btScalar radius = capsuleShape->getRadius();
 
1237
                                btScalar halfHeight = capsuleShape->getHalfHeight();
 
1238
 
 
1239
                                int upAxis = capsuleShape->getUpAxis();
 
1240
                                getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
 
1241
                                break;
 
1242
                        }
 
1243
                case CONE_SHAPE_PROXYTYPE:
 
1244
                        {
 
1245
                                const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
 
1246
                                btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
 
1247
                                btScalar height = coneShape->getHeight();//+coneShape->getMargin();
 
1248
 
 
1249
                                int upAxis= coneShape->getConeUpIndex();
 
1250
                                getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
 
1251
                                break;
 
1252
 
 
1253
                        }
 
1254
                case CYLINDER_SHAPE_PROXYTYPE:
 
1255
                        {
 
1256
                                const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
 
1257
                                int upAxis = cylinder->getUpAxis();
 
1258
                                btScalar radius = cylinder->getRadius();
 
1259
                                btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
 
1260
                                getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
 
1261
                                break;
 
1262
                        }
 
1263
 
 
1264
                case STATIC_PLANE_PROXYTYPE:
 
1265
                        {
 
1266
                                const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
 
1267
                                btScalar planeConst = staticPlaneShape->getPlaneConstant();
 
1268
                                const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
 
1269
                                getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, 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
 
 
1325
void    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
                                        btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
 
1384
                                        minAabb -= contactThreshold;
 
1385
                                        maxAabb += contactThreshold;
 
1386
 
 
1387
                                        btVector3 minAabb2,maxAabb2;
 
1388
 
 
1389
                                        colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
 
1390
                                        minAabb2 -= contactThreshold;
 
1391
                                        maxAabb2 += contactThreshold;
 
1392
 
 
1393
                                        minAabb.setMin(minAabb2);
 
1394
                                        maxAabb.setMax(maxAabb2);
 
1395
 
 
1396
                                        m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
 
1397
                                }
 
1398
                        }
 
1399
 
 
1400
                }
 
1401
        }
 
1402
}
 
1403
 
 
1404
 
 
1405
void    btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
 
1406
{
 
1407
        int i;
 
1408
        //serialize all collision objects
 
1409
        for (i=0;i<m_collisionObjects.size();i++)
 
1410
        {
 
1411
                btCollisionObject* colObj = m_collisionObjects[i];
 
1412
                if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
 
1413
                {
 
1414
                        colObj->serializeSingleObject(serializer);
 
1415
                }
 
1416
        }
 
1417
 
 
1418
        ///keep track of shapes already serialized
 
1419
        btHashMap<btHashPtr,btCollisionShape*>  serializedShapes;
 
1420
 
 
1421
        for (i=0;i<m_collisionObjects.size();i++)
 
1422
        {
 
1423
                btCollisionObject* colObj = m_collisionObjects[i];
 
1424
                btCollisionShape* shape = colObj->getCollisionShape();
 
1425
 
 
1426
                if (!serializedShapes.find(shape))
 
1427
                {
 
1428
                        serializedShapes.insert(shape,shape);
 
1429
                        shape->serializeSingleShape(serializer);
 
1430
                }
 
1431
        }
 
1432
 
 
1433
}
 
1434
 
 
1435
 
 
1436
void    btCollisionWorld::serialize(btSerializer* serializer)
 
1437
{
 
1438
 
 
1439
        serializer->startSerialization();
 
1440
        
 
1441
        serializeCollisionObjects(serializer);
 
1442
        
 
1443
        serializer->finishSerialization();
 
1444
}
 
1445