Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Apr 21, 2011, 6:58:23 PM (13 years ago)
Author:
rgrieder
Message:

Merged revisions 7978 - 8096 from kicklib to kicklib2.

Location:
code/branches/kicklib2
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • code/branches/kicklib2

  • code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

    r5781 r8284  
    11/*
    22Bullet Continuous Collision Detection and Physics Library
    3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
     3Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
    44
    55This software is provided 'as-is', without any express or implied warranty.
    … …  
    2020#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
    2121#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
     22#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
    2223#include "BulletCollision/CollisionShapes/btCollisionShape.h"
    2324#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
    … …  
    3637#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
    3738
    38 //for debug rendering
    39 #include "BulletCollision/CollisionShapes/btBoxShape.h"
    40 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
    41 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
    42 #include "BulletCollision/CollisionShapes/btConeShape.h"
    43 #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
    44 #include "BulletCollision/CollisionShapes/btCylinderShape.h"
    45 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
    46 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
     39#include "LinearMath/btIDebugDraw.h"
    4740#include "BulletCollision/CollisionShapes/btSphereShape.h"
    48 #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
    49 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
    50 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
    51 #include "LinearMath/btIDebugDraw.h"
    5241
    5342
    … …  
    5645#include "LinearMath/btMotionState.h"
    5746
    58 
     47#include "LinearMath/btSerializer.h"
    5948
    6049
    … …  
    6453m_constraintSolver(constraintSolver),
    6554m_gravity(0,-10,0),
    66 m_localTime(btScalar(1.)/btScalar(60.)),
     55m_localTime(0),
     56m_synchronizeAllMotionStates(false),
    6757m_profileTimings(0)
    6858{
    … …  
    10494void    btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
    10595{
    106 
     96///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
     97///to switch status _after_ adding kinematic objects to the world
     98///fix it for Bullet 3.x release
    10799        for (int i=0;i<m_collisionObjects.size();i++)
    108100        {
    109101                btCollisionObject* colObj = m_collisionObjects[i];
    110102                btRigidBody* body = btRigidBody::upcast(colObj);
    111                 if (body)
    112                 {
    113                                 if (body->getActivationState() != ISLAND_SLEEPING)
    114                                 {
    115                                         if (body->isKinematicObject())
    116                                         {
    117                                                 //to calculate velocities next frame
    118                                                 body->saveKinematicState(timeStep);
    119                                         }
    120                                 }
    121                 }
    122         }
     103                if (body && body->getActivationState() != ISLAND_SLEEPING)
     104                {
     105                        if (body->isKinematicObject())
     106                        {
     107                                //to calculate velocities next frame
     108                                body->saveKinematicState(timeStep);
     109                        }
     110                }
     111        }
     112
    123113}
    124114
    … …  
    127117        BT_PROFILE("debugDrawWorld");
    128118
    129         if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
    130         {
    131                 int numManifolds = getDispatcher()->getNumManifolds();
    132                 btVector3 color(0,0,0);
    133                 for (int i=0;i<numManifolds;i++)
    134                 {
    135                         btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
    136                         //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
    137                         //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
    138 
    139                         int numContacts = contactManifold->getNumContacts();
    140                         for (int j=0;j<numContacts;j++)
    141                         {
    142                                 btManifoldPoint& cp = contactManifold->getContactPoint(j);
    143                                 getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
    144                         }
    145                 }
    146         }
     119        btCollisionWorld::debugDrawWorld();
     120
    147121        bool drawConstraints = false;
    148122        if (getDebugDrawer())
    … …  
    169143                int i;
    170144
    171                 for (  i=0;i<m_collisionObjects.size();i++)
    172                 {
    173                         btCollisionObject* colObj = m_collisionObjects[i];
    174                         if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
    175                         {
    176                                 btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
    177                                 switch(colObj->getActivationState())
    178                                 {
    179                                 case  ACTIVE_TAG:
    180                                         color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
    181                                 case ISLAND_SLEEPING:
    182                                         color =  btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
    183                                 case WANTS_DEACTIVATION:
    184                                         color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
    185                                 case DISABLE_DEACTIVATION:
    186                                         color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
    187                                 case DISABLE_SIMULATION:
    188                                         color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
    189                                 default:
    190                                         {
    191                                                 color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
    192                                         }
    193                                 };
    194 
    195                                 debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
    196                         }
    197                         if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
    198                         {
    199                                 btVector3 minAabb,maxAabb;
    200                                 btVector3 colorvec(1,0,0);
    201                                 colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
    202                                 m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
    203                         }
    204 
    205                 }
    206        
    207145                if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
    208146                {
    … …  
    218156{
    219157        ///@todo: iterate over awake simulation islands!
    220         for ( int i=0;i<m_collisionObjects.size();i++)
    221         {
    222                 btCollisionObject* colObj = m_collisionObjects[i];
    223                
    224                 btRigidBody* body = btRigidBody::upcast(colObj);
    225                 if (body)
    226                 {
    227                         body->clearForces();
    228                 }
     158        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
     159        {
     160                btRigidBody* body = m_nonStaticRigidBodies[i];
     161                //need to check if next line is ok
     162                //it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
     163                body->clearForces();
    229164        }
    230165}       
    … …  
    234169{
    235170        ///@todo: iterate over awake simulation islands!
    236         for ( int i=0;i<m_collisionObjects.size();i++)
    237         {
    238                 btCollisionObject* colObj = m_collisionObjects[i];
    239                
    240                 btRigidBody* body = btRigidBody::upcast(colObj);
    241                 if (body && body->isActive())
     171        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
     172        {
     173                btRigidBody* body = m_nonStaticRigidBodies[i];
     174                if (body->isActive())
    242175                {
    243176                        body->applyGravity();
    … …  
    270203{
    271204        BT_PROFILE("synchronizeMotionStates");
    272         {
    273                 //todo: iterate over awake simulation islands!
     205        if (m_synchronizeAllMotionStates)
     206        {
     207                //iterate  over all collision objects
    274208                for ( int i=0;i<m_collisionObjects.size();i++)
    275209                {
    276210                        btCollisionObject* colObj = m_collisionObjects[i];
    277                        
    278211                        btRigidBody* body = btRigidBody::upcast(colObj);
    279212                        if (body)
    280213                                synchronizeSingleMotionState(body);
    281214                }
    282         }
    283 /*
    284         if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
    285         {
    286                 for ( int i=0;i<this->m_vehicles.size();i++)
    287                 {
    288                         for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
    289                         {
    290                                 //synchronize the wheels with the (interpolated) chassis worldtransform
    291                                 m_vehicles[i]->updateWheelTransform(v,true);
    292                         }
    293                 }
    294         }
    295         */
    296 
    297 
     215        } else
     216        {
     217                //iterate over all active rigid bodies
     218                for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
     219                {
     220                        btRigidBody* body = m_nonStaticRigidBodies[i];
     221                        if (body->isActive())
     222                                synchronizeSingleMotionState(body);
     223                }
     224        }
    298225}
    299226
    … …  
    341268        {
    342269
    343                 saveKinematicState(fixedTimeStep);
    344 
    345                 applyGravity();
    346 
    347270                //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
    348271                int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
    349272
     273                saveKinematicState(fixedTimeStep*clampedSimulationSteps);
     274
     275                applyGravity();
     276
     277               
     278
    350279                for (int i=0;i<clampedSimulationSteps;i++)
    351280                {
    … …  
    354283                }
    355284
    356         }
    357 
    358         synchronizeMotionStates();
     285        } else
     286        {
     287                synchronizeMotionStates();
     288        }
    359289
    360290        clearForces();
    … …  
    372302        BT_PROFILE("internalSingleStepSimulation");
    373303
     304        if(0 != m_internalPreTickCallback) {
     305                (*m_internalPreTickCallback)(this, timeStep);
     306        }       
     307
    374308        ///apply gravity, predict motion
    375309        predictUnconstraintMotion(timeStep);
    … …  
    412346{
    413347        m_gravity = gravity;
    414         for ( int i=0;i<m_collisionObjects.size();i++)
    415         {
    416                 btCollisionObject* colObj = m_collisionObjects[i];
    417                 btRigidBody* body = btRigidBody::upcast(colObj);
    418                 if (body)
     348        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
     349        {
     350                btRigidBody* body = m_nonStaticRigidBodies[i];
     351                if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
    419352                {
    420353                        body->setGravity(gravity);
    … …  
    428361}
    429362
     363void    btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
     364{
     365        btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask);
     366}
     367
     368void    btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
     369{
     370        btRigidBody* body = btRigidBody::upcast(collisionObject);
     371        if (body)
     372                removeRigidBody(body);
     373        else
     374                btCollisionWorld::removeCollisionObject(collisionObject);
     375}
    430376
    431377void    btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
    432378{
    433         removeCollisionObject(body);
    434 }
     379        m_nonStaticRigidBodies.remove(body);
     380        btCollisionWorld::removeCollisionObject(body);
     381}
     382
    435383
    436384void    btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
    437385{
    438         if (!body->isStaticOrKinematicObject())
     386        if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
    439387        {
    440388                body->setGravity(m_gravity);
    … …  
    443391        if (body->getCollisionShape())
    444392        {
     393                if (!body->isStaticObject())
     394                {
     395                        m_nonStaticRigidBodies.push_back(body);
     396                } else
     397                {
     398                        body->setActivationState(ISLAND_SLEEPING);
     399                }
     400
    445401                bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
    446402                short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
    … …  
    453409void    btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
    454410{
    455         if (!body->isStaticOrKinematicObject())
     411        if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
    456412        {
    457413                body->setGravity(m_gravity);
    … …  
    460416        if (body->getCollisionShape())
    461417        {
     418                if (!body->isStaticObject())
     419                {
     420                        m_nonStaticRigidBodies.push_back(body);
     421                }
     422                 else
     423                {
     424                        body->setActivationState(ISLAND_SLEEPING);
     425                }
    462426                addCollisionObject(body,group,mask);
    463427        }
    … …  
    480444        BT_PROFILE("updateActivationState");
    481445
    482         for ( int i=0;i<m_collisionObjects.size();i++)
    483         {
    484                 btCollisionObject* colObj = m_collisionObjects[i];
    485                 btRigidBody* body = btRigidBody::upcast(colObj);
     446        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
     447        {
     448                btRigidBody* body = m_nonStaticRigidBodies[i];
    486449                if (body)
    487450                {
    … …  
    586549                }
    587550};
    588 
    589551
    590552
    … …  
    604566                btStackAlloc*                   m_stackAlloc;
    605567                btDispatcher*                   m_dispatcher;
     568               
     569                btAlignedObjectArray<btCollisionObject*> m_bodies;
     570                btAlignedObjectArray<btPersistentManifold*> m_manifolds;
     571                btAlignedObjectArray<btTypedConstraint*> m_constraints;
     572
    606573
    607574                InplaceSolverIslandCallback(
    … …  
    624591                }
    625592
     593
    626594                InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
    627595                {
    … …  
    664632                                }
    665633
    666                                 ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
    667                                 if (numManifolds + numCurConstraints)
    668                                 {
    669                                         m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
    670                                 }
    671                
    672                         }
     634                                if (m_solverInfo.m_minimumSolverBatchSize<=1)
     635                                {
     636                                        ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
     637                                        if (numManifolds + numCurConstraints)
     638                                        {
     639                                                m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
     640                                        }
     641                                } else
     642                                {
     643                                       
     644                                        for (i=0;i<numBodies;i++)
     645                                                m_bodies.push_back(bodies[i]);
     646                                        for (i=0;i<numManifolds;i++)
     647                                                m_manifolds.push_back(manifolds[i]);
     648                                        for (i=0;i<numCurConstraints;i++)
     649                                                m_constraints.push_back(startConstraint[i]);
     650                                        if ((m_constraints.size()+m_manifolds.size())>m_solverInfo.m_minimumSolverBatchSize)
     651                                        {
     652                                                processConstraints();
     653                                        } else
     654                                        {
     655                                                //printf("deferred\n");
     656                                        }
     657                                }
     658                        }
     659                }
     660                void    processConstraints()
     661                {
     662                        if (m_manifolds.size() + m_constraints.size()>0)
     663                        {
     664                                m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
     665                        }
     666                        m_bodies.resize(0);
     667                        m_manifolds.resize(0);
     668                        m_constraints.resize(0);
     669
    673670                }
    674671
    675672        };
     673
     674       
    676675
    677676        //sorted version of all btTypedConstraint, based on islandId
    … …  
    699698        m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);
    700699
     700        solverCallback.processConstraints();
     701
    701702        m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
    702703}
    … …  
    741742
    742743
    743 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
     744
    744745
    745746class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
    … …  
    754755        btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
    755756          btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
     757                m_me(me),
    756758                m_allowedPenetration(0.0f),
    757                 m_me(me),
    758759                m_pairCache(pairCache),
    759760                m_dispatcher(dispatcher)
    … …  
    829830        BT_PROFILE("integrateTransforms");
    830831        btTransform predictedTrans;
    831         for ( int i=0;i<m_collisionObjects.size();i++)
    832         {
    833                 btCollisionObject* colObj = m_collisionObjects[i];
    834                 btRigidBody* body = btRigidBody::upcast(colObj);
    835                 if (body)
    836                 {
    837                         body->setHitFraction(1.f);
    838 
    839                         if (body->isActive() && (!body->isStaticOrKinematicObject()))
    840                         {
    841                                 body->predictIntegratedTransform(timeStep, predictedTrans);
    842                                 btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
    843 
    844                                 if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
    845                                 {
    846                                         BT_PROFILE("CCD motion clamping");
    847                                         if (body->getCollisionShape()->isConvex())
    848                                         {
    849                                                 gNumClampedCcdMotions++;
    850                                                
    851                                                 btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
    852                                                 btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
    853                                                 btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
    854 
    855                                                 sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
    856                                                 sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
    857 
    858                                                 convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
    859                                                 if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
    860                                                 {
    861                                                         body->setHitFraction(sweepResults.m_closestHitFraction);
    862                                                         body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
    863                                                         body->setHitFraction(0.f);
     832        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
     833        {
     834                btRigidBody* body = m_nonStaticRigidBodies[i];
     835                body->setHitFraction(1.f);
     836
     837                if (body->isActive() && (!body->isStaticOrKinematicObject()))
     838                {
     839                        body->predictIntegratedTransform(timeStep, predictedTrans);
     840                        btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
     841
     842                        if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
     843                        {
     844                                BT_PROFILE("CCD motion clamping");
     845                                if (body->getCollisionShape()->isConvex())
     846                                {
     847                                        gNumClampedCcdMotions++;
     848                                       
     849                                        btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
     850                                        //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
     851                                        btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
     852
     853                                        sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
     854                                        sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
     855
     856                                        convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
     857                                        if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
     858                                        {
     859                                                body->setHitFraction(sweepResults.m_closestHitFraction);
     860                                                body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
     861                                                body->setHitFraction(0.f);
    864862//                                                      printf("clamped integration to hit fraction = %f\n",fraction);
    865                                                 }
    866863                                        }
    867864                                }
    868                                
    869                                 body->proceedToTransform( predictedTrans);
    870                         }
     865                        }
     866                       
     867                        body->proceedToTransform( predictedTrans);
    871868                }
    872869        }
    … …  
    880877{
    881878        BT_PROFILE("predictUnconstraintMotion");
    882         for ( int i=0;i<m_collisionObjects.size();i++)
    883         {
    884                 btCollisionObject* colObj = m_collisionObjects[i];
    885                 btRigidBody* body = btRigidBody::upcast(colObj);
    886                 if (body)
    887                 {
    888                         if (!body->isStaticOrKinematicObject())
    889                         {
    890                                
    891                                 body->integrateVelocities( timeStep);
    892                                 //damping
    893                                 body->applyDamping(timeStep);
    894 
    895                                 body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
    896                         }
     879        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
     880        {
     881                btRigidBody* body = m_nonStaticRigidBodies[i];
     882                if (!body->isStaticOrKinematicObject())
     883                {
     884                        body->integrateVelocities( timeStep);
     885                        //damping
     886                        body->applyDamping(timeStep);
     887
     888                        body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
    897889                }
    898890        }
    … …  
    914906
    915907       
    916 
    917 class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
    918 {
    919         btIDebugDraw*   m_debugDrawer;
    920         btVector3       m_color;
    921         btTransform     m_worldTrans;
    922 
    923 public:
    924 
    925         DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
    926                 m_debugDrawer(debugDrawer),
    927                 m_color(color),
    928                 m_worldTrans(worldTrans)
    929         {
    930         }
    931 
    932         virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
    933         {
    934                 processTriangle(triangle,partId,triangleIndex);
    935         }
    936 
    937         virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
    938         {
    939                 (void)partId;
    940                 (void)triangleIndex;
    941 
    942                 btVector3 wv0,wv1,wv2;
    943                 wv0 = m_worldTrans*triangle[0];
    944                 wv1 = m_worldTrans*triangle[1];
    945                 wv2 = m_worldTrans*triangle[2];
    946                 m_debugDrawer->drawLine(wv0,wv1,m_color);
    947                 m_debugDrawer->drawLine(wv1,wv2,m_color);
    948                 m_debugDrawer->drawLine(wv2,wv0,m_color);
    949         }
    950 };
    951 
    952 void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
    953 {
    954         btVector3 start = transform.getOrigin();
    955 
    956         const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
    957         const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
    958         const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
    959 
    960         // XY
    961         getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
    962         getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
    963         getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
    964         getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
    965 
    966         // XZ
    967         getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
    968         getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
    969         getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
    970         getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
    971 
    972         // YZ
    973         getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
    974         getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
    975         getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
    976         getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
    977 }
    978 
    979 void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
    980 {
    981         // Draw a small simplex at the center of the object
    982         {
    983                 btVector3 start = worldTransform.getOrigin();
    984                 getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
    985                 getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
    986                 getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
    987         }
    988 
    989         if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
    990         {
    991                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
    992                 for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
    993                 {
    994                         btTransform childTrans = compoundShape->getChildTransform(i);
    995                         const btCollisionShape* colShape = compoundShape->getChildShape(i);
    996                         debugDrawObject(worldTransform*childTrans,colShape,color);
    997                 }
    998 
    999         } else
    1000         {
    1001                 switch (shape->getShapeType())
    1002                 {
    1003 
    1004                 case SPHERE_SHAPE_PROXYTYPE:
    1005                         {
    1006                                 const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
    1007                                 btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
    1008                                
    1009                                 debugDrawSphere(radius, worldTransform, color);
    1010                                 break;
    1011                         }
    1012                 case MULTI_SPHERE_SHAPE_PROXYTYPE:
    1013                         {
    1014                                 const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
    1015 
    1016                                 for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
    1017                                 {
    1018                                         btTransform childTransform = worldTransform;
    1019                                         childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
    1020                                         debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
    1021                                 }
    1022 
    1023                                 break;
    1024                         }
    1025                 case CAPSULE_SHAPE_PROXYTYPE:
    1026                         {
    1027                                 const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
    1028 
    1029                                 btScalar radius = capsuleShape->getRadius();
    1030                                 btScalar halfHeight = capsuleShape->getHalfHeight();
    1031                                
    1032                                 int upAxis = capsuleShape->getUpAxis();
    1033 
    1034                                
    1035                                 btVector3 capStart(0.f,0.f,0.f);
    1036                                 capStart[upAxis] = -halfHeight;
    1037 
    1038                                 btVector3 capEnd(0.f,0.f,0.f);
    1039                                 capEnd[upAxis] = halfHeight;
    1040 
    1041                                 // Draw the ends
    1042                                 {
    1043                                        
    1044                                         btTransform childTransform = worldTransform;
    1045                                         childTransform.getOrigin() = worldTransform * capStart;
    1046                                         debugDrawSphere(radius, childTransform, color);
    1047                                 }
    1048 
    1049                                 {
    1050                                         btTransform childTransform = worldTransform;
    1051                                         childTransform.getOrigin() = worldTransform * capEnd;
    1052                                         debugDrawSphere(radius, childTransform, color);
    1053                                 }
    1054 
    1055                                 // Draw some additional lines
    1056                                 btVector3 start = worldTransform.getOrigin();
    1057 
    1058                                
    1059                                 capStart[(upAxis+1)%3] = radius;
    1060                                 capEnd[(upAxis+1)%3] = radius;
    1061                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
    1062                                 capStart[(upAxis+1)%3] = -radius;
    1063                                 capEnd[(upAxis+1)%3] = -radius;
    1064                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
    1065 
    1066                                 capStart[(upAxis+1)%3] = 0.f;
    1067                                 capEnd[(upAxis+1)%3] = 0.f;
    1068 
    1069                                 capStart[(upAxis+2)%3] = radius;
    1070                                 capEnd[(upAxis+2)%3] = radius;
    1071                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
    1072                                 capStart[(upAxis+2)%3] = -radius;
    1073                                 capEnd[(upAxis+2)%3] = -radius;
    1074                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
    1075 
    1076                                
    1077                                 break;
    1078                         }
    1079                 case CONE_SHAPE_PROXYTYPE:
    1080                         {
    1081                                 const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
    1082                                 btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
    1083                                 btScalar height = coneShape->getHeight();//+coneShape->getMargin();
    1084                                 btVector3 start = worldTransform.getOrigin();
    1085 
    1086                                 int upAxis= coneShape->getConeUpIndex();
    1087                                
    1088 
    1089                                 btVector3       offsetHeight(0,0,0);
    1090                                 offsetHeight[upAxis] = height * btScalar(0.5);
    1091                                 btVector3       offsetRadius(0,0,0);
    1092                                 offsetRadius[(upAxis+1)%3] = radius;
    1093                                 btVector3       offset2Radius(0,0,0);
    1094                                 offset2Radius[(upAxis+2)%3] = radius;
    1095 
    1096                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
    1097                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
    1098                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
    1099                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
    1100 
    1101 
    1102 
    1103                                 break;
    1104 
    1105                         }
    1106                 case CYLINDER_SHAPE_PROXYTYPE:
    1107                         {
    1108                                 const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
    1109                                 int upAxis = cylinder->getUpAxis();
    1110                                 btScalar radius = cylinder->getRadius();
    1111                                 btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
    1112                                 btVector3 start = worldTransform.getOrigin();
    1113                                 btVector3       offsetHeight(0,0,0);
    1114                                 offsetHeight[upAxis] = halfHeight;
    1115                                 btVector3       offsetRadius(0,0,0);
    1116                                 offsetRadius[(upAxis+1)%3] = radius;
    1117                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
    1118                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
    1119                                 break;
    1120                         }
    1121 
    1122                         case STATIC_PLANE_PROXYTYPE:
    1123                                 {
    1124                                         const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
    1125                                         btScalar planeConst = staticPlaneShape->getPlaneConstant();
    1126                                         const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
    1127                                         btVector3 planeOrigin = planeNormal * planeConst;
    1128                                         btVector3 vec0,vec1;
    1129                                         btPlaneSpace1(planeNormal,vec0,vec1);
    1130                                         btScalar vecLen = 100.f;
    1131                                         btVector3 pt0 = planeOrigin + vec0*vecLen;
    1132                                         btVector3 pt1 = planeOrigin - vec0*vecLen;
    1133                                         btVector3 pt2 = planeOrigin + vec1*vecLen;
    1134                                         btVector3 pt3 = planeOrigin - vec1*vecLen;
    1135                                         getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
    1136                                         getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
    1137                                         break;
    1138 
    1139                                 }
    1140                 default:
    1141                         {
    1142 
    1143                                 if (shape->isConcave())
    1144                                 {
    1145                                         btConcaveShape* concaveMesh = (btConcaveShape*) shape;
    1146                                        
    1147                                         ///@todo pass camera, for some culling? no -> we are not a graphics lib
    1148                                         btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
    1149                                         btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
    1150 
    1151                                         DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
    1152                                         concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
    1153 
    1154                                 }
    1155 
    1156                                 if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
    1157                                 {
    1158                                         btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
    1159                                         //todo: pass camera for some culling                   
    1160                                         btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
    1161                                         btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
    1162                                         //DebugDrawcallback drawCallback;
    1163                                         DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
    1164                                         convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
    1165                                 }
    1166 
    1167 
    1168                                 /// for polyhedral shapes
    1169                                 if (shape->isPolyhedral())
    1170                                 {
    1171                                         btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
    1172 
    1173                                         int i;
    1174                                         for (i=0;i<polyshape->getNumEdges();i++)
    1175                                         {
    1176                                                 btVector3 a,b;
    1177                                                 polyshape->getEdge(i,a,b);
    1178                                                 btVector3 wa = worldTransform * a;
    1179                                                 btVector3 wb = worldTransform * b;
    1180                                                 getDebugDrawer()->drawLine(wa,wb,color);
    1181 
    1182                                         }
    1183 
    1184                                        
    1185                                 }
    1186                         }
    1187                 }
    1188         }
    1189 }
    1190 
    1191908
    1192909void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
    … …  
    13501067                                if(drawLimits)
    13511068                                {
    1352                                         btTransform tr = pSlider->getCalculatedTransformA();
     1069                                        btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
    13531070                                        btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
    13541071                                        btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
    … …  
    13671084        }
    13681085        return;
    1369 } // btDiscreteDynamicsWorld::debugDrawConstraint()
     1086}
    13701087
    13711088
    … …  
    14031120
    14041121
     1122
     1123void    btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
     1124{
     1125        int i;
     1126        //serialize all collision objects
     1127        for (i=0;i<m_collisionObjects.size();i++)
     1128        {
     1129                btCollisionObject* colObj = m_collisionObjects[i];
     1130                if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
     1131                {
     1132                        int len = colObj->calculateSerializeBufferSize();
     1133                        btChunk* chunk = serializer->allocate(len,1);
     1134                        const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
     1135                        serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj);
     1136                }
     1137        }
     1138
     1139        for (i=0;i<m_constraints.size();i++)
     1140        {
     1141                btTypedConstraint* constraint = m_constraints[i];
     1142                int size = constraint->calculateSerializeBufferSize();
     1143                btChunk* chunk = serializer->allocate(size,1);
     1144                const char* structType = constraint->serialize(chunk->m_oldPtr,serializer);
     1145                serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint);
     1146        }
     1147}
     1148
     1149
     1150void    btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
     1151{
     1152
     1153        serializer->startSerialization();
     1154
     1155        serializeRigidBodies(serializer);
     1156
     1157        serializeCollisionObjects(serializer);
     1158
     1159        serializer->finishSerialization();
     1160}
     1161
Note: See TracChangeset for help on using the changeset viewer.