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:
11 edited

Legend:

Unmodified
Added
Removed

  • code/branches/kicklib2

  • code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp

    r5781 r8284  
    4444#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
    4545#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
    46 #include "LinearMath/btStackAlloc.h"
     46
    4747
    4848/*
    … …  
    182182{
    183183        //capsule is convex hull of 2 spheres, so use btMultiSphereShape
    184         btVector3 inertiaHalfExtents(radius,height,radius);
     184       
    185185        const int numSpheres = 2;
    186186        btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
    187187        btScalar radi[numSpheres] = {radius,radius};
    188188        void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
    189         return (plCollisionShapeHandle) new (mem)btMultiSphereShape(inertiaHalfExtents,positions,radi,numSpheres);
     189        return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres);
    190190}
    191191plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
    … …  
    294294        worldTrans.setRotation(orn);
    295295        body->setWorldTransform(worldTrans);
     296}
     297
     298void    plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
     299{
     300        btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
     301        btAssert(body);
     302        btTransform& worldTrans = body->getWorldTransform();
     303        worldTrans.setFromOpenGLMatrix(matrix);
    296304}
    297305
    … …  
    366374        btGjkPairDetector::ClosestPointInput input;
    367375       
    368         btStackAlloc gStackAlloc(1024*1024*2);
    369  
    370         input.m_stackAlloc = &gStackAlloc;
    371        
     376               
    372377        btTransform tr;
    373378        tr.setIdentity();

  • code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btActionInterface.h

    r5781 r8284  
    2121
    2222#include "LinearMath/btScalar.h"
     23#include "btRigidBody.h"
    2324
    2425///Basic interface to allow actions such as vehicles and characters to be updated inside a btDynamicsWorld
    2526class btActionInterface
    2627{
    27         public:
     28protected:
     29
     30        static btRigidBody& getFixedBody();
     31       
     32       
     33public:
    2834
    2935        virtual ~btActionInterface()
    … …  
    3844
    3945#endif //_BT_ACTION_INTERFACE_H
     46

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

    r5781 r8284  
    4949        startProfiling(timeStep);
    5050       
     51        if(0 != m_internalPreTickCallback) {
     52                (*m_internalPreTickCallback)(this, timeStep);
     53        }
     54
    5155
    5256        ///update aabbs information

  • 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

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

    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.
    … …  
    13133. This notice may not be removed or altered from any source distribution.
    1414*/
     15
    1516
    1617#ifndef BT_DISCRETE_DYNAMICS_WORLD_H
    … …  
    4243        btAlignedObjectArray<btTypedConstraint*> m_constraints;
    4344
     45        btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies;
     46
    4447        btVector3       m_gravity;
    4548
    … …  
    5053        bool    m_ownsIslandManager;
    5154        bool    m_ownsConstraintSolver;
     55        bool    m_synchronizeAllMotionStates;
    5256
    5357        btAlignedObjectArray<btActionInterface*>        m_actions;
    … …  
    7478        virtual void    saveKinematicState(btScalar timeStep);
    7579
    76         void    debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
    77 
     80        void    serializeRigidBodies(btSerializer* serializer);
    7881
    7982public:
    … …  
    121124        virtual btVector3 getGravity () const;
    122125
     126        virtual void    addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
     127
    123128        virtual void    addRigidBody(btRigidBody* body);
    124129
    … …  
    127132        virtual void    removeRigidBody(btRigidBody* body);
    128133
    129         void    debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
     134        ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
     135        virtual void    removeCollisionObject(btCollisionObject* collisionObject);
     136
    130137
    131138        void    debugDrawConstraint(btTypedConstraint* constraint);
    … …  
    175182        virtual void    removeCharacter(btActionInterface* character);
    176183
     184        void    setSynchronizeAllMotionStates(bool synchronizeAll)
     185        {
     186                m_synchronizeAllMotionStates = synchronizeAll;
     187        }
     188        bool getSynchronizeAllMotionStates() const
     189        {
     190                return m_synchronizeAllMotionStates;
     191        }
     192
     193        ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo)
     194        virtual void    serialize(btSerializer* serializer);
     195
    177196};
    178197

  • code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h

    r5781 r8284  
    4242protected:
    4343                btInternalTickCallback m_internalTickCallback;
     44                btInternalTickCallback m_internalPreTickCallback;
    4445                void*   m_worldUserInfo;
    4546
    … …  
    5051
    5152                btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
    52                 :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0)
     53                :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0)
    5354                {
    5455                }
    … …  
    103104
    104105                /// Set the callback for when an internal tick (simulation substep) happens, optional user info
    105                 void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0)
     106                void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0,bool isPreTick=false)
    106107                {
    107                         m_internalTickCallback = cb;
     108                        if (isPreTick)
     109                        {
     110                                m_internalPreTickCallback = cb;
     111                        } else
     112                        {
     113                                m_internalTickCallback = cb;
     114                        }
    108115                        m_worldUserInfo = worldUserInfo;
    109116                }

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

    r5781 r8284  
    2020#include "LinearMath/btMotionState.h"
    2121#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
     22#include "LinearMath/btSerializer.h"
    2223
    2324//'temporarily' global variables
    … …  
    4546        m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
    4647        m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
    47         m_angularFactor = btScalar(1.);
     48        m_angularFactor.setValue(1,1,1);
     49        m_linearFactor.setValue(1,1,1);
    4850        m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
    4951        m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
    … …  
    8688        updateInertiaTensor();
    8789
     90        m_rigidbodyFlags = 0;
     91
     92
     93        m_deltaLinearVelocity.setZero();
     94        m_deltaAngularVelocity.setZero();
     95        m_invMass = m_inverseMass*m_linearFactor;
     96        m_pushVelocity.setZero();
     97        m_turnVelocity.setZero();
     98
     99       
     100
    88101}
    89102
    … …  
    136149void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
    137150{
    138         m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
    139         m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
     151        m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
     152        m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
    140153}
    141154
    … …  
    227240                m_inverseMass = btScalar(1.0) / mass;
    228241        }
     242
     243        //Fg = m * a
     244        m_gravity = mass * m_gravity_acceleration;
    229245       
    230246        m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
    … …  
    232248                                   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
    233249
     250        m_invMass = m_linearFactor*m_inverseMass;
    234251}
    235252
    … …  
    301318}
    302319
     320void    btRigidBody::internalWritebackVelocity(btScalar timeStep)
     321{
     322    (void) timeStep;
     323        if (m_inverseMass)
     324        {
     325                setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
     326                setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
     327               
     328                //correct the position/orientation based on push/turn recovery
     329                btTransform newTransform;
     330                btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
     331                setWorldTransform(newTransform);
     332                //m_originalBody->setCompanionId(-1);
     333        }
     334//      m_deltaLinearVelocity.setZero();
     335//      m_deltaAngularVelocity .setZero();
     336//      m_pushVelocity.setZero();
     337//      m_turnVelocity.setZero();
     338}
     339
     340
     341
    303342void btRigidBody::addConstraintRef(btTypedConstraint* c)
    304343{
    … …  
    315354        m_checkCollideWith = m_constraintRefs.size() > 0;
    316355}
     356
     357int     btRigidBody::calculateSerializeBufferSize()     const
     358{
     359        int sz = sizeof(btRigidBodyData);
     360        return sz;
     361}
     362
     363        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     364const char*     btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
     365{
     366        btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
     367
     368        btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
     369
     370        m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
     371        m_linearVelocity.serialize(rbd->m_linearVelocity);
     372        m_angularVelocity.serialize(rbd->m_angularVelocity);
     373        rbd->m_inverseMass = m_inverseMass;
     374        m_angularFactor.serialize(rbd->m_angularFactor);
     375        m_linearFactor.serialize(rbd->m_linearFactor);
     376        m_gravity.serialize(rbd->m_gravity);
     377        m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
     378        m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
     379        m_totalForce.serialize(rbd->m_totalForce);
     380        m_totalTorque.serialize(rbd->m_totalTorque);
     381        rbd->m_linearDamping = m_linearDamping;
     382        rbd->m_angularDamping = m_angularDamping;
     383        rbd->m_additionalDamping = m_additionalDamping;
     384        rbd->m_additionalDampingFactor = m_additionalDampingFactor;
     385        rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
     386        rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
     387        rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
     388        rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
     389        rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
     390
     391        return btRigidBodyDataName;
     392}
     393
     394
     395
     396void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
     397{
     398        btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
     399        const char* structType = serialize(chunk->m_oldPtr, serializer);
     400        serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
     401}
     402
     403

  • code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.h

    r5781 r8284  
    2929extern btScalar gDeactivationTime;
    3030extern bool gDisableDeactivation;
     31
     32#ifdef BT_USE_DOUBLE_PRECISION
     33#define btRigidBodyData btRigidBodyDoubleData
     34#define btRigidBodyDataName     "btRigidBodyDoubleData"
     35#else
     36#define btRigidBodyData btRigidBodyFloatData
     37#define btRigidBodyDataName     "btRigidBodyFloatData"
     38#endif //BT_USE_DOUBLE_PRECISION
     39
     40
     41enum    btRigidBodyFlags
     42{
     43        BT_DISABLE_WORLD_GRAVITY = 1
     44};
    3145
    3246
    … …  
    4660        btVector3               m_angularVelocity;
    4761        btScalar                m_inverseMass;
    48         btScalar                m_angularFactor;
     62        btVector3               m_linearFactor;
    4963
    5064        btVector3               m_gravity;     
    … …  
    7286        //keep track of typed constraints referencing this rigid body
    7387        btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
     88
     89        int                             m_rigidbodyFlags;
     90       
     91        int                             m_debugBodyId;
     92
     93
     94protected:
     95
     96        ATTRIBUTE_ALIGNED64(btVector3           m_deltaLinearVelocity);
     97        btVector3               m_deltaAngularVelocity;
     98        btVector3               m_angularFactor;
     99        btVector3               m_invMass;
     100        btVector3               m_pushVelocity;
     101        btVector3               m_turnVelocity;
     102
    74103
    75104public:
    … …  
    111140                btScalar                        m_additionalAngularDampingFactor;
    112141
    113                
    114142                btRigidBodyConstructionInfo(    btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
    115143                m_mass(mass),
    … …  
    161189        static const btRigidBody*       upcast(const btCollisionObject* colObj)
    162190        {
    163                 if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
     191                if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
    164192                        return (const btRigidBody*)colObj;
    165193                return 0;
    … …  
    167195        static btRigidBody*     upcast(btCollisionObject* colObj)
    168196        {
    169                 if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
     197                if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
    170198                        return (btRigidBody*)colObj;
    171199                return 0;
    … …  
    220248        void                    setMassProps(btScalar mass, const btVector3& inertia);
    221249       
     250        const btVector3& getLinearFactor() const
     251        {
     252                return m_linearFactor;
     253        }
     254        void setLinearFactor(const btVector3& linearFactor)
     255        {
     256                m_linearFactor = linearFactor;
     257                m_invMass = m_linearFactor*m_inverseMass;
     258        }
    222259        btScalar                getInvMass() const { return m_inverseMass; }
    223260        const btMatrix3x3& getInvInertiaTensorWorld() const {
    … …  
    231268        void                    applyCentralForce(const btVector3& force)
    232269        {
    233                 m_totalForce += force;
     270                m_totalForce += force*m_linearFactor;
    234271        }
    235272
    … …  
    262299        void    applyTorque(const btVector3& torque)
    263300        {
    264                 m_totalTorque += torque;
     301                m_totalTorque += torque*m_angularFactor;
    265302        }
    266303       
    … …  
    268305        {
    269306                applyCentralForce(force);
    270                 applyTorque(rel_pos.cross(force)*m_angularFactor);
     307                applyTorque(rel_pos.cross(force*m_linearFactor));
    271308        }
    272309       
    273310        void applyCentralImpulse(const btVector3& impulse)
    274311        {
    275                 m_linearVelocity += impulse * m_inverseMass;
     312                m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
    276313        }
    277314       
    278315        void applyTorqueImpulse(const btVector3& torque)
    279316        {
    280                         m_angularVelocity += m_invInertiaTensorWorld * torque;
     317                        m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
    281318        }
    282319       
    … …  
    288325                        if (m_angularFactor)
    289326                        {
    290                                 applyTorqueImpulse(rel_pos.cross(impulse)*m_angularFactor);
     327                                applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
    291328                        }
    292329                }
    293330        }
    294331
    295         //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
    296         SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
    297         {
    298                 if (m_inverseMass != btScalar(0.))
    299                 {
    300                         m_linearVelocity += linearComponent*impulseMagnitude;
    301                         if (m_angularFactor)
    302                         {
    303                                 m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor;
    304                         }
    305                 }
    306         }
    307        
    308332        void clearForces()
    309333        {
    … …  
    451475        int     m_frictionSolverType;
    452476
     477        void    setAngularFactor(const btVector3& angFac)
     478        {
     479                m_angularFactor = angFac;
     480        }
     481
    453482        void    setAngularFactor(btScalar angFac)
    454483        {
    455                 m_angularFactor = angFac;
    456         }
    457         btScalar        getAngularFactor() const
     484                m_angularFactor.setValue(angFac,angFac,angFac);
     485        }
     486        const btVector3&        getAngularFactor() const
    458487        {
    459488                return m_angularFactor;
    … …  
    481510        }
    482511
    483         int     m_debugBodyId;
     512        void    setFlags(int flags)
     513        {
     514                m_rigidbodyFlags = flags;
     515        }
     516
     517        int getFlags() const
     518        {
     519                return m_rigidbodyFlags;
     520        }
     521
     522        const btVector3& getDeltaLinearVelocity() const
     523        {
     524                return m_deltaLinearVelocity;
     525        }
     526
     527        const btVector3& getDeltaAngularVelocity() const
     528        {
     529                return m_deltaAngularVelocity;
     530        }
     531
     532        const btVector3& getPushVelocity() const
     533        {
     534                return m_pushVelocity;
     535        }
     536
     537        const btVector3& getTurnVelocity() const
     538        {
     539                return m_turnVelocity;
     540        }
     541
     542
     543        ////////////////////////////////////////////////
     544        ///some internal methods, don't use them
     545               
     546        btVector3& internalGetDeltaLinearVelocity()
     547        {
     548                return m_deltaLinearVelocity;
     549        }
     550
     551        btVector3& internalGetDeltaAngularVelocity()
     552        {
     553                return m_deltaAngularVelocity;
     554        }
     555
     556        const btVector3& internalGetAngularFactor() const
     557        {
     558                return m_angularFactor;
     559        }
     560
     561        const btVector3& internalGetInvMass() const
     562        {
     563                return m_invMass;
     564        }
     565       
     566        btVector3& internalGetPushVelocity()
     567        {
     568                return m_pushVelocity;
     569        }
     570
     571        btVector3& internalGetTurnVelocity()
     572        {
     573                return m_turnVelocity;
     574        }
     575
     576        SIMD_FORCE_INLINE void  internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
     577        {
     578                velocity = getLinearVelocity()+m_deltaLinearVelocity + (getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
     579        }
     580
     581        SIMD_FORCE_INLINE void  internalGetAngularVelocity(btVector3& angVel) const
     582        {
     583                angVel = getAngularVelocity()+m_deltaAngularVelocity;
     584        }
     585
     586
     587        //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
     588        SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
     589        {
     590                if (m_inverseMass)
     591                {
     592                        m_deltaLinearVelocity += linearComponent*impulseMagnitude;
     593                        m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
     594                }
     595        }
     596
     597        SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
     598        {
     599                if (m_inverseMass)
     600                {
     601                        m_pushVelocity += linearComponent*impulseMagnitude;
     602                        m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
     603                }
     604        }
     605       
     606        void    internalWritebackVelocity()
     607        {
     608                if (m_inverseMass)
     609                {
     610                        setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
     611                        setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
     612                        //m_deltaLinearVelocity.setZero();
     613                        //m_deltaAngularVelocity .setZero();
     614                        //m_originalBody->setCompanionId(-1);
     615                }
     616        }
     617
     618
     619        void    internalWritebackVelocity(btScalar timeStep);
     620       
     621
     622        ///////////////////////////////////////////////
     623
     624        virtual int     calculateSerializeBufferSize()  const;
     625
     626        ///fills the dataBuffer and returns the struct name (and 0 on failure)
     627        virtual const char*     serialize(void* dataBuffer,  class btSerializer* serializer) const;
     628
     629        virtual void serializeSingleObject(class btSerializer* serializer) const;
     630
    484631};
    485632
     633//@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData
     634///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
     635struct  btRigidBodyFloatData
     636{
     637        btCollisionObjectFloatData      m_collisionObjectData;
     638        btMatrix3x3FloatData            m_invInertiaTensorWorld;
     639        btVector3FloatData              m_linearVelocity;
     640        btVector3FloatData              m_angularVelocity;
     641        btVector3FloatData              m_angularFactor;
     642        btVector3FloatData              m_linearFactor;
     643        btVector3FloatData              m_gravity;     
     644        btVector3FloatData              m_gravity_acceleration;
     645        btVector3FloatData              m_invInertiaLocal;
     646        btVector3FloatData              m_totalForce;
     647        btVector3FloatData              m_totalTorque;
     648        float                                   m_inverseMass;
     649        float                                   m_linearDamping;
     650        float                                   m_angularDamping;
     651        float                                   m_additionalDampingFactor;
     652        float                                   m_additionalLinearDampingThresholdSqr;
     653        float                                   m_additionalAngularDampingThresholdSqr;
     654        float                                   m_additionalAngularDampingFactor;
     655        float                                   m_linearSleepingThreshold;
     656        float                                   m_angularSleepingThreshold;
     657        int                                             m_additionalDamping;
     658};
     659
     660///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
     661struct  btRigidBodyDoubleData
     662{
     663        btCollisionObjectDoubleData     m_collisionObjectData;
     664        btMatrix3x3DoubleData           m_invInertiaTensorWorld;
     665        btVector3DoubleData             m_linearVelocity;
     666        btVector3DoubleData             m_angularVelocity;
     667        btVector3DoubleData             m_angularFactor;
     668        btVector3DoubleData             m_linearFactor;
     669        btVector3DoubleData             m_gravity;     
     670        btVector3DoubleData             m_gravity_acceleration;
     671        btVector3DoubleData             m_invInertiaLocal;
     672        btVector3DoubleData             m_totalForce;
     673        btVector3DoubleData             m_totalTorque;
     674        double                                  m_inverseMass;
     675        double                                  m_linearDamping;
     676        double                                  m_angularDamping;
     677        double                                  m_additionalDampingFactor;
     678        double                                  m_additionalLinearDampingThresholdSqr;
     679        double                                  m_additionalAngularDampingThresholdSqr;
     680        double                                  m_additionalAngularDampingFactor;
     681        double                                  m_linearSleepingThreshold;
     682        double                                  m_angularSleepingThreshold;
     683        int                                             m_additionalDamping;
     684        char    m_padding[4];
     685};
     686
    486687
    487688

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

    r5781 r8284  
    133133void    btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
    134134{
    135         removeCollisionObject(body);
    136 }
     135        btCollisionWorld::removeCollisionObject(body);
     136}
     137
     138void    btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
     139{
     140        btRigidBody* body = btRigidBody::upcast(collisionObject);
     141        if (body)
     142                removeRigidBody(body);
     143        else
     144                btCollisionWorld::removeCollisionObject(collisionObject);
     145}
     146
    137147
    138148void    btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)

  • code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h

    r5781 r8284  
    5858
    5959        virtual void    removeRigidBody(btRigidBody* body);
     60
     61        ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
     62        virtual void    removeCollisionObject(btCollisionObject* collisionObject);
    6063       
    6164        virtual void    updateAabbs();
Note: See TracChangeset for help on using the changeset viewer.