Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Apr 8, 2009, 12:58:47 AM (16 years ago)
Author:
dafrick
Message:

Reverted to revision 2906 (because I'm too stupid to merge correctly, 2nd try will follow shortly. ;))

Location:
code/branches/questsystem5
Files:
1 deleted
112 edited

Legend:

Unmodified
Added
Removed

  • code/branches/questsystem5

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp

    r2907 r2908  
    2020#include "btAxisSweep3.h"
    2121
     22#include <assert.h>
    2223
    2324btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h

    r2907 r2908  
    4343public:
    4444       
    45  BT_DECLARE_ALIGNED_ALLOCATOR();
    4645
    4746        class Edge
    … …  
    140139        SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
    141140
    142         virtual void resetPool(btDispatcher* dispatcher);
     141        void resetPool();
    143142
    144143        void    processAllOverlappingPairs(btOverlapCallback* callback);
    … …  
    222221
    223222        if (checkCardinality)
    224                 btAssert(numEdges == m_numHandles*2+1);
     223                assert(numEdges == m_numHandles*2+1);
    225224}
    226225#endif //DEBUG_BROADPHASE
    … …  
    348347        }
    349348
    350         //btAssert(bounds.HasVolume());
     349        //assert(bounds.HasVolume());
    351350
    352351        // init bounds
    … …  
    454453BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
    455454{
    456         btAssert(m_firstFreeHandle);
     455        assert(m_firstFreeHandle);
    457456
    458457        BP_FP_INT_TYPE handle = m_firstFreeHandle;
    … …  
    466465void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
    467466{
    468         btAssert(handle > 0 && handle < m_maxHandles);
     467        assert(handle > 0 && handle < m_maxHandles);
    469468
    470469        getHandle(handle)->SetNextFree(m_firstFreeHandle);
    … …  
    589588
    590589template <typename BP_FP_INT_TYPE>
    591 void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* dispatcher)
     590void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool()
    592591{
    593592        if (m_numHandles == 0)
    … …  
    643642                        if (!isDuplicate)
    644643                        {
    645                                 ///important to use an AABB test that is consistent with the broadphase
    646644                                bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
    647645
    … …  
    689687        }
    690688
     689
     690
     691       
     692
    691693}
    692694
    … …  
    729731void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
    730732{
    731 //      btAssert(bounds.IsFinite());
    732         //btAssert(bounds.HasVolume());
     733//      assert(bounds.IsFinite());
     734        //assert(bounds.HasVolume());
    733735
    734736        Handle* pHandle = getHandle(handle);

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h

    r2907 r2908  
    6565        virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0;
    6666
    67         ///reset broadphase internal structures, to ensure determinism/reproducability
    68         virtual void resetPool(btDispatcher* dispatcher) {};
    69 
    7067        virtual void    printStats() = 0;
    7168

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h

    r2907 r2908  
    7070
    7171        SOFTBODY_SHAPE_PROXYTYPE,
    72         HFFLUID_SHAPE_PROXYTYPE,
    73         HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE,
     72
    7473        INVALID_SHAPE_PROXYTYPE,
    7574
    … …  
    189188
    190189                //keep them sorted, so the std::set operations work
    191                 if (proxy0.m_uniqueId < proxy1.m_uniqueId)
     190                if (&proxy0 < &proxy1)
    192191        {
    193192            m_pProxy0 = &proxy0;
    … …  
    230229                bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b )
    231230                {
    232                         const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1;
    233                         const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1;
    234                         const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1;
    235                         const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1;
    236 
    237                          return uidA0 > uidB0 ||
    238                                 (a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) ||
     231                         return a.m_pProxy0 > b.m_pProxy0 ||
     232                                (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 > b.m_pProxy1) ||
    239233                                (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm);
    240234                }

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp

    r2907 r2908  
    394394}
    395395
    396 #if 0
     396//
    397397static DBVT_INLINE btDbvtNode*  walkup(btDbvtNode* n,int count)
    398398{
    … …  
    400400        return(n);
    401401}
    402 #endif
    403402
    404403//
    … …  
    425424void                    btDbvt::clear()
    426425{
    427         if(m_root)     
    428                 recursedeletenode(this,m_root);
     426        if(m_root)      recursedeletenode(this,m_root);
    429427        btAlignedFree(m_free);
    430428        m_free=0;
    431         m_lkhd          =       -1;
    432         m_stkStack.clear();
    433         m_opath         =       0;
    434        
    435429}
    436430

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h

    r2907 r2908  
    5858
    5959//SSE gives errors on a MSVC 7.1
    60 #ifdef BT_USE_SSE
     60#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
    6161#define DBVT_SELECT_IMPL                DBVT_IMPL_SSE
    6262#define DBVT_MERGE_IMPL                 DBVT_IMPL_SSE
    … …  
    8383#define DBVT_PREFIX                                     template <typename T>
    8484#define DBVT_IPOLICY                            T& policy
    85 #define DBVT_CHECKTYPE                          static const ICollide&  typechecker=*(T*)1;(void)typechecker;
     85#define DBVT_CHECKTYPE                          static const ICollide&  typechecker=*(T*)0;
    8686#else
    8787#define DBVT_VIRTUAL_DTOR(a)            virtual ~a() {}
    … …  
    147147        DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
    148148                const btDbvtAabbMm& b);
    149        
     149        DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
     150                const btDbvtAabbMm& b,
     151                const btTransform& xform);
    150152        DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
    151153                const btVector3& b);
    … …  
    303305                  const btDbvtNode* root1,
    304306                  DBVT_IPOLICY);
    305 #if 0
     307
    306308        DBVT_PREFIX
    307309                void            collideTT(      const btDbvtNode* root0,
    … …  
    315317                const btTransform& xform1,
    316318                DBVT_IPOLICY);
    317 #endif
    318 
    319319        DBVT_PREFIX
    320320                void            collideTV(      const btDbvtNode* root,
    … …  
    531531}
    532532
    533 
     533//
     534DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
     535                                                                  const btDbvtAabbMm& b,
     536                                                                  const btTransform& xform)
     537{
     538        const btVector3         d0=xform*b.Center()-a.Center();
     539        const btVector3         d1=d0*xform.getBasis();
     540        btScalar                        s0[2]={0,0};
     541        btScalar                        s1[2]={dot(xform.getOrigin(),d0),s1[0]};
     542        a.AddSpan(d0,s0[0],s0[1]);
     543        b.AddSpan(d1,s1[0],s1[1]);
     544        if(s0[0]>(s1[1])) return(false);
     545        if(s0[1]<(s1[0])) return(false);
     546        return(true);
     547}
    534548
    535549//
    … …  
    835849}
    836850
    837 #if 0
     851
    838852//
    839853DBVT_PREFIX
    … …  
    891905                }
    892906}
     907
    893908//
    894909DBVT_PREFIX
    … …  
    902917        collideTT(root0,root1,xform,policy);
    903918}
    904 #endif
    905919
    906920//

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

    r2907 r2908  
    101101                        btDbvtProxy*    pb=(btDbvtProxy*)nb->data;
    102102#if DBVT_BP_SORTPAIRS
    103                         if(pa->m_uniqueId>pb->m_uniqueId)
    104                                 btSwap(pa,pb);
     103                        if(pa>pb) btSwap(pa,pb);
    105104#endif
    106105                        pbp->m_paircache->addOverlappingPair(pa,pb);
    … …  
    134133        m_updates_done          =       0;
    135134        m_updates_ratio         =       0;
    136         m_paircache                     =       paircache? paircache    : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
     135        m_paircache                     =       paircache?
     136paircache       :
     137        new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
    137138        m_gid                           =       0;
    138139        m_pid                           =       0;
    … …  
    210211}
    211212
    212 struct  BroadphaseRayTester : btDbvt::ICollide
    213 {
    214         btBroadphaseRayCallback& m_rayCallback;
    215         BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
    216                 :m_rayCallback(orgCallback)
    217         {
    218         }
    219         void                                    Process(const btDbvtNode* leaf)
    220         {
    221                 btDbvtProxy*    proxy=(btDbvtProxy*)leaf->data;
    222                 m_rayCallback.process(proxy);
    223         }
    224 };     
    225 
    226213void    btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
    227214{
     215
     216        struct  BroadphaseRayTester : btDbvt::ICollide
     217        {
     218                btBroadphaseRayCallback& m_rayCallback;
     219                BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
     220                        :m_rayCallback(orgCallback)
     221                {
     222                }
     223                void                                    Process(const btDbvtNode* leaf)
     224                {
     225                        btDbvtProxy*    proxy=(btDbvtProxy*)leaf->data;
     226                        m_rayCallback.process(proxy);
     227                }
     228        };     
     229
    228230        BroadphaseRayTester callback(rayCallback);
    229231
    … …  
    340342        }
    341343#endif
    342 
    343         performDeferredRemoval(dispatcher);
    344 
    345 }
    346 
    347 void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
    348 {
    349 
    350         if (m_paircache->hasDeferredRemoval())
    351         {
    352 
    353                 btBroadphasePairArray&  overlappingPairArray = m_paircache->getOverlappingPairArray();
    354 
    355                 //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
    356                 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
    357 
    358                 int invalidPair = 0;
    359 
    360                
    361                 int i;
    362 
    363                 btBroadphasePair previousPair;
    364                 previousPair.m_pProxy0 = 0;
    365                 previousPair.m_pProxy1 = 0;
    366                 previousPair.m_algorithm = 0;
    367                
    368                
    369                 for (i=0;i<overlappingPairArray.size();i++)
    370                 {
    371                
    372                         btBroadphasePair& pair = overlappingPairArray[i];
    373 
    374                         bool isDuplicate = (pair == previousPair);
    375 
    376                         previousPair = pair;
    377 
    378                         bool needsRemoval = false;
    379 
    380                         if (!isDuplicate)
    381                         {
    382                                 //important to perform AABB check that is consistent with the broadphase
    383                                 btDbvtProxy*            pa=(btDbvtProxy*)pair.m_pProxy0;
    384                                 btDbvtProxy*            pb=(btDbvtProxy*)pair.m_pProxy1;
    385                                 bool hasOverlap = Intersect(pa->leaf->volume,pb->leaf->volume);
    386 
    387                                 if (hasOverlap)
    388                                 {
    389                                         needsRemoval = false;
    390                                 } else
    391                                 {
    392                                         needsRemoval = true;
    393                                 }
    394                         } else
    395                         {
    396                                 //remove duplicate
    397                                 needsRemoval = true;
    398                                 //should have no algorithm
    399                                 btAssert(!pair.m_algorithm);
    400                         }
    401                        
    402                         if (needsRemoval)
    403                         {
    404                                 m_paircache->cleanOverlappingPair(pair,dispatcher);
    405 
    406                                 pair.m_pProxy0 = 0;
    407                                 pair.m_pProxy1 = 0;
    408                                 invalidPair++;
    409                         }
    410                        
    411                 }
    412 
    413                 //perform a sort, to sort 'invalid' pairs to the end
    414                 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
    415                 overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
    416         }
    417344}
    418345
    … …  
    420347void                                                    btDbvtBroadphase::collide(btDispatcher* dispatcher)
    421348{
    422         /*printf("---------------------------------------------------------\n");
    423         printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
    424         printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
    425         printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
    426         {
    427                 int i;
    428                 for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
    429                 {
    430                         printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
    431                                 getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
    432                 }
    433                 printf("\n");
    434         }
    435 */
    436 
    437 
    438 
    439349        SPC(m_profiling.m_total);
    440350        /* optimize                             */
    … …  
    492402                if(pairs.size()>0)
    493403                {
    494 
    495                         int                     ni=btMin(pairs.size(),btMax<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
     404                        const int       ci=pairs.size();
     405                        int                     ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
    496406                        for(int i=0;i<ni;++i)
    497407                        {
    498                                 btBroadphasePair&       p=pairs[(m_cid+i)%pairs.size()];
     408                                btBroadphasePair&       p=pairs[(m_cid+i)%ci];
    499409                                btDbvtProxy*            pa=(btDbvtProxy*)p.m_pProxy0;
    500410                                btDbvtProxy*            pb=(btDbvtProxy*)p.m_pProxy1;
    … …  
    502412                                {
    503413#if DBVT_BP_SORTPAIRS
    504                                         if(pa->m_uniqueId>pb->m_uniqueId)
    505                                                 btSwap(pa,pb);
     414                                        if(pa>pb) btSwap(pa,pb);
    506415#endif
    507416                                        m_paircache->removeOverlappingPair(pa,pb,dispatcher);
    … …  
    558467        aabbMin=bounds.Mins();
    559468        aabbMax=bounds.Maxs();
    560 }
    561 
    562 void btDbvtBroadphase::resetPool(btDispatcher* dispatcher)
    563 {
    564        
    565         int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
    566         if (!totalObjects)
    567         {
    568                 //reset internal dynamic tree data structures
    569                 m_sets[0].clear();
    570                 m_sets[1].clear();
    571                
    572                 m_deferedcollide        =       false;
    573                 m_needcleanup           =       true;
    574                 m_prediction            =       1/(btScalar)2;
    575                 m_stageCurrent          =       0;
    576                 m_fixedleft                     =       0;
    577                 m_fupdates                      =       1;
    578                 m_dupdates                      =       0;
    579                 m_cupdates                      =       10;
    580                 m_newpairs                      =       1;
    581                 m_updates_call          =       0;
    582                 m_updates_done          =       0;
    583                 m_updates_ratio         =       0;
    584                
    585                 m_gid                           =       0;
    586                 m_pid                           =       0;
    587                 m_cid                           =       0;
    588                 for(int i=0;i<=STAGECOUNT;++i)
    589                 {
    590                         m_stageRoots[i]=0;
    591                 }
    592         }
    593469}
    594470
    … …  
    725601#undef  SPC
    726602#endif
    727 

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

    r2907 r2908  
    2525
    2626#define DBVT_BP_PROFILE                                 0
    27 //#define DBVT_BP_SORTPAIRS                             1
     27#define DBVT_BP_SORTPAIRS                               1
    2828#define DBVT_BP_PREVENTFALSEUPDATE              0
    2929#define DBVT_BP_ACCURATESLEEPING                0
    … …  
    115115        void                                                    printStats();
    116116        static void                                             benchmark(btBroadphaseInterface*);
    117 
    118 
    119         void    performDeferredRemoval(btDispatcher* dispatcher);
    120 
    121         ///reset broadphase internal structures, to ensure determinism/reproducability
    122         virtual void resetPool(btDispatcher* dispatcher);
    123 
    124117};
    125118

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp

    r2907 r2908  
    483483
    484484}
    485 
    486 void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
    487 {
    488         // not yet
    489 }

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h

    r2907 r2908  
    144144        void quicksort (btBroadphasePairArray& a, int lo, int hi);
    145145
    146         ///reset broadphase internal structures, to ensure determinism/reproducability
    147         virtual void resetPool(btDispatcher* dispatcher);
    148 
    149146};
    150147

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

    r2907 r2908  
    2020#include "btDispatcher.h"
    2121#include "btCollisionAlgorithm.h"
    22 #include "LinearMath/btAabbUtil2.h"
    2322
    2423#include <stdio.h>
    … …  
    137136{
    138137        gFindPairs++;
    139         if(proxy0->m_uniqueId>proxy1->m_uniqueId)
    140                 btSwap(proxy0,proxy1);
     138        if(proxy0>proxy1) btSwap(proxy0,proxy1);
    141139        int proxyId1 = proxy0->getUid();
    142140        int proxyId2 = proxy1->getUid();
    … …  
    214212btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
    215213{
    216         if(proxy0->m_uniqueId>proxy1->m_uniqueId)
    217                 btSwap(proxy0,proxy1);
     214        if(proxy0>proxy1) btSwap(proxy0,proxy1);
    218215        int proxyId1 = proxy0->getUid();
    219216        int proxyId2 = proxy1->getUid();
    … …  
    274271{
    275272        gRemovePairs++;
    276         if(proxy0->m_uniqueId>proxy1->m_uniqueId)
    277                 btSwap(proxy0,proxy1);
     273        if(proxy0>proxy1) btSwap(proxy0,proxy1);
    278274        int proxyId1 = proxy0->getUid();
    279275        int proxyId2 = proxy1->getUid();
    … …  
    397393}
    398394
    399 void    btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
    400 {
    401         ///need to keep hashmap in sync with pair address, so rebuild all
    402         btBroadphasePairArray tmpPairs;
    403         int i;
    404         for (i=0;i<m_overlappingPairArray.size();i++)
    405         {
    406                 tmpPairs.push_back(m_overlappingPairArray[i]);
    407         }
    408 
    409         for (i=0;i<tmpPairs.size();i++)
    410         {
    411                 removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher);
    412         }
    413        
    414         for (i = 0; i < m_next.size(); i++)
    415         {
    416                 m_next[i] = BT_NULL_PAIR;
    417         }
    418 
    419         tmpPairs.quickSort(btBroadphasePairSortPredicate());
    420 
    421         for (i=0;i<tmpPairs.size();i++)
    422         {
    423                 addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1);
    424         }
    425 
    426        
    427 }
    428395
    429396
    … …  
    463430{
    464431        //don't add overlap with own
    465         btAssert(proxy0 != proxy1);
     432        assert(proxy0 != proxy1);
    466433
    467434        if (!needsBroadphaseCollision(proxy0,proxy1))
    … …  
    494461        if (findIndex < m_overlappingPairArray.size())
    495462        {
    496                 //btAssert(it != m_overlappingPairSet.end());
     463                //assert(it != m_overlappingPairSet.end());
    497464                 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
    498465                return pair;
    … …  
    524491                {
    525492                        cleanOverlappingPair(*pair,dispatcher);
    526                         pair->m_pProxy0 = 0;
    527                         pair->m_pProxy1 = 0;
    528                         m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
     493
     494                        m_overlappingPairArray.swap(i,m_overlappingPairArray.capacity()-1);
    529495                        m_overlappingPairArray.pop_back();
    530496                        gOverlappingPairs--;
    … …  
    626592        processAllOverlappingPairs(&removeCallback,dispatcher);
    627593}
    628 
    629 void    btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
    630 {
    631         //should already be sorted
    632 }
    633 

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

    r2907 r2908  
    8484
    8585        virtual void    setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0;
    86 
    87         virtual void    sortOverlappingPairs(btDispatcher* dispatcher) = 0;
    88 
    8986
    9087};
    … …  
    263260        }
    264261
    265         virtual void    sortOverlappingPairs(btDispatcher* dispatcher);
    266        
    267 
    268 protected:
     262public:
    269263       
    270264        btAlignedObjectArray<int>       m_hashTable;
    … …  
    376370                }
    377371
    378                 virtual void    sortOverlappingPairs(btDispatcher* dispatcher);
    379                
    380372
    381373};
    … …  
    456448        }
    457449       
    458         virtual void    sortOverlappingPairs(btDispatcher* dispatcher)
    459         {
    460         }
    461450
    462451

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp

    r2907 r2908  
    470470
    471471#ifdef RAYAABB2
     472        btVector3 rayFrom = raySource;
    472473        btVector3 rayDir = (rayTarget-raySource);
    473474        rayDir.normalize ();
    … …  
    558559
    559560#ifdef RAYAABB2
     561        btVector3 rayFrom = raySource;
    560562        btVector3 rayDirection = (rayTarget-raySource);
    561563        rayDirection.normalize ();
    … …  
    816818#include <new>
    817819
    818 #if 0
    819820//PCK: consts
    820821static const unsigned BVH_ALIGNMENT = 16;
    … …  
    822823
    823824static const unsigned BVH_ALIGNMENT_BLOCKS = 2;
    824 #endif
     825
    825826
    826827
    … …  
    11461147
    11471148
    1148 

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

    r2907 r2908  
    325325}
    326326
    327 void    btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
    328 {
    329         //not yet
    330 }
     327
     328

  • code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h

    r2907 r2908  
    109109        }
    110110
    111         ///reset broadphase internal structures, to ensure determinism/reproducability
    112         virtual void resetPool(btDispatcher* dispatcher);
    113 
    114111
    115112        void    validate();

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

    r2907 r2908  
    5353                {
    5454                        m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
    55                         btAssert(m_doubleDispatch[i][j]);
     55                        assert(m_doubleDispatch[i][j]);
    5656                }
    5757        }
    … …  
    8080        btCollisionObject* body1 = (btCollisionObject*)b1;
    8181
    82         //test for Bullet 2.74: use a relative contact breaking threshold without clamping against 'gContactBreakingThreshold'
    83         //btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()));
    84         btScalar contactBreakingThreshold = btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold());
    85 
    86         btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
    87                
     82        btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()));
     83       
    8884        void* mem = 0;
    8985       
    … …  
    9692
    9793        }
    98         btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold);
     94        btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold);
    9995        manifold->m_index1a = m_manifoldsPtr.size();
    10096        m_manifoldsPtr.push_back(manifold);
    … …  
    147143        return algo;
    148144}
     145
    149146
    150147
    … …  
    164161bool    btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1)
    165162{
    166         btAssert(body0);
    167         btAssert(body1);
     163        assert(body0);
     164        assert(body1);
    168165
    169166        bool needsCollision = true;

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp

    r2907 r2908  
    1818
    1919btCollisionObject::btCollisionObject()
    20         :       m_anisotropicFriction(1.f,1.f,1.f),
    21         m_hasAnisotropicFriction(false),
    22         m_contactProcessingThreshold(1e30f),
    23                 m_broadphaseHandle(0),
     20        :       m_broadphaseHandle(0),
    2421                m_collisionShape(0),
    2522                m_rootCollisionShape(0),

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h

    r2907 r2908  
    5353        btVector3       m_interpolationLinearVelocity;
    5454        btVector3       m_interpolationAngularVelocity;
    55        
    56         btVector3               m_anisotropicFriction;
    57         bool                            m_hasAnisotropicFriction;
    58         btScalar                m_contactProcessingThreshold;   
    59 
    6055        btBroadphaseProxy*              m_broadphaseHandle;
    6156        btCollisionShape*               m_collisionShape;
    … …  
    120115                CO_COLLISION_OBJECT =1,
    121116                CO_RIGID_BODY,
     117                CO_SOFT_BODY,
    122118                ///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter
    123119                ///It is useful for collision sensors, explosion objects, character controller etc.
    124                 CO_GHOST_OBJECT,
    125                 CO_SOFT_BODY,
    126                 CO_HF_FLUID
     120                CO_GHOST_OBJECT
    127121        };
    128122
    … …  
    133127        }
    134128
    135         const btVector3& getAnisotropicFriction() const
    136         {
    137                 return m_anisotropicFriction;
    138         }
    139         void    setAnisotropicFriction(const btVector3& anisotropicFriction)
    140         {
    141                 m_anisotropicFriction = anisotropicFriction;
    142                 m_hasAnisotropicFriction = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f);
    143         }
    144         bool    hasAnisotropicFriction() const
    145         {
    146                 return m_hasAnisotropicFriction;
    147         }
    148 
    149         ///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default.
    150         ///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges
    151         void    setContactProcessingThreshold( btScalar contactProcessingThreshold)
    152         {
    153                 m_contactProcessingThreshold = contactProcessingThreshold;
    154         }
    155         btScalar        getContactProcessingThreshold() const
    156         {
    157                 return m_contactProcessingThreshold;
    158         }
    159129
    160130        SIMD_FORCE_INLINE bool          isStaticObject() const {

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

    r2907 r2908  
    7070                        getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
    7171                        getBroadphase()->destroyProxy(bp,m_dispatcher1);
    72                         collisionObject->setBroadphaseHandle(0);
    7372                }
    7473        }
    … …  
    120119
    121120
    122 void    btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
    123 {
    124         btVector3 minAabb,maxAabb;
    125         colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
    126         //need to increase the aabb for contact thresholds
    127         btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
    128         minAabb -= contactThreshold;
    129         maxAabb += contactThreshold;
    130 
    131         btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
    132 
    133         //moving objects should be moderately sized, probably something wrong if not
    134         if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
    135         {
    136                 bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
    137         } else
    138         {
    139                 //something went wrong, investigate
    140                 //this assert is unwanted in 3D modelers (danger of loosing work)
    141                 colObj->setActivationState(DISABLE_SIMULATION);
    142 
    143                 static bool reportMe = true;
    144                 if (reportMe && m_debugDrawer)
    145                 {
    146                         reportMe = false;
    147                         m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
    148                         m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
    149                         m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
    150                         m_debugDrawer->reportErrorWarning("Thanks.\n");
    151                 }
    152         }
    153 }
    154 
    155121void    btCollisionWorld::updateAabbs()
    156122{
    … …  
    165131                if (colObj->isActive())
    166132                {
    167                         updateSingleAabb(colObj);
    168                 }
    169         }
     133                        btVector3 minAabb,maxAabb;
     134                        colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
     135                        //need to increase the aabb for contact thresholds
     136                        btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
     137                        minAabb -= contactThreshold;
     138                        maxAabb += contactThreshold;
     139
     140                        btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
     141
     142                        //moving objects should be moderately sized, probably something wrong if not
     143                        if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
     144                        {
     145                                bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
     146                        } else
     147                        {
     148                                //something went wrong, investigate
     149                                //this assert is unwanted in 3D modelers (danger of loosing work)
     150                                colObj->setActivationState(DISABLE_SIMULATION);
     151
     152                                static bool reportMe = true;
     153                                if (reportMe && m_debugDrawer)
     154                                {
     155                                        reportMe = false;
     156                                        m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
     157                                        m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
     158                                        m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
     159                                        m_debugDrawer->reportErrorWarning("Thanks.\n");
     160                                }
     161                        }
     162                }
     163        }
     164
    170165}
    171166
    … …  
    299294                                        BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
    300295                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh):
    301                   //@BP Mod
    302                                                 btTriangleRaycastCallback(from,to, resultCallback->m_flags),
     296                                                btTriangleRaycastCallback(from,to),
    303297                                                        m_resultCallback(resultCallback),
    304298                                                        m_collisionObject(collisionObject),
    … …  
    349343                                        BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
    350344                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh):
    351                   //@BP Mod
    352                   btTriangleRaycastCallback(from,to, resultCallback->m_flags),
     345                                                btTriangleRaycastCallback(from,to),
    353346                                                        m_resultCallback(resultCallback),
    354347                                                        m_collisionObject(collisionObject),
    … …  
    671664                        //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
    672665                        //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
    673 #if 0
     666                       
    674667#ifdef RECALCULATE_AABB
    675668                        btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
    … …  
    679672                        const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
    680673                        const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
    681 #endif
    682674#endif
    683675                        //btScalar hitLambda = m_resultCallback.m_closestHitFraction;

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h

    r2907 r2908  
    5959 *
    6060 */
    61 
    62 
    6361 
    6462 
    … …  
    8280{
    8381
    84 
    8582       
    8683protected:
    8784
    8885        btAlignedObjectArray<btCollisionObject*>        m_collisionObjects;
    89 
    9086       
    9187        btDispatcher*   m_dispatcher1;
    … …  
    9995        btIDebugDraw*   m_debugDrawer;
    10096
    101 
    10297       
    10398public:
    … …  
    138133                return m_dispatcher1;
    139134        }
    140 
    141         void    updateSingleAabb(btCollisionObject* colObj);
    142135
    143136        virtual void    updateAabbs();
    … …  
    161154                int     m_shapePart;
    162155                int     m_triangleIndex;
    163 
    164156               
    165157                //const btCollisionShape*       m_shapeTemp;
    … …  
    194186                short int       m_collisionFilterGroup;
    195187                short int       m_collisionFilterMask;
    196       //@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback
    197       unsigned int m_flags;
    198188
    199189                virtual ~RayResultCallback()
    … …  
    209199                        m_collisionObject(0),
    210200                        m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
    211                         m_collisionFilterMask(btBroadphaseProxy::AllFilter),
    212          //@BP Mod
    213          m_flags(0)
     201                        m_collisionFilterMask(btBroadphaseProxy::AllFilter)
    214202                {
    215203                }
    … …  
    239227                btVector3       m_hitNormalWorld;
    240228                btVector3       m_hitPointWorld;
    241 
    242229                       
    243230                virtual btScalar        addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
    … …  
    245232                        //caller already does the filter on the m_closestHitFraction
    246233                        btAssert(rayResult.m_hitFraction <= m_closestHitFraction);
    247 
    248234                       
    249235                        m_closestHitFraction = rayResult.m_hitFraction;
    … …  
    292278                short int       m_collisionFilterGroup;
    293279                short int       m_collisionFilterMask;
    294 
    295280               
    296281                ConvexResultCallback()
    … …  
    304289                {
    305290                }
    306 
    307291               
    308292                bool    hasHit() const
    … …  
    310294                        return (m_closestHitFraction < btScalar(1.));
    311295                }
    312 
    313296
    314297               
    … …  
    339322                btVector3       m_hitPointWorld;
    340323                btCollisionObject*      m_hitCollisionObject;
    341 
    342324               
    343325                virtual btScalar        addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace)
    … …  
    345327//caller already does the filter on the m_closestHitFraction
    346328                        btAssert(convexResult.m_hitFraction <= m_closestHitFraction);
    347 
    348329                                               
    349330                        m_closestHitFraction = convexResult.m_hitFraction;

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

    r2907 r2908  
    2020#include "LinearMath/btIDebugDraw.h"
    2121#include "LinearMath/btAabbUtil2.h"
    22 #include "btManifoldResult.h"
    2322
    2423btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
    … …  
    3029
    3130        btCollisionObject* colObj = m_isSwapped? body1 : body0;
    32         btAssert (colObj->getCollisionShape()->isCompound());
     31        btCollisionObject* otherObj = m_isSwapped? body0 : body1;
     32        assert (colObj->getCollisionShape()->isCompound());
    3333       
    3434        btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
    35         m_compoundShapeRevision = compoundShape->getUpdateRevision();
    36        
    37         preallocateChildAlgorithms(body0,body1);
    38 }
    39 
    40 void    btCompoundCollisionAlgorithm::preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1)
    41 {
    42         btCollisionObject* colObj = m_isSwapped? body1 : body0;
    43         btCollisionObject* otherObj = m_isSwapped? body0 : body1;
    44         btAssert (colObj->getCollisionShape()->isCompound());
    45        
    46         btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
    47 
    4835        int numChildren = compoundShape->getNumChildShapes();
    4936        int i;
    … …  
    6047                        btCollisionShape* childShape = compoundShape->getChildShape(i);
    6148                        colObj->internalSetTemporaryCollisionShape( childShape );
    62                         m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(colObj,otherObj,m_sharedManifold);
     49                        m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj,m_sharedManifold);
    6350                        colObj->internalSetTemporaryCollisionShape( tmpShape );
    6451                }
    … …  
    6653}
    6754
    68 void    btCompoundCollisionAlgorithm::removeChildAlgorithms()
     55
     56btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
    6957{
    7058        int numChildren = m_childCollisionAlgorithms.size();
    … …  
    7866                }
    7967        }
    80 }
    81 
    82 btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
    83 {
    84         removeChildAlgorithms();
    8568}
    8669
    … …  
    185168        btCollisionObject* otherObj = m_isSwapped? body0 : body1;
    186169
    187        
    188 
    189         btAssert (colObj->getCollisionShape()->isCompound());
     170        assert (colObj->getCollisionShape()->isCompound());
    190171        btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
    191 
    192         ///btCompoundShape might have changed:
    193         ////make sure the internal child collision algorithm caches are still valid
    194         if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
    195         {
    196                 ///clear and update all
    197                 removeChildAlgorithms();
    198                
    199                 preallocateChildAlgorithms(body0,body1);
    200         }
    201 
    202172
    203173        btDbvt* tree = compoundShape->getDynamicAabbTree();
    … …  
    205175        btCompoundLeafCallback  callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
    206176
    207         ///we need to refresh all contact manifolds
    208         ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
    209         ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
    210         {
    211                 int i;
    212                 btManifoldArray manifoldArray;
    213                 for (i=0;i<m_childCollisionAlgorithms.size();i++)
    214                 {
    215                         if (m_childCollisionAlgorithms[i])
    216                         {
    217                                 m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
    218                                 for (int m=0;m<manifoldArray.size();m++)
    219                                 {
    220                                         if (manifoldArray[m]->getNumContacts())
    221                                         {
    222                                                 resultOut->setPersistentManifold(manifoldArray[m]);
    223                                                 resultOut->refreshContactPoints();
    224                                                 resultOut->setPersistentManifold(0);//??necessary?
    225                                         }
    226                                 }
    227                                 manifoldArray.clear();
    228                         }
    229                 }
    230         }
    231177
    232178        if (tree)
    … …  
    297243        btCollisionObject* otherObj = m_isSwapped? body0 : body1;
    298244
    299         btAssert (colObj->getCollisionShape()->isCompound());
     245        assert (colObj->getCollisionShape()->isCompound());
    300246       
    301247        btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
    … …  
    340286
    341287
    342 

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h

    r2907 r2908  
    2727#include "LinearMath/btAlignedObjectArray.h"
    2828class btDispatcher;
    29 class btCollisionObject;
    3029
    3130/// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
    … …  
    3736        class btPersistentManifold*     m_sharedManifold;
    3837        bool                                    m_ownsManifold;
    39 
    40         int     m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated
    4138       
    42         void    removeChildAlgorithms();
    43        
    44         void    preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1);
    45 
    4639public:
    4740

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

    r2907 r2908  
    5252btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*                       simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
    5353{
    54         m_numPerturbationIterations = 0;
    55         m_minimumPointsPerturbationThreshold = 3;
    5654        m_simplexSolver = simplexSolver;
    5755        m_pdSolver = pdSolver;
    … …  
    6260}
    6361
    64 btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
     62btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
    6563: btActivatingCollisionAlgorithm(ci,body0,body1),
    6664m_simplexSolver(simplexSolver),
    … …  
    6866m_ownManifold (false),
    6967m_manifoldPtr(mf),
    70 m_lowLevelOfDetail(false),
     68m_lowLevelOfDetail(false)
    7169#ifdef USE_SEPDISTANCE_UTIL2
    7270,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
    73                           (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
     71                          (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc())
    7472#endif
    75 m_numPerturbationIterations(numPerturbationIterations),
    76 m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
    7773{
    7874        (void)body0;
    … …  
    9894
    9995
    100 struct btPerturbedContactResult : public btManifoldResult
    101 {
    102         btManifoldResult* m_originalManifoldResult;
    103         btTransform m_transformA;
    104         btTransform m_transformB;
    105         btTransform     m_unPerturbedTransform;
    106         bool    m_perturbA;
    107         btIDebugDraw*   m_debugDrawer;
    108 
    109 
    110         btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
    111                 :m_originalManifoldResult(originalResult),
    112                 m_transformA(transformA),
    113                 m_transformB(transformB),
    114                 m_perturbA(perturbA),
    115                 m_unPerturbedTransform(unPerturbedTransform),
    116                 m_debugDrawer(debugDrawer)
    117         {
    118         }
    119         virtual ~ btPerturbedContactResult()
    120         {
    121         }
    122 
    123         virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
    124         {
    125                 btVector3 endPt,startPt;
    126                 btScalar newDepth;
    127                 btVector3 newNormal;
    128 
    129                 if (m_perturbA)
    130                 {
    131                         btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
    132                         endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
    133                         newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
    134                         startPt = endPt+normalOnBInWorld*newDepth;
    135                 } else
    136                 {
    137                         endPt = pointInWorld + normalOnBInWorld*orgDepth;
    138                         startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
    139                         newDepth = (endPt -  startPt).dot(normalOnBInWorld);
    140                        
    141                 }
    142 
    143 //#define DEBUG_CONTACTS 1
    144 #ifdef DEBUG_CONTACTS
    145                 m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
    146                 m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
    147                 m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
    148 #endif //DEBUG_CONTACTS
    149 
    150                
    151                 m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
    152         }
    153 
    154 };
    155 
    156 extern btScalar gContactBreakingThreshold;
     96
     97
    15798
    15899//
    … …  
    170111        resultOut->setPersistentManifold(m_manifoldPtr);
    171112
    172         //comment-out next line to test multi-contact generation
    173         //resultOut->getPersistentManifold()->clearManifold();
    174113       
    175114
    … …  
    208147
    209148        gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
     149
    210150        btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
    211 
    212         //now perturbe directions to get multiple contact points
    213         btVector3 v0,v1;
    214         btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
    215         btPlaneSpace1(sepNormalWorldSpace,v0,v1);
    216         //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
    217        
    218         //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
    219         if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
    220         {
    221                
    222                 int i;
    223 
    224                 bool perturbeA = true;
    225                 const btScalar angleLimit = 0.125f * SIMD_PI;
    226                 btScalar perturbeAngle;
    227                 btScalar radiusA = min0->getAngularMotionDisc();
    228                 btScalar radiusB = min1->getAngularMotionDisc();
    229                 if (radiusA < radiusB)
    230                 {
    231                         perturbeAngle = gContactBreakingThreshold /radiusA;
    232                         perturbeA = true;
    233                 } else
    234                 {
    235                         perturbeAngle = gContactBreakingThreshold / radiusB;
    236                         perturbeA = false;
    237                 }
    238                 if ( perturbeAngle > angleLimit )
    239                                 perturbeAngle = angleLimit;
    240 
    241                 btTransform unPerturbedTransform;
    242                 if (perturbeA)
    243                 {
    244                         unPerturbedTransform = input.m_transformA;
    245                 } else
    246                 {
    247                         unPerturbedTransform = input.m_transformB;
    248                 }
    249                
    250                 for ( i=0;i<m_numPerturbationIterations;i++)
    251                 {
    252                         btQuaternion perturbeRot(v0,perturbeAngle);
    253                         btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
    254                         btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
    255                        
    256                        
    257                         if (perturbeA)
    258                         {
    259                                 input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis());
    260                                 input.m_transformB = body1->getWorldTransform();
    261 #ifdef DEBUG_CONTACTS
    262                                 dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
    263 #endif //DEBUG_CONTACTS
    264                         } else
    265                         {
    266                                 input.m_transformA = body0->getWorldTransform();
    267                                 input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis());
    268 #ifdef DEBUG_CONTACTS
    269                                 dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
    270 #endif
    271                         }
    272                        
    273                         btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
    274                         gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
    275                        
    276                        
    277                 }
    278         }
    279 
    280        
    281151
    282152#ifdef USE_SEPDISTANCE_UTIL2

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

    r2907 r2908  
    3434///#define USE_SEPDISTANCE_UTIL2 1
    3535
    36 ///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects.
    37 ///Multiple contact points are calculated by perturbing the orientation of the smallest object orthogonal to the separating normal.
    38 ///This idea was described by Gino van den Bergen in this forum topic http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888
     36///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
    3937class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
    4038{
    … …  
    5048        bool                    m_lowLevelOfDetail;
    5149       
    52         int m_numPerturbationIterations;
    53         int m_minimumPointsPerturbationThreshold;
    54 
    55 
    5650        ///cache separating vector to speedup collision detection
    5751       
    … …  
    5953public:
    6054
    61         btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
    62 
     55        btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
    6356
    6457        virtual ~btConvexConvexAlgorithm();
    … …  
    8679        struct CreateFunc :public       btCollisionAlgorithmCreateFunc
    8780        {
    88 
    8981                btConvexPenetrationDepthSolver*         m_pdSolver;
    9082                btSimplexSolverInterface*                       m_simplexSolver;
    91                 int m_numPerturbationIterations;
    92                 int m_minimumPointsPerturbationThreshold;
    93 
     83               
    9484                CreateFunc(btSimplexSolverInterface*                    simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
    9585               
    … …  
    9989                {
    10090                        void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
    101                         return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
     91                        return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver);
    10292                }
    10393        };

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp

    r2907 r2908  
    55This software is provided 'as-is', without any express or implied warranty.
    66In no event will the authors be held liable for any damages arising from the use of this software.
    7 Permission is granted to anyone to use this software for any purpose,
    8 including commercial applications, and to alter it and redistribute it freely,
     7Permission is granted to anyone to use this software for any purpose, 
     8including commercial applications, and to alter it and redistribute it freely, 
    99subject to the following restrictions:
    1010
    … …  
    2323//#include <stdio.h>
    2424
    25 btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold)
     25btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
    2626: btCollisionAlgorithm(ci),
    2727m_ownManifold(false),
    2828m_manifoldPtr(mf),
    29 m_isSwapped(isSwapped),
    30 m_numPerturbationIterations(numPerturbationIterations),
    31 m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
     29m_isSwapped(isSwapped)
    3230{
    3331        btCollisionObject* convexObj = m_isSwapped? col1 : col0;
    3432        btCollisionObject* planeObj = m_isSwapped? col0 : col1;
    35 
     33       
    3634        if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj))
    3735        {
    … …  
    5149}
    5250
    53 void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
     51
     52
     53void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
    5454{
    55     btCollisionObject* convexObj = m_isSwapped? body1 : body0;
     55        (void)dispatchInfo;
     56        (void)resultOut;
     57        if (!m_manifoldPtr)
     58                return;
     59
     60        btCollisionObject* convexObj = m_isSwapped? body1 : body0;
    5661        btCollisionObject* planeObj = m_isSwapped? body0: body1;
    5762
    … …  
    5964        btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
    6065
    61     bool hasCollision = false;
     66        bool hasCollision = false;
    6267        const btVector3& planeNormal = planeShape->getPlaneNormal();
    6368        const btScalar& planeConstant = planeShape->getPlaneConstant();
    64        
    65         btTransform convexWorldTransform = convexObj->getWorldTransform();
     69        btTransform planeInConvex;
     70        planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform();
    6671        btTransform convexInPlaneTrans;
    67         convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexWorldTransform;
    68         //now perturbe the convex-world transform
    69         convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot);
    70         btTransform planeInConvex;
    71         planeInConvex= convexWorldTransform.inverse() * planeObj->getWorldTransform();
    72        
     72        convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform();
     73
    7374        btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
    74 
    7575        btVector3 vtxInPlane = convexInPlaneTrans(vtx);
    7676        btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
    … …  
    8888                resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
    8989        }
    90 }
    91 
    92 
    93 void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
    94 {
    95         (void)dispatchInfo;
    96         if (!m_manifoldPtr)
    97                 return;
    98 
    99     btCollisionObject* convexObj = m_isSwapped? body1 : body0;
    100         btCollisionObject* planeObj = m_isSwapped? body0: body1;
    101 
    102         btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape();
    103         btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
    104 
    105     bool hasCollision = false;
    106         const btVector3& planeNormal = planeShape->getPlaneNormal();
    107         const btScalar& planeConstant = planeShape->getPlaneConstant();
    108 
    109         //first perform a collision query with the non-perturbated collision objects
    110         {
    111                 btQuaternion rotq(0,0,0,1);
    112                 collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut);
    113         }
    114 
    115         if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
    116         {
    117                 btVector3 v0,v1;
    118                 btPlaneSpace1(planeNormal,v0,v1);
    119                 //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
    120 
    121                 const btScalar angleLimit = 0.125f * SIMD_PI;
    122                 btScalar perturbeAngle;
    123                 btScalar radius = convexShape->getAngularMotionDisc();
    124                 perturbeAngle = gContactBreakingThreshold / radius;
    125                 if ( perturbeAngle > angleLimit )
    126                                 perturbeAngle = angleLimit;
    127 
    128                 btQuaternion perturbeRot(v0,perturbeAngle);
    129                 for (int i=0;i<m_numPerturbationIterations;i++)
    130                 {
    131                         btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
    132                         btQuaternion rotq(planeNormal,iterationAngle);
    133                         collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0,body1,dispatchInfo,resultOut);
    134                 }
    135         }
    136 
    13790        if (m_ownManifold)
    13891        {

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h

    r2907 r2908  
    55This software is provided 'as-is', without any express or implied warranty.
    66In no event will the authors be held liable for any damages arising from the use of this software.
    7 Permission is granted to anyone to use this software for any purpose,
    8 including commercial applications, and to alter it and redistribute it freely,
     7Permission is granted to anyone to use this software for any purpose, 
     8including commercial applications, and to alter it and redistribute it freely, 
    99subject to the following restrictions:
    1010
    … …  
    2929class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
    3030{
    31         bool            m_ownManifold;
     31        bool    m_ownManifold;
    3232        btPersistentManifold*   m_manifoldPtr;
    33         bool            m_isSwapped;
    34         int                     m_numPerturbationIterations;
    35         int                     m_minimumPointsPerturbationThreshold;
    36 
     33        bool    m_isSwapped;
     34       
    3735public:
    3836
    39         btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold);
     37        btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
    4038
    4139        virtual ~btConvexPlaneCollisionAlgorithm();
    4240
    4341        virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
    44 
    45         void collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
    4642
    4743        virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
    … …  
    5753        struct CreateFunc :public       btCollisionAlgorithmCreateFunc
    5854        {
    59                 int     m_numPerturbationIterations;
    60                 int m_minimumPointsPerturbationThreshold;
    61                        
    62                 CreateFunc()
    63                         : m_numPerturbationIterations(3),
    64                         m_minimumPointsPerturbationThreshold(3)
    65                 {
    66                 }
    67                
    6855                virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
    6956                {
    … …  
    7158                        if (!m_swapped)
    7259                        {
    73                                 return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
     60                                return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false);
    7461                        } else
    7562                        {
    76                                 return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
     63                                return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true);
    7764                        }
    7865                }

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp

    r2907 r2908  
    289289        return m_emptyCreateFunc;
    290290}
    291 
    292 void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold)
    293 {
    294         btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc;
    295         convexConvex->m_numPerturbationIterations = numPerturbationIterations;
    296         convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
    297 }

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h

    r2907 r2908  
    112112        virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
    113113
    114         ///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm.
    115         ///By default, this feature is disabled for best performance.
    116         ///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature.
    117         ///@param minimumPointsPerturbationThreshold is the minimum number of points in the contact cache, above which the feature is disabled
    118         ///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first.
    119         ///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points.
    120         ///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection.
    121         void    setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3);
    122114
    123115};

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp

    r2907 r2908  
    6464btPairCachingGhostObject::~btPairCachingGhostObject()
    6565{
    66         m_hashPairCache->~btHashedOverlappingPairCache();
    6766        btAlignedFree( m_hashPairCache );
    6867}

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp

    r2907 r2908  
    5656void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
    5757{
    58         btAssert(m_manifoldPtr);
     58        assert(m_manifoldPtr);
    5959        //order in manifold needs to match
    6060       
    … …  
    9393   newPt.m_index0  = m_index0;
    9494   newPt.m_index1  = m_index1;
    95         //printf("depth=%f\n",depth);
     95       
    9696        ///@todo, check this for any side effects
    9797        if (insertIndex >= 0)

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h

    r2907 r2908  
    4646        int m_index0;
    4747        int m_index1;
    48        
    49 
    5048public:
    5149
    … …  
    8078        }
    8179
    82 
    8380        virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
    8481

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

    r2907 r2908  
    2525#include "LinearMath/btQuickprof.h"
    2626
    27 btSimulationIslandManager::btSimulationIslandManager():
    28 m_splitIslands(true)
     27btSimulationIslandManager::btSimulationIslandManager()
    2928{
    3029}
    … …  
    4544       
    4645        {
    47                
     46                btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
     47
    4848                for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++)
    4949                {
    50                         btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
    5150                        const btBroadphasePair& collisionPair = pairPtr[i];
    5251                        btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
    … …  
    187186                        }
    188187
    189                         btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
     188                        assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
    190189                        if (colObj0->getIslandTag() == islandId)
    191190                        {
    … …  
    214213                                }
    215214
    216                                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
     215                                assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
    217216
    218217                                if (colObj0->getIslandTag() == islandId)
    … …  
    235234                                }
    236235
    237                                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
     236                                assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
    238237
    239238                                if (colObj0->getIslandTag() == islandId)
    … …  
    253252        int maxNumManifolds = dispatcher->getNumManifolds();
    254253
    255 //#define SPLIT_ISLANDS 1
    256 //#ifdef SPLIT_ISLANDS
    257 
    258        
    259 //#endif //SPLIT_ISLANDS
     254#define SPLIT_ISLANDS 1
     255#ifdef SPLIT_ISLANDS
     256
     257       
     258#endif //SPLIT_ISLANDS
    260259
    261260       
    … …  
    281280                                colObj0->activate();
    282281                        }
    283                         if(m_splitIslands)
    284                         {
    285                                 //filtering for response
    286                                 if (dispatcher->needsResponse(colObj0,colObj1))
    287                                         m_islandmanifold.push_back(manifold);
    288                         }
     282#ifdef SPLIT_ISLANDS
     283        //              //filtering for response
     284                        if (dispatcher->needsResponse(colObj0,colObj1))
     285                                m_islandmanifold.push_back(manifold);
     286#endif //SPLIT_ISLANDS
    289287                }
    290288        }
    … …  
    306304        BT_PROFILE("processIslands");
    307305
    308         if(!m_splitIslands)
    309         {
    310                 btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
    311                 int maxNumManifolds = dispatcher->getNumManifolds();
    312                 callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
    313         }
    314         else
    315         {
    316                 // Sort manifolds, based on islands
    317                 // Sort the vector using predicate and std::sort
    318                 //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
    319 
    320                 int numManifolds = int (m_islandmanifold.size());
    321 
    322                 //we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
    323                 m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
    324 
    325                 //now process all active islands (sets of manifolds for now)
    326 
    327                 int startManifoldIndex = 0;
    328                 int endManifoldIndex = 1;
    329 
    330                 //int islandId;
    331 
    332                
    333 
    334         //      printf("Start Islands\n");
    335 
    336                 //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
    337                 for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
    338                 {
    339                         int islandId = getUnionFind().getElement(startIslandIndex).m_id;
    340 
    341 
    342                            bool islandSleeping = false;
    343                        
    344                                         for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
    345                                         {
    346                                                         int i = getUnionFind().getElement(endIslandIndex).m_sz;
    347                                                         btCollisionObject* colObj0 = collisionObjects[i];
    348                                                         m_islandBodies.push_back(colObj0);
    349                                                         if (!colObj0->isActive())
    350                                                                         islandSleeping = true;
    351                                         }
    352                        
    353 
    354                         //find the accompanying contact manifold for this islandId
    355                         int numIslandManifolds = 0;
    356                         btPersistentManifold** startManifold = 0;
    357 
    358                         if (startManifoldIndex<numManifolds)
    359                         {
    360                                 int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
    361                                 if (curIslandId == islandId)
    362                                 {
    363                                         startManifold = &m_islandmanifold[startManifoldIndex];
    364                                
    365                                         for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
    366                                         {
    367 
    368                                         }
    369                                         /// Process the actual simulation, only if not sleeping/deactivated
    370                                         numIslandManifolds = endManifoldIndex-startManifoldIndex;
    371                                 }
    372 
    373                         }
    374 
    375                         if (!islandSleeping)
    376                         {
    377                                 callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
    378         //                      printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
    379                         }
     306#ifndef SPLIT_ISLANDS
     307        btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
     308       
     309        callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
     310#else
     311        // Sort manifolds, based on islands
     312        // Sort the vector using predicate and std::sort
     313        //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
     314
     315        int numManifolds = int (m_islandmanifold.size());
     316
     317        //we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
     318        m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
     319
     320        //now process all active islands (sets of manifolds for now)
     321
     322        int startManifoldIndex = 0;
     323        int endManifoldIndex = 1;
     324
     325        //int islandId;
     326
     327       
     328
     329//      printf("Start Islands\n");
     330
     331        //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
     332        for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
     333        {
     334                int islandId = getUnionFind().getElement(startIslandIndex).m_id;
     335
     336
     337               bool islandSleeping = false;
     338               
     339                for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
     340                {
     341                        int i = getUnionFind().getElement(endIslandIndex).m_sz;
     342                        btCollisionObject* colObj0 = collisionObjects[i];
     343                                                m_islandBodies.push_back(colObj0);
     344                        if (!colObj0->isActive())
     345                                islandSleeping = true;
     346                }
     347               
     348
     349                //find the accompanying contact manifold for this islandId
     350                int numIslandManifolds = 0;
     351                btPersistentManifold** startManifold = 0;
     352
     353                if (startManifoldIndex<numManifolds)
     354                {
     355                        int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
     356                        if (curIslandId == islandId)
     357                        {
     358                                startManifold = &m_islandmanifold[startManifoldIndex];
    380359                       
    381                         if (numIslandManifolds)
    382                         {
    383                                 startManifoldIndex = endManifoldIndex;
    384                         }
    385 
    386                         m_islandBodies.resize(0);
    387                 }
    388         } // else if(!splitIslands)
    389 
    390 }
     360                                for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
     361                                {
     362
     363                                }
     364                                /// Process the actual simulation, only if not sleeping/deactivated
     365                                numIslandManifolds = endManifoldIndex-startManifoldIndex;
     366                        }
     367
     368                }
     369
     370                if (!islandSleeping)
     371                {
     372                        callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
     373//                      printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
     374                }
     375               
     376                if (numIslandManifolds)
     377                {
     378                        startManifoldIndex = endManifoldIndex;
     379                }
     380
     381                m_islandBodies.resize(0);
     382        }
     383#endif //SPLIT_ISLANDS
     384
     385
     386}

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h

    r2907 r2908  
    3636        btAlignedObjectArray<btCollisionObject* >  m_islandBodies;
    3737       
    38         bool m_splitIslands;
    3938       
    4039public:
    … …  
    6766        void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
    6867
    69         bool getSplitIslands()
    70         {
    71                 return m_splitIslands;
    72         }
    73         void setSplitIslands(bool doSplitIslands)
    74         {
    75                 m_splitIslands = doSplitIslands;
    76         }
    77 
    7868};
    7969

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp

    r2907 r2908  
    7979
    8080        ///point on A (worldspace)
    81         ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
     81        btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
    8282        ///point on B (worldspace)
    8383        btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp

    r2907 r2908  
    1515
    1616#include "btUnionFind.h"
     17#include <assert.h>
     18
    1719
    1820

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.h

    r2907 r2908  
    9999                int find(int x)
    100100                {
    101                         //btAssert(x < m_N);
    102                         //btAssert(x >= 0);
     101                        //assert(x < m_N);
     102                        //assert(x >= 0);
    103103
    104104                        while (x != m_elements[x].m_id)
    … …  
    111111                #endif //
    112112                                x = m_elements[x].m_id;
    113                                 //btAssert(x < m_N);
    114                                 //btAssert(x >= 0);
     113                                //assert(x < m_N);
     114                                //assert(x >= 0);
    115115
    116116                        }

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btBoxShape.h

    r2907 r2908  
    162162                {
    163163                case 0:
    164                         plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
     164                        plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
     165                        plane[3] = -halfExtents.x();
    165166                        break;
    166167                case 1:
    167                         plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
     168                        plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
     169                        plane[3] = -halfExtents.x();
    168170                        break;
    169171                case 2:
    170                         plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
     172                        plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
     173                        plane[3] = -halfExtents.y();
    171174                        break;
    172175                case 3:
    173                         plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
     176                        plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
     177                        plane[3] = -halfExtents.y();
    174178                        break;
    175179                case 4:
    176                         plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
     180                        plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
     181                        plane[3] = -halfExtents.z();
    177182                        break;
    178183                case 5:
    179                         plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
     184                        plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
     185                        plane[3] = -halfExtents.z();
    180186                        break;
    181187                default:
    182                         btAssert(0);
     188                        assert(0);
    183189                }
    184190        }
    … …  
    307313                        break;
    308314                default:
    309                         btAssert(0);
     315                        assert(0);
    310316                }
    311317        }

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp

    r2907 r2908  
    1515
    1616#include "BulletCollision/CollisionShapes/btCollisionShape.h"
    17 
    18 
    19 btScalar gContactThresholdFactor=btScalar(0.02);
    2017
    2118
    … …  
    4845btScalar        btCollisionShape::getContactBreakingThreshold() const
    4946{
    50         return getAngularMotionDisc() * gContactThresholdFactor;
     47        ///@todo make this 0.1 configurable
     48        return getAngularMotionDisc() * btScalar(0.1);
    5149}
    5250btScalar        btCollisionShape::getAngularMotionDisc() const

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp

    r2907 r2908  
    2323m_collisionMargin(btScalar(0.)),
    2424m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
    25 m_dynamicAabbTree(0),
    26 m_updateRevision(1)
     25m_dynamicAabbTree(0)
    2726{
    2827        m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
    … …  
    4847void    btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
    4948{
    50         m_updateRevision++;
    5149        //m_childTransforms.push_back(localTransform);
    5250        //m_childShapes.push_back(shape);
    … …  
    5755        child.m_childMargin = shape->getMargin();
    5856
    59        
     57        m_children.push_back(child);
     58
    6059        //extend the local aabbMin/aabbMax
    6160        btVector3 localAabbMin,localAabbMax;
    … …  
    7675        {
    7776                const btDbvtVolume      bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
    78                 int index = m_children.size();
     77                int index = m_children.size()-1;
    7978                child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
    8079        }
    81 
    82         m_children.push_back(child);
    8380
    8481}
    … …  
    103100void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
    104101{
    105         m_updateRevision++;
    106102        btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
    107103        if (m_dynamicAabbTree)
    … …  
    118114void btCompoundShape::removeChildShape(btCollisionShape* shape)
    119115{
    120         m_updateRevision++;
    121116        // Find the children containing the shape specified, and remove those children.
    122117        //note: there might be multiple children using the same shape!
    … …  
    145140        // Recalculate the local aabb
    146141        // Brute force, it iterates over all the shapes left.
    147 
    148142        m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
    149143        m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
    … …  
    168162{
    169163        btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
     164        localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
    170165        btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
    171        
    172         //avoid an illegal AABB when there are no children
    173         if (!m_children.size())
    174         {
    175                 localHalfExtents.setValue(0,0,0);
    176                 localCenter.setValue(0,0,0);
    177         }
    178         localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
    179                
    180166
    181167        btMatrix3x3 abs_b = trans.getBasis().absolute(); 
    … …  
    188174        aabbMin = center-extent;
    189175        aabbMax = center+extent;
    190        
     176
    191177}
    192178

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.h

    r2907 r2908  
    5959
    6060        btDbvt*                                                 m_dynamicAabbTree;
    61 
    62         ///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated
    63         int                                                             m_updateRevision;
    6461
    6562public:
    … …  
    156153        void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const;
    157154
    158         int     getUpdateRevision() const
    159         {
    160                 return m_updateRevision;
    161         }
    162155
    163156private:

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btConeShape.cpp

    r2907 r2908  
    6161                break;
    6262        default:
    63                 btAssert(0);
     63                assert(0);
    6464        };
    6565}

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp

    r2907 r2908  
    182182bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const
    183183{
    184         btAssert(0);
     184        assert(0);
    185185        return false;
    186186}

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp

    r2907 r2908  
    151151bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const
    152152{
    153         btAssert(0);
     153        assert(0);
    154154        return false;
    155155}

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp

    r2907 r2908  
    155155
    156156                btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
     157                btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
    157158                btScalar halfHeight = capsuleShape->getHalfHeight();
    158159                int capsuleUpAxis = capsuleShape->getUpAxis();
    … …  
    301302        {
    302303                btSphereShape* sphereShape = (btSphereShape*)this;
    303                 btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
    304                 btScalar margin = radius + sphereShape->getMarginNonVirtual();
     304                float radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
     305                float margin = radius + sphereShape->getMarginNonVirtual();
    305306                const btVector3& center = t.getOrigin();
    306307                btVector3 extent(margin,margin,margin);
    … …  
    314315        {
    315316                btBoxShape* convexShape = (btBoxShape*)this;
    316                 btScalar margin=convexShape->getMarginNonVirtual();
     317                float margin=convexShape->getMarginNonVirtual();
    317318                btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
    318319                halfExtents += btVector3(margin,margin,margin);

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

    r2907 r2908  
    145145}
    146146
    147 
    148 /// This returns the "raw" (user's initial) height, not the actual height.
    149 /// The actual height needs to be adjusted to be relative to the center
    150 ///   of the heightfield's AABB.
    151 btScalar
    152 btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const
     147btScalar        btHeightfieldTerrainShape::getHeightFieldValue(int x,int y) const
    153148{
    154149        btScalar val = 0.f;
    … …  
    187182
    188183
    189 /// this returns the vertex in bullet-local coordinates
     184
    190185void    btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const
    191186{
     187
    192188        btAssert(x>=0);
    193189        btAssert(y>=0);
    … …  
    195191        btAssert(y<m_heightStickLength);
    196192
    197         btScalar        height = getRawHeightFieldValue(x,y);
     193
     194        btScalar        height = getHeightFieldValue(x,y);
    198195
    199196        switch (m_upAxis)
    … …  
    202199                {
    203200                vertex.setValue(
    204                         height - m_localOrigin.getX(),
     201                        height,
    205202                        (-m_width/btScalar(2.0)) + x,
    206203                        (-m_length/btScalar(2.0) ) + y
    … …  
    212209                        vertex.setValue(
    213210                        (-m_width/btScalar(2.0)) + x,
    214                         height - m_localOrigin.getY(),
     211                        height,
    215212                        (-m_length/btScalar(2.0)) + y
    216213                        );
    … …  
    222219                        (-m_width/btScalar(2.0)) + x,
    223220                        (-m_length/btScalar(2.0)) + y,
    224                         height - m_localOrigin.getZ()
     221                        height
    225222                        );
    226223                        break;
    … …  
    234231
    235232        vertex*=m_localScaling;
     233       
    236234}
    237235
    … …  
    241239getQuantized
    242240(
    243 btScalar x
     241float x
    244242)
    245243{

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h

    r2907 r2908  
    3030  center (as determined by width and length and height, with each
    3131  axis multiplied by the localScaling).
    32 
    33   \b NOTE: be careful with coordinates.  If you have a heightfield with a local
    34   min height of -100m, and a max height of +500m, you may be tempted to place it
    35   at the origin (0,0) and expect the heights in world coordinates to be
    36   -100 to +500 meters.
    37   Actually, the heights will be -300 to +300m, because bullet will re-center
    38   the heightfield based on its AABB (which is determined by the min/max
    39   heights).  So keep in mind that once you create a btHeightfieldTerrainShape
    40   object, the heights will be adjusted relative to the center of the AABB.  This
    41   is different to the behavior of many rendering engines, but is useful for
    42   physics engines.
    4332
    4433  Most (but not all) rendering and heightfield libraries assume upAxis = 1
    … …  
    10089        btVector3       m_localScaling;
    10190
    102         virtual btScalar        getRawHeightFieldValue(int x,int y) const;
     91        virtual btScalar        getHeightFieldValue(int x,int y) const;
    10392        void            quantizeWithClamp(int* out, const btVector3& point,int isMax) const;
    10493        void            getVertex(int x,int y,btVector3& vertex) const;

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp

    r2907 r2908  
    2929btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
    3030{
    31         btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis()));
    32         btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(-vec*m_transB.getBasis()));
     31        btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(-vec*m_transA.getBasis()));
     32        btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(vec*m_transB.getBasis()));
    3333        return  supVertexA - supVertexB;
    3434}

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h

    r2907 r2908  
    3232        BT_DECLARE_ALIGNED_ALLOCATOR();
    3333
    34     btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;}
     34    btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {}
    3535    btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true):
    3636        btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh)
    3737        {
    38             m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
    39 
    4038            btVector3 m_triangle[3];
    4139            const unsigned char *vertexbase;
    … …  
    7068        btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh)
    7169        {
    72             m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
    73 
    7470            btVector3 m_triangle[3];
    7571            const unsigned char *vertexbase;
    … …  
    112108*/
    113109    }
     110        virtual int     getShapeType() const
     111        {
     112                return MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
     113        }
     114       
    114115        //debugging
    115116        virtual const char*     getName()const {return "MULTIMATERIALTRIANGLEMESH";}

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h

    r2907 r2908  
    4848        btScalar        getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();}
    4949
    50         void    setUnscaledRadius(btScalar      radius)
    51         {
    52                 m_implicitShapeDimensions.setX(radius);
    53                 btConvexInternalShape::setMargin(radius);
    54         }
    55 
    5650        //debugging
    5751        virtual const char*     getName()const {return "SPHERE";}

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp

    r2907 r2908  
    8383}
    8484
    85 
    86 void    btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const
     85void    btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax )
    8786{
    8887        m_aabbMin = aabbMin;
    … …  
    9796}
    9897
    99 

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h

    r2907 r2908  
    5353        IndexedMeshArray        m_indexedMeshes;
    5454        int m_pad[2];
    55         mutable int m_hasAabb; // using int instead of bool to maintain alignment
    56         mutable btVector3 m_aabbMin;
    57         mutable btVector3 m_aabbMax;
     55        int m_hasAabb; // using int instead of bool to maintain alignment
     56        btVector3 m_aabbMin;
     57        btVector3 m_aabbMax;
    5858
    5959public:
    … …  
    107107
    108108        virtual bool    hasPremadeAabb() const;
    109         virtual void    setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const;
     109        virtual void    setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax );
    110110        virtual void    getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const;
    111111

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp

    r2907 r2908  
    1 
    21/*
    32Bullet Continuous Collision Detection and Physics Library

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp

    r2907 r2908  
    3636        {
    3737                m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size()/3;
    38                 m_indexedMeshes[0].m_triangleIndexBase = 0;
     38                m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0];
    3939                m_indexedMeshes[0].m_indexType = PHY_INTEGER;
    4040                m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(int);
    … …  
    4242        {
    4343                m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size()/3;
    44                 m_indexedMeshes[0].m_triangleIndexBase = 0;
     44                m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_16bitIndices[0];
    4545                m_indexedMeshes[0].m_indexType = PHY_SHORT;
    4646                m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(short int);
    … …  
    5050        {
    5151                m_indexedMeshes[0].m_numVertices = m_4componentVertices.size();
    52                 m_indexedMeshes[0].m_vertexBase = 0;
     52                m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0];
    5353                m_indexedMeshes[0].m_vertexStride = sizeof(btVector3);
    5454        } else
    5555        {
    5656                m_indexedMeshes[0].m_numVertices = m_3componentVertices.size()/3;
    57                 m_indexedMeshes[0].m_vertexBase = 0;
     57                m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];
    5858                m_indexedMeshes[0].m_vertexStride = 3*sizeof(btScalar);
    5959        }
    … …  
    112112                        }
    113113        }
    114                 m_3componentVertices.push_back((float)vertex.getX());
    115                 m_3componentVertices.push_back((float)vertex.getY());
    116                 m_3componentVertices.push_back((float)vertex.getZ());
     114                m_3componentVertices.push_back(vertex.getX());
     115                m_3componentVertices.push_back(vertex.getY());
     116                m_3componentVertices.push_back(vertex.getZ());
    117117                m_indexedMeshes[0].m_numVertices++;
    118118                m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];

  • code/branches/questsystem5/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h

    r2907 r2908  
    4141        virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec)const
    4242        {
    43                 btAssert(0);
     43                assert(0);
    4444                return localGetSupportingVertex(vec);
    4545        }

  • code/branches/questsystem5/src/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp

    r2907 r2908  
    151151        }
    152152
    153         btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
     153        bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
     154        btAssert(!unbal);
    154155
    155156        return splitIndex;

  • code/branches/questsystem5/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp

    r2907 r2908  
    172172        }
    173173
    174         btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
     174        bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
     175        btAssert(!unbal);
    175176
    176177        return splitIndex;

  • code/branches/questsystem5/src/bullet/BulletCollision/Gimpact/btGImpactShape.h

    r2907 r2908  
    882882class btGImpactMeshShape : public btGImpactShapeInterface
    883883{
    884         btStridingMeshInterface* m_meshInterface;
    885 
    886884protected:
    887885        btAlignedObjectArray<btGImpactMeshShapePart*> m_mesh_parts;
    … …  
    910908        btGImpactMeshShape(btStridingMeshInterface * meshInterface)
    911909        {
    912                 m_meshInterface = meshInterface;
    913910                buildMeshParts(meshInterface);
    914911        }
    … …  
    926923
    927924
    928         btStridingMeshInterface* getMeshInterface()
    929         {
    930                 return m_meshInterface;
    931         }
    932 
    933         const btStridingMeshInterface* getMeshInterface() const
    934         {
    935                 return m_meshInterface;
    936         }
    937925
    938926        int getMeshPartCount() const
    … …  
    10461034
    10471035        //! call when reading child shapes
    1048         virtual void lockChildShapes() const
    1049         {
    1050                 btAssert(0);
    1051         }
    1052 
    1053         virtual void unlockChildShapes() const
     1036        virtual void lockChildShapes()
     1037        {
     1038                btAssert(0);
     1039        }
     1040
     1041        virtual void unlockChildShapes()
    10541042        {
    10551043                btAssert(0);

  • code/branches/questsystem5/src/bullet/BulletCollision/Gimpact/gim_box_set.cpp

    r2907 r2908  
    111111        }
    112112
    113         btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
     113        bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
     114        btAssert(!unbal);
    114115
    115116        return splitIndex;
    … …  
    180181}
    181182
    182 

  • code/branches/questsystem5/src/bullet/BulletCollision/Gimpact/gim_math.h

    r2907 r2908  
    108108#define GIM_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
    109109
    110 #define GIM_GREATER(x, y)       btFabs(x) > (y)
     110#define GIM_GREATER(x, y)       fabsf(x) > (y)
    111111
    112112///Swap numbers

  • code/branches/questsystem5/src/bullet/BulletCollision/Gimpact/gim_tri_collision.h

    r2907 r2908  
    276276                else
    277277                {
    278                         btScalar sumuv;
     278                        float sumuv;
    279279                        sumuv = u+v;
    280280                        if(sumuv<-G_EPSILON)

  • code/branches/questsystem5/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp

    r2907 r2908  
    136136                        //btScalar clippedDist  = dist;
    137137                       
    138                         //don't report time of impact for motion away from the contact normal (or causes minor penetration)
    139                         if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
    140                                 return false;
    141138                       
    142139                        dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
    … …  
    200197
    201198                }
    202        
     199
     200                //don't report time of impact for motion away from the contact normal (or causes minor penetration)
    203201                if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON)
    204202                        return false;
    205                        
     203
    206204                result.m_fraction = lambda;
    207205                result.m_normal = n;

  • code/branches/questsystem5/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

    r2907 r2908  
    151151                        if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
    152152                        {
    153                                 checkSimplex=true;
    154                                 //checkPenetration = false;
     153                                checkPenetration = false;
    155154                                break;
    156155                        }

  • code/branches/questsystem5/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

    r2907 r2908  
    113113                        }
    114114                       
    115                         ///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver
    116                         btScalar        getAppliedImpulse() const
    117                         {
    118                                 return m_appliedImpulse;
    119                         }
    120 
    121115                       
    122116

  • code/branches/questsystem5/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

    r2907 r2908  
    184184               
    185185        }
    186         if (insertIndex<0)
    187                 insertIndex=0;
    188 
    189186        btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
    190187        m_pointCache[insertIndex] = newPoint;

  • code/branches/questsystem5/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

    r2907 r2908  
    5656
    5757        btScalar        m_contactBreakingThreshold;
    58         btScalar        m_contactProcessingThreshold;
    5958
    6059       
    … …  
    7271        btPersistentManifold();
    7372
    74         btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold)
     73        btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold)
    7574                : m_body0(body0),m_body1(body1),m_cachedPoints(0),
    76                 m_contactBreakingThreshold(contactBreakingThreshold),
    77                 m_contactProcessingThreshold(contactProcessingThreshold)
     75                m_contactBreakingThreshold(contactBreakingThreshold)
    7876        {
    7977               
    … …  
    114112        ///@todo: get this margin from the current physics / collision environment
    115113        btScalar        getContactBreakingThreshold() const;
    116 
    117         btScalar        getContactProcessingThreshold() const
    118         {
    119                 return m_contactProcessingThreshold;
    120         }
    121114       
    122115        int getCacheEntry(const btManifoldPoint& newPoint) const;

  • code/branches/questsystem5/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp

    r2907 r2908  
    2424#include "btRaycastCallback.h"
    2525
    26 btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags)
     26btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to)
    2727        :
    2828        m_from(from),
    2929        m_to(to),
    30    //@BP Mod
    31    m_flags(flags),
    3230        m_hitFraction(btScalar(1.))
    3331{
    … …  
    5856                return ; // same sign
    5957        }
    60    //@BP Mod - Backface filtering
    61    if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a > btScalar(0.0)))
    62    {
    63       // Backface, skip check
    64       return;
    65    }
    6658       
    6759        const btScalar proj_length=dist_a-dist_b;
    … …  
    9890                                        if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance)
    9991                                        {
    100                   //@BP Mod
    101                   // Triangle normal isn't normalized
    102                                       triangleNormal.normalize();
    10392
    104                   //@BP Mod - Allow for unflipped normal when raycasting against backfaces
    105                   if (((m_flags & kF_KeepUnflippedNormal) != 0) || (dist_a <= btScalar(0.0)))
     93                                                if ( dist_a > 0 )
    10694                                                {
    107                                                         m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex);
     95                                                        m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
    10896                                                }
    10997                                                else
    11098                                                {
    111                      m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
     99                                                        m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex);
    112100                                                }
    113101                                        }

  • code/branches/questsystem5/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h

    r2907 r2908  
    3030        btVector3 m_to;
    3131
    32    //@BP Mod - allow backface filtering and unflipped normals
    33    enum EFlags
    34    {
    35       kF_None                 = 0,
    36       kF_FilterBackfaces      = 1 << 0,
    37       kF_KeepUnflippedNormal  = 1 << 1,   // Prevents returned face normal getting flipped when a ray hits a back-facing triangle
    38 
    39       kF_Terminator        = 0xFFFFFFFF
    40    };
    41    unsigned int m_flags;
    42 
    4332        btScalar        m_hitFraction;
    4433
    45         btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0);
     34        btTriangleRaycastCallback(const btVector3& from,const btVector3& to);
    4635       
    4736        virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);

  • code/branches/questsystem5/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp

    r2907 r2908  
    2626
    2727#include "btVoronoiSimplexSolver.h"
     28#include <assert.h>
     29//#include <stdio.h>
    2830
    2931#define VERTA  0
    … …  
    3638{
    3739       
    38         btAssert(m_numVertices>0);
     40        assert(m_numVertices>0);
    3941        m_numVertices--;
    4042        m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];

  • code/branches/questsystem5/src/bullet/BulletDynamics/CMakeLists.txt

    r2907 r2908  
    3636        ConstraintSolver/btTypedConstraint.h
    3737
    38         Dynamics/btActionInterface.h
    3938        Dynamics/btContinuousDynamicsWorld.h
    4039        Dynamics/btDiscreteDynamicsWorld.h

  • code/branches/questsystem5/src/bullet/BulletDynamics/Character/btCharacterControllerInterface.h

    r2907 r2908  
    1818
    1919#include "LinearMath/btVector3.h"
    20 #include "BulletDynamics/Dynamics/btActionInterface.h"
    2120
    2221class btCollisionShape;
    … …  
    2423class btCollisionWorld;
    2524
    26 class btCharacterControllerInterface : public btActionInterface
     25class btCharacterControllerInterface
    2726{
    2827public:

  • code/branches/questsystem5/src/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp

    r2907 r2908  
    2323#include "btKinematicCharacterController.h"
    2424
    25 static btVector3 upAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
    26 
    2725///@todo Interact with dynamic objects,
    2826///Ride kinematicly animated platforms properly
    … …  
    9694}
    9795
    98 btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
    99 {
    100         m_upAxis = upAxis;
     96btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight)
     97{
    10198        m_addedMargin = 0.02f;
    10299        m_walkDirection.setValue(0,0,0);
    … …  
    105102        m_stepHeight = stepHeight;
    106103        m_turnAngle = btScalar(0.0);
    107         m_convexShape=convexShape;     
     104        m_convexShape=convexShape;
     105       
    108106}
    109107
    … …  
    112110}
    113111
     112
    114113btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
    115114{
    … …  
    117116}
    118117
    119 bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
     118bool btKinematicCharacterController::recoverFromPenetration (btCollisionWorld* collisionWorld)
    120119{
    121120
    … …  
    174173        // phase 1: up
    175174        btTransform start, end;
    176         m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
     175        m_targetPosition = m_currentPosition + btVector3 (btScalar(0.0), m_stepHeight, btScalar(0.0));
    177176
    178177        start.setIdentity ();
    … …  
    180179
    181180        /* FIXME: Handle penetration properly */
    182         start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f));
     181        start.setOrigin (m_currentPosition + btVector3(btScalar(0.0), btScalar(0.1), btScalar(0.0)));
    183182        end.setOrigin (m_targetPosition);
    184183
    … …  
    345344
    346345        // phase 3: down
    347         btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
    348         btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight;
     346        btVector3 step_drop = btVector3(btScalar(0.0), m_currentStepOffset, btScalar(0.0));
     347        btVector3 gravity_drop = btVector3(btScalar(0.0), m_stepHeight, btScalar(0.0));
    349348        m_targetPosition -= (step_drop + gravity_drop);
    350349
    … …  
    391390
    392391
    393 void btKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
     392void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
    394393{
    395394       
    … …  
    414413}
    415414
    416 void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
     415void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
    417416{
    418417        btTransform xform;
    … …  
    470469        return true;
    471470}
    472 
    473 
    474 void    btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
    475 {
    476 }

  • code/branches/questsystem5/src/bullet/BulletDynamics/Character/btKinematicCharacterController.h

    r2907 r2908  
    6363
    6464        bool    m_useGhostObjectSweepTest;
    65 
    66         int m_upAxis;
    6765       
    6866        btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
    … …  
    7068        btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
    7169
    72         bool recoverFromPenetration ( btCollisionWorld* collisionWorld);
     70        bool recoverFromPenetration (btCollisionWorld* collisionWorld);
    7371        void stepUp (btCollisionWorld* collisionWorld);
    7472        void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
    … …  
    7674        void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
    7775public:
    78         btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
     76        btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight);
    7977        ~btKinematicCharacterController ();
    8078       
    81 
    82         ///btActionInterface interface
    83         virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime)
    84         {
    85                 preStep ( collisionWorld);
    86                 playerStep (collisionWorld, deltaTime);
    87         }
    88        
    89         ///btActionInterface interface
    90         void    debugDraw(btIDebugDraw* debugDrawer);
    91 
    92         void setUpAxis (int axis)
    93         {
    94                 if (axis < 0)
    95                         axis = 0;
    96                 if (axis > 2)
    97                         axis = 2;
    98                 m_upAxis = axis;
    99         }
    100 
    10179        virtual void    setWalkDirection(const btVector3& walkDirection)
    10280        {
    … …  
    10785        void warp (const btVector3& origin);
    10886
    109         void preStep (  btCollisionWorld* collisionWorld);
    110         void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
     87        void preStep ( btCollisionWorld* collisionWorld);
     88        void playerStep (btCollisionWorld* collisionWorld, btScalar dt);
    11189
    11290        void setFallSpeed (btScalar fallSpeed);

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

    r2907 r2908  
    2323#include <new>
    2424
    25 //-----------------------------------------------------------------------------
    26 
    27 #define CONETWIST_USE_OBSOLETE_SOLVER false
    28 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
    29 
    30 //-----------------------------------------------------------------------------
    31 
    3225btConeTwistConstraint::btConeTwistConstraint()
    33 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
    34 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
     26:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
    3527{
    3628}
    … …  
    4032                                                                                         const btTransform& rbAFrame,const btTransform& rbBFrame)
    4133                                                                                         :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
    42                                                                                          m_angularOnly(false),
    43                                                                                          m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
    44 {
    45         init();
     34                                                                                         m_angularOnly(false)
     35{
     36        m_swingSpan1 = btScalar(1e30);
     37        m_swingSpan2 = btScalar(1e30);
     38        m_twistSpan  = btScalar(1e30);
     39        m_biasFactor = 0.3f;
     40        m_relaxationFactor = 1.0f;
     41
     42        m_solveTwistLimit = false;
     43        m_solveSwingLimit = false;
     44
    4645}
    4746
    4847btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
    4948                                                                                        :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
    50                                                                                          m_angularOnly(false),
    51                                                                                          m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
     49                                                                                         m_angularOnly(false)
    5250{
    5351        m_rbBFrame = m_rbAFrame;
    54         init();
    55 }
    56 
    57 
    58 void btConeTwistConstraint::init()
    59 {
    60         m_angularOnly = false;
     52       
     53        m_swingSpan1 = btScalar(1e30);
     54        m_swingSpan2 = btScalar(1e30);
     55        m_twistSpan  = btScalar(1e30);
     56        m_biasFactor = 0.3f;
     57        m_relaxationFactor = 1.0f;
     58
    6159        m_solveTwistLimit = false;
    6260        m_solveSwingLimit = false;
    63         m_bMotorEnabled = false;
    64         m_maxMotorImpulse = btScalar(-1);
    65 
    66         setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30));
    67         m_damping = btScalar(0.01);
    68         m_fixThresh = CONETWIST_DEF_FIX_THRESH;
    69 }
    70 
    71 
    72 //-----------------------------------------------------------------------------
    73 
    74 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
    75 {
    76         if (m_useSolveConstraintObsolete)
    77         {
    78                 info->m_numConstraintRows = 0;
    79                 info->nub = 0;
    80         }
    81         else
    82         {
    83                 info->m_numConstraintRows = 3;
    84                 info->nub = 3;
    85                 calcAngleInfo2();
    86                 if(m_solveSwingLimit)
    87                 {
    88                         info->m_numConstraintRows++;
    89                         info->nub--;
    90                         if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
    91                         {
    92                                 info->m_numConstraintRows++;
    93                                 info->nub--;
    94                         }
    95                 }
    96                 if(m_solveTwistLimit)
    97                 {
    98                         info->m_numConstraintRows++;
    99                         info->nub--;
    100                 }
    101         }
    102 } // btConeTwistConstraint::getInfo1()
    103        
    104 //-----------------------------------------------------------------------------
    105 
    106 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
    107 {
    108         btAssert(!m_useSolveConstraintObsolete);
    109         //retrieve matrices
    110         btTransform body0_trans;
    111         body0_trans = m_rbA.getCenterOfMassTransform();
    112     btTransform body1_trans;
    113         body1_trans = m_rbB.getCenterOfMassTransform();
    114     // set jacobian
    115     info->m_J1linearAxis[0] = 1;
    116     info->m_J1linearAxis[info->rowskip+1] = 1;
    117     info->m_J1linearAxis[2*info->rowskip+2] = 1;
    118         btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
    119         {
    120                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
    121                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
    122                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
    123                 btVector3 a1neg = -a1;
    124                 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
    125         }
    126         btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
    127         {
    128                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
    129                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
    130                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
    131                 a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
    132         }
    133     // set right hand side
    134     btScalar k = info->fps * info->erp;
    135     int j;
    136         for (j=0; j<3; j++)
    137     {
    138         info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
    139                 info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
    140                 info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
    141     }
    142         int row = 3;
    143     int srow = row * info->rowskip;
    144         btVector3 ax1;
    145         // angular limits
    146         if(m_solveSwingLimit)
    147         {
    148                 btScalar *J1 = info->m_J1angularAxis;
    149                 btScalar *J2 = info->m_J2angularAxis;
    150                 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
    151                 {
    152                         btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
    153                         btVector3 p = trA.getBasis().getColumn(1);
    154                         btVector3 q = trA.getBasis().getColumn(2);
    155                         int srow1 = srow + info->rowskip;
    156                         J1[srow+0] = p[0];
    157                         J1[srow+1] = p[1];
    158                         J1[srow+2] = p[2];
    159                         J1[srow1+0] = q[0];
    160                         J1[srow1+1] = q[1];
    161                         J1[srow1+2] = q[2];
    162                         J2[srow+0] = -p[0];
    163                         J2[srow+1] = -p[1];
    164                         J2[srow+2] = -p[2];
    165                         J2[srow1+0] = -q[0];
    166                         J2[srow1+1] = -q[1];
    167                         J2[srow1+2] = -q[2];
    168                         btScalar fact = info->fps * m_relaxationFactor;
    169                         info->m_constraintError[srow] =   fact * m_swingAxis.dot(p);
    170                         info->m_constraintError[srow1] =  fact * m_swingAxis.dot(q);
    171                         info->m_lowerLimit[srow] = -SIMD_INFINITY;
    172                         info->m_upperLimit[srow] = SIMD_INFINITY;
    173                         info->m_lowerLimit[srow1] = -SIMD_INFINITY;
    174                         info->m_upperLimit[srow1] = SIMD_INFINITY;
    175                         srow = srow1 + info->rowskip;
     61       
     62}                       
     63
     64void    btConeTwistConstraint::buildJacobian()
     65{
     66        m_appliedImpulse = btScalar(0.);
     67
     68        //set bias, sign, clear accumulator
     69        m_swingCorrection = btScalar(0.);
     70        m_twistLimitSign = btScalar(0.);
     71        m_solveTwistLimit = false;
     72        m_solveSwingLimit = false;
     73        m_accTwistLimitImpulse = btScalar(0.);
     74        m_accSwingLimitImpulse = btScalar(0.);
     75
     76        if (!m_angularOnly)
     77        {
     78                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
     79                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
     80                btVector3 relPos = pivotBInW - pivotAInW;
     81
     82                btVector3 normal[3];
     83                if (relPos.length2() > SIMD_EPSILON)
     84                {
     85                        normal[0] = relPos.normalized();
    17686                }
    17787                else
    17888                {
    179                         ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
    180                         J1[srow+0] = ax1[0];
    181                         J1[srow+1] = ax1[1];
    182                         J1[srow+2] = ax1[2];
    183                         J2[srow+0] = -ax1[0];
    184                         J2[srow+1] = -ax1[1];
    185                         J2[srow+2] = -ax1[2];
    186                         btScalar k = info->fps * m_biasFactor;
    187 
    188                         info->m_constraintError[srow] = k * m_swingCorrection;
    189                         info->cfm[srow] = 0.0f;
    190                         // m_swingCorrection is always positive or 0
    191                         info->m_lowerLimit[srow] = 0;
    192                         info->m_upperLimit[srow] = SIMD_INFINITY;
    193                         srow += info->rowskip;
    194                 }
    195         }
    196         if(m_solveTwistLimit)
    197         {
    198                 ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
    199                 btScalar *J1 = info->m_J1angularAxis;
    200                 btScalar *J2 = info->m_J2angularAxis;
    201                 J1[srow+0] = ax1[0];
    202                 J1[srow+1] = ax1[1];
    203                 J1[srow+2] = ax1[2];
    204                 J2[srow+0] = -ax1[0];
    205                 J2[srow+1] = -ax1[1];
    206                 J2[srow+2] = -ax1[2];
    207                 btScalar k = info->fps * m_biasFactor;
    208                 info->m_constraintError[srow] = k * m_twistCorrection;
    209                 info->cfm[srow] = 0.0f;
    210                 if(m_twistSpan > 0.0f)
    211                 {
    212 
    213                         if(m_twistCorrection > 0.0f)
    214                         {
    215                                 info->m_lowerLimit[srow] = 0;
    216                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    217                         }
    218                         else
    219                         {
    220                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    221                                 info->m_upperLimit[srow] = 0;
    222                         }
    223                 }
    224                 else
    225                 {
    226                         info->m_lowerLimit[srow] = -SIMD_INFINITY;
    227                         info->m_upperLimit[srow] = SIMD_INFINITY;
    228                 }
    229                 srow += info->rowskip;
    230         }
    231 }
    232        
    233 //-----------------------------------------------------------------------------
    234 
    235 void    btConeTwistConstraint::buildJacobian()
    236 {
    237         if (m_useSolveConstraintObsolete)
    238         {
    239                 m_appliedImpulse = btScalar(0.);
    240                 m_accTwistLimitImpulse = btScalar(0.);
    241                 m_accSwingLimitImpulse = btScalar(0.);
    242 
    243                 if (!m_angularOnly)
    244                 {
    245                         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
    246                         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
    247                         btVector3 relPos = pivotBInW - pivotAInW;
    248 
    249                         btVector3 normal[3];
    250                         if (relPos.length2() > SIMD_EPSILON)
    251                         {
    252                                 normal[0] = relPos.normalized();
    253                         }
    254                         else
    255                         {
    256                                 normal[0].setValue(btScalar(1.0),0,0);
    257                         }
    258 
    259                         btPlaneSpace1(normal[0], normal[1], normal[2]);
    260 
    261                         for (int i=0;i<3;i++)
    262                         {
    263                                 new (&m_jac[i]) btJacobianEntry(
     89                        normal[0].setValue(btScalar(1.0),0,0);
     90                }
     91
     92                btPlaneSpace1(normal[0], normal[1], normal[2]);
     93
     94                for (int i=0;i<3;i++)
     95                {
     96                        new (&m_jac[i]) btJacobianEntry(
    26497                                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    26598                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    271104                                m_rbB.getInvInertiaDiagLocal(),
    272105                                m_rbB.getInvMass());
    273                         }
    274                 }
    275 
    276                 calcAngleInfo2();
    277         }
    278 }
    279 
    280 //-----------------------------------------------------------------------------
    281 
    282 void    btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
    283 {
    284         if (m_useSolveConstraintObsolete)
    285         {
    286                 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
    287                 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
    288 
    289                 btScalar tau = btScalar(0.3);
    290 
    291                 //linear part
    292                 if (!m_angularOnly)
    293                 {
    294                         btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
    295                         btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    296 
    297                         btVector3 vel1;
    298                         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
    299                         btVector3 vel2;
    300                         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
    301                         btVector3 vel = vel1 - vel2;
    302 
    303                         for (int i=0;i<3;i++)
    304                         {               
    305                                 const btVector3& normal = m_jac[i].m_linearJointAxis;
    306                                 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
    307 
    308                                 btScalar rel_vel;
    309                                 rel_vel = normal.dot(vel);
    310                                 //positional error (zeroth order error)
    311                                 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
    312                                 btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
    313                                 m_appliedImpulse += impulse;
    314                                
    315                                 btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
    316                                 btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
    317                                 bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
    318                                 bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
    319                
    320                         }
    321                 }
    322 
    323                 // apply motor
    324                 if (m_bMotorEnabled)
    325                 {
    326                         // compute current and predicted transforms
    327                         btTransform trACur = m_rbA.getCenterOfMassTransform();
    328                         btTransform trBCur = m_rbB.getCenterOfMassTransform();
    329                         btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
    330                         btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
    331                         btTransform trAPred; trAPred.setIdentity();
    332                         btVector3 zerovec(0,0,0);
    333                         btTransformUtil::integrateTransform(
    334                                 trACur, zerovec, omegaA, timeStep, trAPred);
    335                         btTransform trBPred; trBPred.setIdentity();
    336                         btTransformUtil::integrateTransform(
    337                                 trBCur, zerovec, omegaB, timeStep, trBPred);
    338 
    339                         // compute desired transforms in world
    340                         btTransform trPose(m_qTarget);
    341                         btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
    342                         btTransform trADes = trBPred * trABDes;
    343                         btTransform trBDes = trAPred * trABDes.inverse();
    344 
    345                         // compute desired omegas in world
    346                         btVector3 omegaADes, omegaBDes;
    347                        
    348                         btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
    349                         btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
    350 
    351                         // compute delta omegas
    352                         btVector3 dOmegaA = omegaADes - omegaA;
    353                         btVector3 dOmegaB = omegaBDes - omegaB;
    354 
    355                         // compute weighted avg axis of dOmega (weighting based on inertias)
    356                         btVector3 axisA, axisB;
    357                         btScalar kAxisAInv = 0, kAxisBInv = 0;
    358 
    359                         if (dOmegaA.length2() > SIMD_EPSILON)
    360                         {
    361                                 axisA = dOmegaA.normalized();
    362                                 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
    363                         }
    364 
    365                         if (dOmegaB.length2() > SIMD_EPSILON)
    366                         {
    367                                 axisB = dOmegaB.normalized();
    368                                 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
    369                         }
    370 
    371                         btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
    372 
    373                         static bool bDoTorque = true;
    374                         if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
    375                         {
    376                                 avgAxis.normalize();
    377                                 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
    378                                 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
    379                                 btScalar kInvCombined = kAxisAInv + kAxisBInv;
    380 
    381                                 btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
    382                                                                         (kInvCombined * kInvCombined);
    383 
    384                                 if (m_maxMotorImpulse >= 0)
    385                                 {
    386                                         btScalar fMaxImpulse = m_maxMotorImpulse;
    387                                         if (m_bNormalizedMotorStrength)
    388                                                 fMaxImpulse = fMaxImpulse/kAxisAInv;
    389 
    390                                         btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
    391                                         btScalar  newUnclampedMag = newUnclampedAccImpulse.length();
    392                                         if (newUnclampedMag > fMaxImpulse)
    393                                         {
    394                                                 newUnclampedAccImpulse.normalize();
    395                                                 newUnclampedAccImpulse *= fMaxImpulse;
    396                                                 impulse = newUnclampedAccImpulse - m_accMotorImpulse;
    397                                         }
    398                                         m_accMotorImpulse += impulse;
    399                                 }
    400 
    401                                 btScalar  impulseMag  = impulse.length();
    402                                 btVector3 impulseAxis =  impulse / impulseMag;
    403 
    404                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
    405                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
    406 
    407                         }
    408                 }
    409                 else // no motor: do a little damping
    410                 {
    411                         const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
    412                         const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
    413                         btVector3 relVel = angVelB - angVelA;
    414                         if (relVel.length2() > SIMD_EPSILON)
    415                         {
    416                                 btVector3 relVelAxis = relVel.normalized();
    417                                 btScalar m_kDamping =  btScalar(1.) /
    418                                         (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
    419                                          getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
    420                                 btVector3 impulse = m_damping * m_kDamping * relVel;
    421 
    422                                 btScalar  impulseMag  = impulse.length();
    423                                 btVector3 impulseAxis = impulse / impulseMag;
    424                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
    425                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
    426                         }
    427                 }
    428 
    429                 // joint limits
    430                 {
    431                         ///solve angular part
    432                         btVector3 angVelA;
    433                         bodyA.getAngularVelocity(angVelA);
    434                         btVector3 angVelB;
    435                         bodyB.getAngularVelocity(angVelB);
    436 
    437                         // solve swing limit
    438                         if (m_solveSwingLimit)
    439                         {
    440                                 btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
    441                                 btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
    442                                 if (relSwingVel > 0)
    443                                         amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
    444                                 btScalar impulseMag = amplitude * m_kSwing;
    445 
    446                                 // Clamp the accumulated impulse
    447                                 btScalar temp = m_accSwingLimitImpulse;
    448                                 m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
    449                                 impulseMag = m_accSwingLimitImpulse - temp;
    450 
    451                                 btVector3 impulse = m_swingAxis * impulseMag;
    452 
    453                                 // don't let cone response affect twist
    454                                 // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
    455                                 {
    456                                         btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
    457                                         btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
    458                                         impulse = impulseNoTwistCouple;
    459                                 }
    460 
    461                                 impulseMag = impulse.length();
    462                                 btVector3 noTwistSwingAxis = impulse / impulseMag;
    463 
    464                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
    465                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
    466                         }
    467 
    468 
    469                         // solve twist limit
    470                         if (m_solveTwistLimit)
    471                         {
    472                                 btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
    473                                 btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
    474                                 if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
    475                                         amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
    476                                 btScalar impulseMag = amplitude * m_kTwist;
    477 
    478                                 // Clamp the accumulated impulse
    479                                 btScalar temp = m_accTwistLimitImpulse;
    480                                 m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
    481                                 impulseMag = m_accTwistLimitImpulse - temp;
    482 
    483                                 btVector3 impulse = m_twistAxis * impulseMag;
    484 
    485                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
    486                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
    487                         }               
    488                 }
    489         }
    490 
    491 }
    492 
    493 //-----------------------------------------------------------------------------
    494 
    495 void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
    496 {
    497         (void)timeStep;
    498 
    499 }
    500 
    501 //-----------------------------------------------------------------------------
    502 
    503 void btConeTwistConstraint::calcAngleInfo()
    504 {
    505         m_swingCorrection = btScalar(0.);
    506         m_twistLimitSign = btScalar(0.);
    507         m_solveTwistLimit = false;
    508         m_solveSwingLimit = false;
     106                }
     107        }
    509108
    510109        btVector3 b1Axis1,b1Axis2,b1Axis3;
    … …  
    524123        {
    525124                b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
     125//              swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
    526126                swx = b2Axis1.dot(b1Axis1);
    527127                swy = b2Axis1.dot(b1Axis2);
    … …  
    530130                fact = fact / (fact + btScalar(1.0));
    531131                swing1 *= fact;
     132
    532133        }
    533134
    … …  
    535136        {
    536137                b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);                     
     138//              swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
    537139                swx = b2Axis1.dot(b1Axis1);
    538140                swy = b2Axis1.dot(b1Axis3);
    … …  
    551153                m_swingCorrection = EllipseAngle-1.0f;
    552154                m_solveSwingLimit = true;
     155               
    553156                // Calculate necessary axis & factors
    554157                m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
    555158                m_swingAxis.normalize();
     159
    556160                btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
    557161                m_swingAxis *= swingAxisSign;
     162
     163                m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
     164                        getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
     165
    558166        }
    559167
    … …  
    565173                btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
    566174                btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
    567                 m_twistAngle = twist;
    568 
    569 //              btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
    570                 btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
     175
     176                btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
    571177                if (twist <= -m_twistSpan*lockedFreeFactor)
    572178                {
    573179                        m_twistCorrection = -(twist + m_twistSpan);
    574180                        m_solveTwistLimit = true;
     181
    575182                        m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
    576183                        m_twistAxis.normalize();
    577184                        m_twistAxis *= -1.0f;
    578                 }
    579                 else if (twist >  m_twistSpan*lockedFreeFactor)
    580                 {
    581                         m_twistCorrection = (twist - m_twistSpan);
    582                         m_solveTwistLimit = true;
    583                         m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
    584                         m_twistAxis.normalize();
    585                 }
    586         }
    587 } // btConeTwistConstraint::calcAngleInfo()
    588 
    589 
    590 static btVector3 vTwist(1,0,0); // twist axis in constraint's space
    591 
    592 //-----------------------------------------------------------------------------
    593 
    594 void btConeTwistConstraint::calcAngleInfo2()
    595 {
    596         m_swingCorrection = btScalar(0.);
    597         m_twistLimitSign = btScalar(0.);
    598         m_solveTwistLimit = false;
    599         m_solveSwingLimit = false;
    600 
    601         {
    602                 // compute rotation of A wrt B (in constraint space)
    603                 btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
    604                 btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
    605                 btQuaternion qAB = qB.inverse() * qA;
    606 
    607                 // split rotation into cone and twist
    608                 // (all this is done from B's perspective. Maybe I should be averaging axes...)
    609                 btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
    610                 btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
    611                 btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
    612 
    613                 if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
    614                 {
    615                         btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
    616                         computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
    617 
    618                         if (swingAngle > swingLimit * m_limitSoftness)
     185
     186                        m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
     187                                getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
     188
     189                }       else
     190                        if (twist >  m_twistSpan*lockedFreeFactor)
    619191                        {
    620                                 m_solveSwingLimit = true;
    621 
    622                                 // compute limit ratio: 0->1, where
    623                                 // 0 == beginning of soft limit
    624                                 // 1 == hard/real limit
    625                                 m_swingLimitRatio = 1.f;
    626                                 if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
    627                                 {
    628                                         m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
    629                                                                                 (swingLimit - swingLimit * m_limitSoftness);
    630                                 }                               
    631 
    632                                 // swing correction tries to get back to soft limit
    633                                 m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
    634 
    635                                 // adjustment of swing axis (based on ellipse normal)
    636                                 adjustSwingAxisToUseEllipseNormal(swingAxis);
    637 
    638                                 // Calculate necessary axis & factors           
    639                                 m_swingAxis = quatRotate(qB, -swingAxis);
    640 
    641                                 m_twistAxisA.setValue(0,0,0);
    642 
    643                                 m_kSwing =  btScalar(1.) /
    644                                         (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
    645                                          getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
     192                                m_twistCorrection = (twist - m_twistSpan);
     193                                m_solveTwistLimit = true;
     194
     195                                m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
     196                                m_twistAxis.normalize();
     197
     198                                m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
     199                                        getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
     200
    646201                        }
    647                 }
    648                 else
    649                 {
    650                         // you haven't set any limits;
    651                         // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
    652                         // anyway, we have either hinge or fixed joint
    653                         btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
    654                         btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
    655                         btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
    656                         btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0);
    657                         btVector3 target;
    658                         btScalar x = ivB.dot(ivA);
    659                         btScalar y = ivB.dot(jvA);
    660                         btScalar z = ivB.dot(kvA);
    661                         if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
    662                         { // fixed. We'll need to add one more row to constraint
    663                                 if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
    664                                 {
    665                                         m_solveSwingLimit = true;
    666                                         m_swingAxis = -ivB.cross(ivA);
    667                                 }
    668                         }
    669                         else
    670                         {
    671                                 if(m_swingSpan1 < m_fixThresh)
    672                                 { // hinge around Y axis
    673                                         if(!(btFuzzyZero(y)))
    674                                         {
    675                                                 m_solveSwingLimit = true;
    676                                                 if(m_swingSpan2 >= m_fixThresh)
    677                                                 {
    678                                                         y = btScalar(0.f);
    679                                                         btScalar span2 = btAtan2(z, x);
    680                                                         if(span2 > m_swingSpan2)
    681                                                         {
    682                                                                 x = btCos(m_swingSpan2);
    683                                                                 z = btSin(m_swingSpan2);
    684                                                         }
    685                                                         else if(span2 < -m_swingSpan2)
    686                                                         {
    687                                                                 x =  btCos(m_swingSpan2);
    688                                                                 z = -btSin(m_swingSpan2);
    689                                                         }
    690                                                 }
    691                                         }
    692                                 }
    693                                 else
    694                                 { // hinge around Z axis
    695                                         if(!btFuzzyZero(z))
    696                                         {
    697                                                 m_solveSwingLimit = true;
    698                                                 if(m_swingSpan1 >= m_fixThresh)
    699                                                 {
    700                                                         z = btScalar(0.f);
    701                                                         btScalar span1 = btAtan2(y, x);
    702                                                         if(span1 > m_swingSpan1)
    703                                                         {
    704                                                                 x = btCos(m_swingSpan1);
    705                                                                 y = btSin(m_swingSpan1);
    706                                                         }
    707                                                         else if(span1 < -m_swingSpan1)
    708                                                         {
    709                                                                 x =  btCos(m_swingSpan1);
    710                                                                 y = -btSin(m_swingSpan1);
    711                                                         }
    712                                                 }
    713                                         }
    714                                 }
    715                                 target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
    716                                 target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
    717                                 target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
    718                                 target.normalize();
    719                                 m_swingAxis = -ivB.cross(target);
    720                                 m_swingCorrection = m_swingAxis.length();
    721                                 m_swingAxis.normalize();
    722                         }
    723                 }
    724 
    725                 if (m_twistSpan >= btScalar(0.f))
    726                 {
    727                         btVector3 twistAxis;
    728                         computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
    729 
    730                         if (m_twistAngle > m_twistSpan*m_limitSoftness)
    731                         {
    732                                 m_solveTwistLimit = true;
    733 
    734                                 m_twistLimitRatio = 1.f;
    735                                 if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
    736                                 {
    737                                         m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
    738                                                                                 (m_twistSpan  - m_twistSpan * m_limitSoftness);
    739                                 }
    740 
    741                                 // twist correction tries to get back to soft limit
    742                                 m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
    743 
    744                                 m_twistAxis = quatRotate(qB, -twistAxis);
    745 
    746                                 m_kTwist = btScalar(1.) /
    747                                         (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
    748                                          getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
    749                         }
    750 
    751                         if (m_solveSwingLimit)
    752                                 m_twistAxisA = quatRotate(qA, -twistAxis);
    753                 }
    754                 else
    755                 {
    756                         m_twistAngle = btScalar(0.f);
    757                 }
    758         }
    759 } // btConeTwistConstraint::calcAngleInfo2()
    760 
    761 
    762 
    763 // given a cone rotation in constraint space, (pre: twist must already be removed)
    764 // this method computes its corresponding swing angle and axis.
    765 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
    766 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
    767                                                                                                  btScalar& swingAngle, // out
    768                                                                                                  btVector3& vSwingAxis, // out
    769                                                                                                  btScalar& swingLimit) // out
    770 {
    771         swingAngle = qCone.getAngle();
    772         if (swingAngle > SIMD_EPSILON)
    773         {
    774                 vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
    775                 vSwingAxis.normalize();
    776                 if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
    777                 {
    778                         // non-zero twist?! this should never happen.
    779                         int wtf = 0; wtf = wtf;
    780                 }
    781 
    782                 // Compute limit for given swing. tricky:
    783                 // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
    784                 // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
    785 
    786                 // For starters, compute the direction from center to surface of ellipse.
    787                 // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
    788                 // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
    789                 btScalar xEllipse =  vSwingAxis.y();
    790                 btScalar yEllipse = -vSwingAxis.z();
    791 
    792                 // Now, we use the slope of the vector (using x/yEllipse) and find the length
    793                 // of the line that intersects the ellipse:
    794                 //  x^2   y^2
    795                 //  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
    796                 //  a^2   b^2
    797                 // Do the math and it should be clear.
    798 
    799                 swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
    800                 if (fabs(xEllipse) > SIMD_EPSILON)
    801                 {
    802                         btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
    803                         btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
    804                         norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
    805                         btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
    806                         swingLimit = sqrt(swingLimit2);
    807                 }
    808 
    809                 // test!
    810                 /*swingLimit = m_swingSpan2;
    811                 if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
    812                 {
    813                 btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
    814                 btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
    815                 btScalar phi = asin(sinphi);
    816                 btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
    817                 btScalar alpha = 3.14159f - theta - phi;
    818                 btScalar sinalpha = sin(alpha);
    819                 swingLimit = m_swingSpan1 * sinphi/sinalpha;
    820                 }*/
    821         }
    822         else if (swingAngle < 0)
    823         {
    824                 // this should never happen!
    825                 int wtf = 0; wtf = wtf;
    826         }
    827 }
    828 
    829 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
    830 {
    831         // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
    832         btScalar xEllipse = btCos(fAngleInRadians);
    833         btScalar yEllipse = btSin(fAngleInRadians);
    834 
    835         // Use the slope of the vector (using x/yEllipse) and find the length
    836         // of the line that intersects the ellipse:
    837         //  x^2   y^2
    838         //  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
    839         //  a^2   b^2
    840         // Do the math and it should be clear.
    841 
    842         float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
    843         if (fabs(xEllipse) > SIMD_EPSILON)
    844         {
    845                 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
    846                 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
    847                 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
    848                 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
    849                 swingLimit = sqrt(swingLimit2);
    850         }
    851 
    852         // convert into point in constraint space:
    853         // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
    854         btVector3 vSwingAxis(0, xEllipse, -yEllipse);
    855         btQuaternion qSwing(vSwingAxis, swingLimit);
    856         btVector3 vPointInConstraintSpace(fLength,0,0);
    857         return quatRotate(qSwing, vPointInConstraintSpace);
    858 }
    859 
    860 // given a twist rotation in constraint space, (pre: cone must already be removed)
    861 // this method computes its corresponding angle and axis.
    862 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
    863                                                                                                   btScalar& twistAngle, // out
    864                                                                                                   btVector3& vTwistAxis) // out
    865 {
    866         btQuaternion qMinTwist = qTwist;
    867         twistAngle = qTwist.getAngle();
    868 
    869         if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
    870         {
    871                 qMinTwist = operator-(qTwist);
    872                 twistAngle = qMinTwist.getAngle();
    873         }
    874         if (twistAngle < 0)
    875         {
    876                 // this should never happen
    877                 int wtf = 0; wtf = wtf;                 
    878         }
    879 
    880         vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
    881         if (twistAngle > SIMD_EPSILON)
    882                 vTwistAxis.normalize();
    883 }
    884 
    885 
    886 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
    887 {
    888         // the swing axis is computed as the "twist-free" cone rotation,
    889         // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
    890         // so, if we're outside the limits, the closest way back inside the cone isn't
    891         // along the vector back to the center. better (and more stable) to use the ellipse normal.
    892 
    893         // convert swing axis to direction from center to surface of ellipse
    894         // (ie. rotate 2D vector by PI/2)
    895         btScalar y = -vSwingAxis.z();
    896         btScalar z =  vSwingAxis.y();
    897 
    898         // do the math...
    899         if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
    900         {
    901                 // compute gradient/normal of ellipse surface at current "point"
    902                 btScalar grad = y/z;
    903                 grad *= m_swingSpan2 / m_swingSpan1;
    904 
    905                 // adjust y/z to represent normal at point (instead of vector to point)
    906                 if (y > 0)
    907                         y =  fabs(grad * z);
    908                 else
    909                         y = -fabs(grad * z);
    910 
    911                 // convert ellipse direction back to swing axis
    912                 vSwingAxis.setZ(-y);
    913                 vSwingAxis.setY( z);
    914                 vSwingAxis.normalize();
    915         }
    916 }
    917 
    918 
    919 
    920 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
    921 {
    922         btTransform trACur = m_rbA.getCenterOfMassTransform();
    923         btTransform trBCur = m_rbB.getCenterOfMassTransform();
    924         btTransform trABCur = trBCur.inverse() * trACur;
    925         btQuaternion qABCur = trABCur.getRotation();
    926         btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
    927         btQuaternion qConstraintCur = trConstraintCur.getRotation();
    928 
    929         btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
    930         setMotorTargetInConstraintSpace(qConstraint);
    931 }
    932 
    933 
    934 void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
    935 {
    936         m_qTarget = q;
    937 
    938         // clamp motor target to within limits
    939         {
    940                 btScalar softness = 1.f;//m_limitSoftness;
    941 
    942                 // split into twist and cone
    943                 btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
    944                 btQuaternion qTargetCone  = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
    945                 btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
    946 
    947                 // clamp cone
    948                 if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
    949                 {
    950                         btScalar swingAngle, swingLimit; btVector3 swingAxis;
    951                         computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
    952 
    953                         if (fabs(swingAngle) > SIMD_EPSILON)
    954                         {
    955                                 if (swingAngle > swingLimit*softness)
    956                                         swingAngle = swingLimit*softness;
    957                                 else if (swingAngle < -swingLimit*softness)
    958                                         swingAngle = -swingLimit*softness;
    959                                 qTargetCone = btQuaternion(swingAxis, swingAngle);
    960                         }
    961                 }
    962 
    963                 // clamp twist
    964                 if (m_twistSpan >= btScalar(0.05f))
    965                 {
    966                         btScalar twistAngle; btVector3 twistAxis;
    967                         computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
    968 
    969                         if (fabs(twistAngle) > SIMD_EPSILON)
    970                         {
    971                                 // eddy todo: limitSoftness used here???
    972                                 if (twistAngle > m_twistSpan*softness)
    973                                         twistAngle = m_twistSpan*softness;
    974                                 else if (twistAngle < -m_twistSpan*softness)
    975                                         twistAngle = -m_twistSpan*softness;
    976                                 qTargetTwist = btQuaternion(twistAxis, twistAngle);
    977                         }
    978                 }
    979 
    980                 m_qTarget = qTargetCone * qTargetTwist;
    981         }
    982 }
    983 
    984 
    985 //-----------------------------------------------------------------------------
    986 //-----------------------------------------------------------------------------
    987 //-----------------------------------------------------------------------------
    988 
    989 
     202        }
     203}
     204
     205void    btConeTwistConstraint::solveConstraint(btScalar timeStep)
     206{
     207
     208        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
     209        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
     210
     211        btScalar tau = btScalar(0.3);
     212
     213        //linear part
     214        if (!m_angularOnly)
     215        {
     216                btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
     217                btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
     218
     219                btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
     220                btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
     221                btVector3 vel = vel1 - vel2;
     222
     223                for (int i=0;i<3;i++)
     224                {               
     225                        const btVector3& normal = m_jac[i].m_linearJointAxis;
     226                        btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
     227
     228                        btScalar rel_vel;
     229                        rel_vel = normal.dot(vel);
     230                        //positional error (zeroth order error)
     231                        btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
     232                        btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
     233                        m_appliedImpulse += impulse;
     234                        btVector3 impulse_vector = normal * impulse;
     235                        m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
     236                        m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
     237                }
     238        }
     239       
     240        {
     241                ///solve angular part
     242                const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
     243                const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
     244
     245                // solve swing limit
     246                if (m_solveSwingLimit)
     247                {
     248                        btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
     249                        btScalar impulseMag = amplitude * m_kSwing;
     250
     251                        // Clamp the accumulated impulse
     252                        btScalar temp = m_accSwingLimitImpulse;
     253                        m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
     254                        impulseMag = m_accSwingLimitImpulse - temp;
     255
     256                        btVector3 impulse = m_swingAxis * impulseMag;
     257
     258                        m_rbA.applyTorqueImpulse(impulse);
     259                        m_rbB.applyTorqueImpulse(-impulse);
     260
     261                }
     262
     263                // solve twist limit
     264                if (m_solveTwistLimit)
     265                {
     266                        btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
     267                        btScalar impulseMag = amplitude * m_kTwist;
     268
     269                        // Clamp the accumulated impulse
     270                        btScalar temp = m_accTwistLimitImpulse;
     271                        m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
     272                        impulseMag = m_accTwistLimitImpulse - temp;
     273
     274                        btVector3 impulse = m_twistAxis * impulseMag;
     275
     276                        m_rbA.applyTorqueImpulse(impulse);
     277                        m_rbB.applyTorqueImpulse(-impulse);
     278
     279                }
     280       
     281        }
     282
     283}
     284
     285void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
     286{
     287        (void)timeStep;
     288
     289}

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

    r2907 r2908  
    4343        btScalar        m_relaxationFactor;
    4444
    45         btScalar        m_damping;
    46 
    4745        btScalar        m_swingSpan1;
    4846        btScalar        m_swingSpan2;
    4947        btScalar        m_twistSpan;
    50 
    51         btScalar        m_fixThresh;
    5248
    5349        btVector3   m_swingAxis;
    … …  
    6157        btScalar        m_twistCorrection;
    6258
    63         btScalar        m_twistAngle;
    64 
    6559        btScalar        m_accSwingLimitImpulse;
    6660        btScalar        m_accTwistLimitImpulse;
    … …  
    7064        bool            m_solveSwingLimit;
    7165
    72         bool    m_useSolveConstraintObsolete;
    73 
    74         // not yet used...
    75         btScalar        m_swingLimitRatio;
    76         btScalar        m_twistLimitRatio;
    77         btVector3   m_twistAxisA;
    78 
    79         // motor
    80         bool             m_bMotorEnabled;
    81         bool             m_bNormalizedMotorStrength;
    82         btQuaternion m_qTarget;
    83         btScalar         m_maxMotorImpulse;
    84         btVector3        m_accMotorImpulse;
    8566       
    8667public:
    … …  
    9475        virtual void    buildJacobian();
    9576
    96         virtual void getInfo1 (btConstraintInfo1* info);
    97        
    98         virtual void getInfo2 (btConstraintInfo2* info);
    99        
    100 
    101         virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     77        virtual void    solveConstraint(btScalar        timeStep);
    10278
    10379        void    updateRHS(btScalar      timeStep);
    … …  
    11793        }
    11894
    119         void    setLimit(int limitIndex,btScalar limitValue)
    120         {
    121                 switch (limitIndex)
    122                 {
    123                 case 3:
    124                         {
    125                                 m_twistSpan = limitValue;
    126                                 break;
    127                         }
    128                 case 4:
    129                         {
    130                                 m_swingSpan2 = limitValue;
    131                                 break;
    132                         }
    133                 case 5:
    134                         {
    135                                 m_swingSpan1 = limitValue;
    136                                 break;
    137                         }
    138                 default:
    139                         {
    140                         }
    141                 };
    142         }
    143 
    144         void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
     95        void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
    14596        {
    14697                m_swingSpan1 = _swingSpan1;
    … …  
    171122        }
    172123
    173         void calcAngleInfo();
    174         void calcAngleInfo2();
    175 
    176         inline btScalar getSwingSpan1()
    177         {
    178                 return m_swingSpan1;
    179         }
    180         inline btScalar getSwingSpan2()
    181         {
    182                 return m_swingSpan2;
    183         }
    184         inline btScalar getTwistSpan()
    185         {
    186                 return m_twistSpan;
    187         }
    188         inline btScalar getTwistAngle()
    189         {
    190                 return m_twistAngle;
    191         }
    192         bool isPastSwingLimit() { return m_solveSwingLimit; }
    193 
    194 
    195         void setDamping(btScalar damping) { m_damping = damping; }
    196 
    197         void enableMotor(bool b) { m_bMotorEnabled = b; }
    198         void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
    199         void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
    200 
    201         btScalar getFixThresh() { return m_fixThresh; }
    202         void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
    203 
    204         // setMotorTarget:
    205         // q: the desired rotation of bodyA wrt bodyB.
    206         // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
    207         // note: don't forget to enableMotor()
    208         void setMotorTarget(const btQuaternion &q);
    209 
    210         // same as above, but q is the desired rotation of frameA wrt frameB in constraint space
    211         void setMotorTargetInConstraintSpace(const btQuaternion &q);
    212 
    213         btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
    214 
    215 
    216 
    217 protected:
    218         void init();
    219 
    220         void computeConeLimitInfo(const btQuaternion& qCone, // in
    221                 btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
    222 
    223         void computeTwistLimitInfo(const btQuaternion& qTwist, // in
    224                 btScalar& twistAngle, btVector3& vTwistAxis); // all outs
    225 
    226         void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
    227124};
    228125

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

    r2907 r2908  
    2323#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
    2424
    25 #define ASSERT2 btAssert
     25#define ASSERT2 assert
    2626
    2727#define USE_INTERNAL_APPLY_IMPULSE 1
    … …  
    5353       
    5454
    55            btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
     55          btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
    5656                body2.getCenterOfMassTransform().getBasis().transpose(),
    5757                rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
    … …  
    115115
    116116        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    117         btAssert(cpd);
     117        assert(cpd);
    118118        btScalar distance = cpd->m_penetration;
    119119        btScalar positionalError = Kcor *-distance;
    … …  
    167167       
    168168        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    169         btAssert(cpd);
     169        assert(cpd);
    170170
    171171        btScalar combinedFriction = cpd->m_friction;
    … …  
    256256       
    257257        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    258         btAssert(cpd);
     258        assert(cpd);
    259259
    260260        btScalar combinedFriction = cpd->m_friction;
    … …  
    338338
    339339        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    340         btAssert(cpd);
     340        assert(cpd);
    341341        btScalar distance = cpd->m_penetration;
    342342        btScalar positionalError = Kcor *-distance;

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

    r2907 r2908  
    2323        SOLVER_USE_WARMSTARTING = 4,
    2424        SOLVER_USE_FRICTION_WARMSTARTING = 8,
    25         SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
    26         SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
    27         SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
    28         SOLVER_CACHE_FRIENDLY = 128,
    29         SOLVER_SIMD = 256,      //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
    30         SOLVER_CUDA = 512       //will be open sourced during Game Developers Conference 2009. Much faster.
     25        SOLVER_CACHE_FRIENDLY = 16
    3126};
    3227
    … …  
    4540        btScalar        m_erp;//used as Baumgarte factor
    4641        btScalar        m_erp2;//used in Split Impulse
    47         btScalar        m_globalCfm;//constraint force mixing
    4842        int                     m_splitImpulse;
    4943        btScalar        m_splitImpulsePenetrationThreshold;
    … …  
    7266                m_erp = btScalar(0.2);
    7367                m_erp2 = btScalar(0.1);
    74                 m_globalCfm = btScalar(0.);
    75                 m_sor = btScalar(1.);
     68                m_sor = btScalar(1.3);
    7669                m_splitImpulse = false;
    7770                m_splitImpulsePenetrationThreshold = -0.02f;
    7871                m_linearSlop = btScalar(0.0);
    7972                m_warmstartingFactor=btScalar(0.85);
    80                 m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER
     73                m_solverMode = SOLVER_CACHE_FRIENDLY |  SOLVER_RANDMIZE_ORDER |  SOLVER_USE_WARMSTARTING;
    8174                m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
    8275        }

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

    r2907 r2908  
    2020*/
    2121
     22
    2223#include "btGeneric6DofConstraint.h"
    2324#include "BulletDynamics/Dynamics/btRigidBody.h"
    … …  
    2627
    2728
    28 #define D6_USE_OBSOLETE_METHOD false
    29 //-----------------------------------------------------------------------------
    30 
    31 btGeneric6DofConstraint::btGeneric6DofConstraint()
    32 :btTypedConstraint(D6_CONSTRAINT_TYPE),
    33 m_useLinearReferenceFrameA(true),
    34 m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
    35 {
    36 }
    37 
    38 //-----------------------------------------------------------------------------
    39 
    40 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
    41 : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
    42 , m_frameInA(frameInA)
    43 , m_frameInB(frameInB),
    44 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
    45 m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
    46 {
    47 
    48 }
    49 //-----------------------------------------------------------------------------
    50 
    51 
    5229#define GENERIC_D6_DISABLE_WARMSTARTING 1
    53 
    54 //-----------------------------------------------------------------------------
    5530
    5631btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
    … …  
    6237}
    6338
    64 //-----------------------------------------------------------------------------
    65 
    6639///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
    6740bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
    6841bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
    6942{
    70         //      // rot =  cy*cz          -cy*sz           sy
    71         //      //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
    72         //      //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
    73         //
    74 
    75         btScalar fi = btGetMatrixElem(mat,2);
    76         if (fi < btScalar(1.0f))
    77         {
    78                 if (fi > btScalar(-1.0f))
     43//      // rot =  cy*cz          -cy*sz           sy
     44//      //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
     45//      //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
     46//
     47
     48                if (btGetMatrixElem(mat,2) < btScalar(1.0))
    7949                {
    80                         xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
    81                         xyz[1] = btAsin(btGetMatrixElem(mat,2));
    82                         xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
    83                         return true;
     50                        if (btGetMatrixElem(mat,2) > btScalar(-1.0))
     51                        {
     52                                xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
     53                                xyz[1] = btAsin(btGetMatrixElem(mat,2));
     54                                xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
     55                                return true;
     56                        }
     57                        else
     58                        {
     59                                // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
     60                                xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
     61                                xyz[1] = -SIMD_HALF_PI;
     62                                xyz[2] = btScalar(0.0);
     63                                return false;
     64                        }
    8465                }
    8566                else
    8667                {
    87                         // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
    88                         xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
    89                         xyz[1] = -SIMD_HALF_PI;
    90                         xyz[2] = btScalar(0.0);
    91                         return false;
     68                        // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
     69                        xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
     70                        xyz[1] = SIMD_HALF_PI;
     71                        xyz[2] = 0.0;
     72
    9273                }
    93         }
    94         else
    95         {
    96                 // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
    97                 xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
    98                 xyz[1] = SIMD_HALF_PI;
    99                 xyz[2] = 0.0;
    100         }
     74
     75
    10176        return false;
    10277}
    10378
     79
     80
    10481//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
     82
    10583
    10684int btRotationalLimitMotor::testLimitValue(btScalar test_value)
    … …  
    127105        m_currentLimit = 0;//Free from violation
    128106        return 0;
    129 
    130 }
    131 
    132 //-----------------------------------------------------------------------------
     107       
     108}
     109
    133110
    134111btScalar btRotationalLimitMotor::solveAngularLimits(
    135         btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
    136         btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
    137 {
    138         if (needApplyTorques()==false) return 0.0f;
    139 
    140         btScalar target_velocity = m_targetVelocity;
    141         btScalar maxMotorForce = m_maxMotorForce;
     112                btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
     113                btRigidBody * body0, btRigidBody * body1)
     114{
     115    if (needApplyTorques()==false) return 0.0f;
     116
     117    btScalar target_velocity = m_targetVelocity;
     118    btScalar maxMotorForce = m_maxMotorForce;
    142119
    143120        //current error correction
    144         if (m_currentLimit!=0)
    145         {
    146                 target_velocity = -m_ERP*m_currentLimitError/(timeStep);
    147                 maxMotorForce = m_maxLimitForce;
    148         }
    149 
    150         maxMotorForce *= timeStep;
    151 
    152         // current velocity difference
    153 
    154         btVector3 angVelA;
    155         bodyA.getAngularVelocity(angVelA);
    156         btVector3 angVelB;
    157         bodyB.getAngularVelocity(angVelB);
    158 
    159         btVector3 vel_diff;
    160         vel_diff = angVelA-angVelB;
    161 
    162 
    163 
    164         btScalar rel_vel = axis.dot(vel_diff);
     121    if (m_currentLimit!=0)
     122    {
     123        target_velocity = -m_ERP*m_currentLimitError/(timeStep);
     124        maxMotorForce = m_maxLimitForce;
     125    }
     126
     127    maxMotorForce *= timeStep;
     128
     129    // current velocity difference
     130    btVector3 vel_diff = body0->getAngularVelocity();
     131    if (body1)
     132    {
     133        vel_diff -= body1->getAngularVelocity();
     134    }
     135
     136
     137
     138    btScalar rel_vel = axis.dot(vel_diff);
    165139
    166140        // correction velocity
    167         btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
    168 
    169 
    170         if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
    171         {
    172                 return 0.0f;//no need for applying force
    173         }
     141    btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
     142
     143
     144    if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
     145    {
     146        return 0.0f;//no need for applying force
     147    }
    174148
    175149
    176150        // correction impulse
    177         btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
     151    btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
    178152
    179153        // clip correction impulse
    180         btScalar clippedMotorImpulse;
    181 
    182         ///@todo: should clip against accumulated impulse
    183         if (unclippedMotorImpulse>0.0f)
    184         {
    185                 clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
    186         }
    187         else
    188         {
    189                 clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
    190         }
     154    btScalar clippedMotorImpulse;
     155
     156    ///@todo: should clip against accumulated impulse
     157    if (unclippedMotorImpulse>0.0f)
     158    {
     159        clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
     160    }
     161    else
     162    {
     163        clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
     164    }
    191165
    192166
    193167        // sort with accumulated impulses
    194         btScalar        lo = btScalar(-1e30);
    195         btScalar        hi = btScalar(1e30);
    196 
    197         btScalar oldaccumImpulse = m_accumulatedImpulse;
    198         btScalar sum = oldaccumImpulse + clippedMotorImpulse;
    199         m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
    200 
    201         clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
    202 
    203         btVector3 motorImp = clippedMotorImpulse * axis;
    204 
    205         //body0->applyTorqueImpulse(motorImp);
    206         //body1->applyTorqueImpulse(-motorImp);
    207 
    208         bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
    209         bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
    210 
    211 
    212         return clippedMotorImpulse;
     168    btScalar    lo = btScalar(-1e30);
     169    btScalar    hi = btScalar(1e30);
     170
     171    btScalar oldaccumImpulse = m_accumulatedImpulse;
     172    btScalar sum = oldaccumImpulse + clippedMotorImpulse;
     173    m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
     174
     175    clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
     176
     177
     178
     179    btVector3 motorImp = clippedMotorImpulse * axis;
     180
     181
     182    body0->applyTorqueImpulse(motorImp);
     183    if (body1) body1->applyTorqueImpulse(-motorImp);
     184
     185    return clippedMotorImpulse;
    213186
    214187
    … …  
    217190//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
    218191
    219 
    220 
    221 
    222192//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
    223 
    224 
    225 int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
    226 {
    227         btScalar loLimit = m_lowerLimit[limitIndex];
    228         btScalar hiLimit = m_upperLimit[limitIndex];
    229         if(loLimit > hiLimit)
    230         {
    231                 m_currentLimit[limitIndex] = 0;//Free from violation
    232                 m_currentLimitError[limitIndex] = btScalar(0.f);
    233                 return 0;
    234         }
    235 
    236         if (test_value < loLimit)
    237         {
    238                 m_currentLimit[limitIndex] = 2;//low limit violation
    239                 m_currentLimitError[limitIndex] =  test_value - loLimit;
    240                 return 2;
    241         }
    242         else if (test_value> hiLimit)
    243         {
    244                 m_currentLimit[limitIndex] = 1;//High limit violation
    245                 m_currentLimitError[limitIndex] = test_value - hiLimit;
    246                 return 1;
    247         };
    248 
    249         m_currentLimit[limitIndex] = 0;//Free from violation
    250         m_currentLimitError[limitIndex] = btScalar(0.f);
    251         return 0;
    252 } // btTranslationalLimitMotor::testLimitValue()
    253 
    254 //-----------------------------------------------------------------------------
    255 
    256193btScalar btTranslationalLimitMotor::solveLinearAxis(
    257         btScalar timeStep,
    258         btScalar jacDiagABInv,
    259         btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
    260         btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
    261         int limit_index,
    262         const btVector3 & axis_normal_on_a,
    263         const btVector3 & anchorPos)
    264 {
    265 
    266         ///find relative velocity
    267         //    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
    268         //    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
    269         btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
    270         btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
    271 
    272         btVector3 vel1;
    273         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
    274         btVector3 vel2;
    275         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
    276         btVector3 vel = vel1 - vel2;
    277 
    278         btScalar rel_vel = axis_normal_on_a.dot(vel);
    279 
    280 
    281 
    282         /// apply displacement correction
    283 
    284         //positional error (zeroth order error)
    285         btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
    286         btScalar        lo = btScalar(-1e30);
    287         btScalar        hi = btScalar(1e30);
    288 
    289         btScalar minLimit = m_lowerLimit[limit_index];
    290         btScalar maxLimit = m_upperLimit[limit_index];
    291 
    292         //handle the limits
    293         if (minLimit < maxLimit)
    294         {
    295                 {
    296                         if (depth > maxLimit)
    297                         {
    298                                 depth -= maxLimit;
    299                                 lo = btScalar(0.);
    300 
    301                         }
    302                         else
    303                         {
    304                                 if (depth < minLimit)
    305                                 {
    306                                         depth -= minLimit;
    307                                         hi = btScalar(0.);
    308                                 }
    309                                 else
    310                                 {
    311                                         return 0.0f;
    312                                 }
    313                         }
    314                 }
    315         }
    316 
    317         btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
    318 
    319 
    320 
    321 
    322         btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
    323         btScalar sum = oldNormalImpulse + normalImpulse;
    324         m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
    325         normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
    326 
    327         btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
    328         //body1.applyImpulse( impulse_vector, rel_pos1);
    329         //body2.applyImpulse(-impulse_vector, rel_pos2);
    330 
    331         btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
    332         btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
    333         bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
    334         bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
    335 
    336 
    337 
    338 
    339         return normalImpulse;
     194                btScalar timeStep,
     195        btScalar jacDiagABInv,
     196        btRigidBody& body1,const btVector3 &pointInA,
     197        btRigidBody& body2,const btVector3 &pointInB,
     198        int limit_index,
     199        const btVector3 & axis_normal_on_a,
     200                const btVector3 & anchorPos)
     201{
     202
     203///find relative velocity
     204//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
     205//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
     206    btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
     207    btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
     208
     209    btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
     210    btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
     211    btVector3 vel = vel1 - vel2;
     212
     213    btScalar rel_vel = axis_normal_on_a.dot(vel);
     214
     215
     216
     217/// apply displacement correction
     218
     219//positional error (zeroth order error)
     220    btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
     221    btScalar    lo = btScalar(-1e30);
     222    btScalar    hi = btScalar(1e30);
     223
     224    btScalar minLimit = m_lowerLimit[limit_index];
     225    btScalar maxLimit = m_upperLimit[limit_index];
     226
     227    //handle the limits
     228    if (minLimit < maxLimit)
     229    {
     230        {
     231            if (depth > maxLimit)
     232            {
     233                depth -= maxLimit;
     234                lo = btScalar(0.);
     235
     236            }
     237            else
     238            {
     239                if (depth < minLimit)
     240                {
     241                    depth -= minLimit;
     242                    hi = btScalar(0.);
     243                }
     244                else
     245                {
     246                    return 0.0f;
     247                }
     248            }
     249        }
     250    }
     251
     252    btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
     253
     254
     255
     256
     257    btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
     258    btScalar sum = oldNormalImpulse + normalImpulse;
     259    m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
     260    normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
     261
     262    btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
     263    body1.applyImpulse( impulse_vector, rel_pos1);
     264    body2.applyImpulse(-impulse_vector, rel_pos2);
     265    return normalImpulse;
    340266}
    341267
    342268//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
    343269
     270
     271btGeneric6DofConstraint::btGeneric6DofConstraint()
     272        :btTypedConstraint(D6_CONSTRAINT_TYPE),
     273                m_useLinearReferenceFrameA(true)
     274{
     275}
     276
     277btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
     278        : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
     279        , m_frameInA(frameInA)
     280        , m_frameInB(frameInB),
     281                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
     282{
     283
     284}
     285
     286
     287
     288
     289
    344290void btGeneric6DofConstraint::calculateAngleInfo()
    345291{
    346292        btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
     293
    347294        matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
     295
     296
     297
    348298        // in euler angle mode we do not actually constrain the angular velocity
    349         // along the axes axis[0] and axis[2] (although we do use axis[1]) :
    350         //
    351         //    to get                    constrain w2-w1 along           ...not
    352         //    ------                    ---------------------           ------
    353         //    d(angle[0])/dt = 0        ax[1] x ax[2]                   ax[0]
    354         //    d(angle[1])/dt = 0        ax[1]
    355         //    d(angle[2])/dt = 0        ax[0] x ax[1]                   ax[2]
    356         //
    357         // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
    358         // to prove the result for angle[0], write the expression for angle[0] from
    359         // GetInfo1 then take the derivative. to prove this for angle[2] it is
    360         // easier to take the euler rate expression for d(angle[2])/dt with respect
    361         // to the components of w and set that to 0.
     299  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
     300  //
     301  //    to get                  constrain w2-w1 along           ...not
     302  //    ------                  ---------------------           ------
     303  //    d(angle[0])/dt = 0      ax[1] x ax[2]                   ax[0]
     304  //    d(angle[1])/dt = 0      ax[1]
     305  //    d(angle[2])/dt = 0      ax[0] x ax[1]                   ax[2]
     306  //
     307  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
     308  // to prove the result for angle[0], write the expression for angle[0] from
     309  // GetInfo1 then take the derivative. to prove this for angle[2] it is
     310  // easier to take the euler rate expression for d(angle[2])/dt with respect
     311  // to the components of w and set that to 0.
     312
    362313        btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
    363314        btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
    … …  
    367318        m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
    368319
    369         m_calculatedAxis[0].normalize();
    370         m_calculatedAxis[1].normalize();
    371         m_calculatedAxis[2].normalize();
    372 
    373 }
    374 
    375 //-----------------------------------------------------------------------------
     320
     321//    if(m_debugDrawer)
     322//    {
     323//
     324//      char buff[300];
     325//              sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
     326//              m_calculatedAxisAngleDiff[0],
     327//              m_calculatedAxisAngleDiff[1],
     328//              m_calculatedAxisAngleDiff[2]);
     329//      m_debugDrawer->reportErrorWarning(buff);
     330//    }
     331
     332}
    376333
    377334void btGeneric6DofConstraint::calculateTransforms()
    378335{
    379         m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
    380         m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
    381         calculateLinearInfo();
    382         calculateAngleInfo();
    383 }
    384 
    385 //-----------------------------------------------------------------------------
     336    m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
     337    m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
     338
     339    calculateAngleInfo();
     340}
     341
    386342
    387343void btGeneric6DofConstraint::buildLinearJacobian(
    388         btJacobianEntry & jacLinear,const btVector3 & normalWorld,
    389         const btVector3 & pivotAInW,const btVector3 & pivotBInW)
    390 {
    391         new (&jacLinear) btJacobianEntry(
     344    btJacobianEntry & jacLinear,const btVector3 & normalWorld,
     345    const btVector3 & pivotAInW,const btVector3 & pivotBInW)
     346{
     347    new (&jacLinear) btJacobianEntry(
    392348        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    393349        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    399355        m_rbB.getInvInertiaDiagLocal(),
    400356        m_rbB.getInvMass());
    401 }
    402 
    403 //-----------------------------------------------------------------------------
     357
     358}
    404359
    405360void btGeneric6DofConstraint::buildAngularJacobian(
    406         btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
    407 {
    408         new (&jacAngular)      btJacobianEntry(jointAxisW,
     361    btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
     362{
     363    new (&jacAngular)   btJacobianEntry(jointAxisW,
    409364                                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    410365                                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    414369}
    415370
    416 //-----------------------------------------------------------------------------
    417 
    418371bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
    419372{
    420         btScalar angle = m_calculatedAxisAngleDiff[axis_index];
    421         //test limits
    422         m_angularLimits[axis_index].testLimitValue(angle);
    423         return m_angularLimits[axis_index].needApplyTorques();
    424 }
    425 
    426 //-----------------------------------------------------------------------------
     373    btScalar angle = m_calculatedAxisAngleDiff[axis_index];
     374
     375    //test limits
     376    m_angularLimits[axis_index].testLimitValue(angle);
     377    return m_angularLimits[axis_index].needApplyTorques();
     378}
    427379
    428380void btGeneric6DofConstraint::buildJacobian()
    429381{
    430         if (m_useSolveConstraintObsolete)
    431         {
    432 
    433                 // Clear accumulated impulses for the next simulation step
    434                 m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
    435                 int i;
    436                 for(i = 0; i < 3; i++)
    437                 {
    438                         m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
    439                 }
    440                 //calculates transform
    441                 calculateTransforms();
    442 
    443                 //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
    444                 //  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
    445                 calcAnchorPos();
    446                 btVector3 pivotAInW = m_AnchorPos;
    447                 btVector3 pivotBInW = m_AnchorPos;
    448 
    449                 // not used here
    450                 //    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
    451                 //    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    452 
    453                 btVector3 normalWorld;
    454                 //linear part
    455                 for (i=0;i<3;i++)
    456                 {
    457                         if (m_linearLimits.isLimited(i))
    458                         {
    459                                 if (m_useLinearReferenceFrameA)
    460                                         normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
    461                                 else
    462                                         normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
    463 
    464                                 buildLinearJacobian(
    465                                         m_jacLinear[i],normalWorld ,
    466                                         pivotAInW,pivotBInW);
    467 
    468                         }
    469                 }
    470 
    471                 // angular part
    472                 for (i=0;i<3;i++)
    473                 {
    474                         //calculates error angle
    475                         if (testAngularLimitMotor(i))
    476                         {
    477                                 normalWorld = this->getAxis(i);
    478                                 // Create angular atom
    479                                 buildAngularJacobian(m_jacAng[i],normalWorld);
    480                         }
    481                 }
    482 
    483         }
    484 }
    485 
    486 //-----------------------------------------------------------------------------
    487 
    488 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
    489 {
    490         if (m_useSolveConstraintObsolete)
    491         {
    492                 info->m_numConstraintRows = 0;
    493                 info->nub = 0;
    494         } else
    495         {
    496                 //prepare constraint
    497                 calculateTransforms();
    498                 info->m_numConstraintRows = 0;
    499                 info->nub = 6;
    500                 int i;
    501                 //test linear limits
    502                 for(i = 0; i < 3; i++)
    503                 {
    504                         if(m_linearLimits.needApplyForce(i))
    505                         {
    506                                 info->m_numConstraintRows++;
    507                                 info->nub--;
    508                         }
    509                 }
    510                 //test angular limits
    511                 for (i=0;i<3 ;i++ )
    512                 {
    513                         if(testAngularLimitMotor(i))
    514                         {
    515                                 info->m_numConstraintRows++;
    516                                 info->nub--;
    517                         }
    518                 }
    519         }
    520 }
    521 
    522 //-----------------------------------------------------------------------------
    523 
    524 void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
    525 {
    526         btAssert(!m_useSolveConstraintObsolete);
    527         int row = setLinearLimits(info);
    528         setAngularLimits(info, row);
    529 }
    530 
    531 //-----------------------------------------------------------------------------
    532 
    533 int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
    534 {
    535         btGeneric6DofConstraint * d6constraint = this;
    536         int row = 0;
    537         //solve linear limits
    538         btRotationalLimitMotor limot;
    539         for (int i=0;i<3 ;i++ )
    540         {
    541                 if(m_linearLimits.needApplyForce(i))
    542                 { // re-use rotational motor code
    543                         limot.m_bounce = btScalar(0.f);
    544                         limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
    545                         limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
    546                         limot.m_damping  = m_linearLimits.m_damping;
    547                         limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
    548                         limot.m_ERP  = m_linearLimits.m_restitution;
    549                         limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
    550                         limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
    551                         limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
    552                         limot.m_maxLimitForce  = btScalar(0.f);
    553                         limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
    554                         limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
    555                         btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
    556                         row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
    557                 }
    558         }
    559         return row;
    560 }
    561 
    562 //-----------------------------------------------------------------------------
    563 
    564 int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
    565 {
    566         btGeneric6DofConstraint * d6constraint = this;
    567         int row = row_offset;
    568         //solve angular limits
    569         for (int i=0;i<3 ;i++ )
    570         {
    571                 if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
    572                 {
    573                         btVector3 axis = d6constraint->getAxis(i);
    574                         row += get_limit_motor_info2(
    575                                 d6constraint->getRotationalLimitMotor(i),
    576                                 &m_rbA,
    577                                 &m_rbB,
    578                                 info,row,axis,1);
    579                 }
    580         }
    581 
    582         return row;
    583 }
    584 
    585 //-----------------------------------------------------------------------------
    586 
    587 void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar  timeStep)
    588 {
    589         if (m_useSolveConstraintObsolete)
    590         {
    591 
    592 
    593                 m_timeStep = timeStep;
    594 
    595                 //calculateTransforms();
    596 
    597                 int i;
    598 
    599                 // linear
    600 
    601                 btVector3 pointInA = m_calculatedTransformA.getOrigin();
    602                 btVector3 pointInB = m_calculatedTransformB.getOrigin();
    603 
    604                 btScalar jacDiagABInv;
    605                 btVector3 linear_axis;
    606                 for (i=0;i<3;i++)
    607                 {
    608                         if (m_linearLimits.isLimited(i))
    609                         {
    610                                 jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
    611 
    612                                 if (m_useLinearReferenceFrameA)
    613                                         linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
    614                                 else
    615                                         linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
    616 
    617                                 m_linearLimits.solveLinearAxis(
    618                                         m_timeStep,
    619                                         jacDiagABInv,
    620                                         m_rbA,bodyA,pointInA,
    621                                         m_rbB,bodyB,pointInB,
    622                                         i,linear_axis, m_AnchorPos);
    623 
    624                         }
    625                 }
    626 
    627                 // angular
    628                 btVector3 angular_axis;
    629                 btScalar angularJacDiagABInv;
    630                 for (i=0;i<3;i++)
    631                 {
    632                         if (m_angularLimits[i].needApplyTorques())
    633                         {
    634 
    635                                 // get axis
    636                                 angular_axis = getAxis(i);
    637 
    638                                 angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
    639 
    640                                 m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
    641                         }
    642                 }
    643         }
    644 }
    645 
    646 //-----------------------------------------------------------------------------
     382
     383        // Clear accumulated impulses for the next simulation step
     384    m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
     385    int i;
     386    for(i = 0; i < 3; i++)
     387    {
     388        m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
     389    }
     390    //calculates transform
     391    calculateTransforms();
     392
     393//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
     394//  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
     395        calcAnchorPos();
     396        btVector3 pivotAInW = m_AnchorPos;
     397        btVector3 pivotBInW = m_AnchorPos;
     398
     399// not used here
     400//    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
     401//    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
     402
     403    btVector3 normalWorld;
     404    //linear part
     405    for (i=0;i<3;i++)
     406    {
     407        if (m_linearLimits.isLimited(i))
     408        {
     409                        if (m_useLinearReferenceFrameA)
     410                    normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
     411                        else
     412                    normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
     413
     414            buildLinearJacobian(
     415                m_jacLinear[i],normalWorld ,
     416                pivotAInW,pivotBInW);
     417
     418        }
     419    }
     420
     421    // angular part
     422    for (i=0;i<3;i++)
     423    {
     424        //calculates error angle
     425        if (testAngularLimitMotor(i))
     426        {
     427            normalWorld = this->getAxis(i);
     428            // Create angular atom
     429            buildAngularJacobian(m_jacAng[i],normalWorld);
     430        }
     431    }
     432
     433
     434}
     435
     436
     437void btGeneric6DofConstraint::solveConstraint(btScalar  timeStep)
     438{
     439    m_timeStep = timeStep;
     440
     441    //calculateTransforms();
     442
     443    int i;
     444
     445    // linear
     446
     447    btVector3 pointInA = m_calculatedTransformA.getOrigin();
     448        btVector3 pointInB = m_calculatedTransformB.getOrigin();
     449
     450        btScalar jacDiagABInv;
     451        btVector3 linear_axis;
     452    for (i=0;i<3;i++)
     453    {
     454        if (m_linearLimits.isLimited(i))
     455        {
     456            jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
     457
     458                        if (m_useLinearReferenceFrameA)
     459                    linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
     460                        else
     461                    linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
     462
     463            m_linearLimits.solveLinearAxis(
     464                m_timeStep,
     465                jacDiagABInv,
     466                m_rbA,pointInA,
     467                m_rbB,pointInB,
     468                i,linear_axis, m_AnchorPos);
     469
     470        }
     471    }
     472
     473    // angular
     474    btVector3 angular_axis;
     475    btScalar angularJacDiagABInv;
     476    for (i=0;i<3;i++)
     477    {
     478        if (m_angularLimits[i].needApplyTorques())
     479        {
     480
     481                        // get axis
     482                        angular_axis = getAxis(i);
     483
     484                        angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
     485
     486                        m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB);
     487        }
     488    }
     489}
    647490
    648491void    btGeneric6DofConstraint::updateRHS(btScalar     timeStep)
    649492{
    650         (void)timeStep;
    651 
    652 }
    653 
    654 //-----------------------------------------------------------------------------
     493    (void)timeStep;
     494
     495}
    655496
    656497btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
    657498{
    658         return m_calculatedAxis[axis_index];
    659 }
    660 
    661 //-----------------------------------------------------------------------------
     499    return m_calculatedAxis[axis_index];
     500}
    662501
    663502btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
    664503{
    665         return m_calculatedAxisAngleDiff[axis_index];
    666 }
    667 
    668 //-----------------------------------------------------------------------------
     504    return m_calculatedAxisAngleDiff[axis_index];
     505}
    669506
    670507void btGeneric6DofConstraint::calcAnchorPos(void)
    … …  
    687524} // btGeneric6DofConstraint::calcAnchorPos()
    688525
    689 //-----------------------------------------------------------------------------
    690 
    691 void btGeneric6DofConstraint::calculateLinearInfo()
    692 {
    693         m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
    694         m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
    695         for(int i = 0; i < 3; i++)
    696         {
    697                 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
    698         }
    699 } // btGeneric6DofConstraint::calculateLinearInfo()
    700 
    701 //-----------------------------------------------------------------------------
    702 
    703 int btGeneric6DofConstraint::get_limit_motor_info2(
    704         btRotationalLimitMotor * limot,
    705         btRigidBody * body0, btRigidBody * body1,
    706         btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
    707 {
    708     int srow = row * info->rowskip;
    709     int powered = limot->m_enableMotor;
    710     int limit = limot->m_currentLimit;
    711     if (powered || limit)
    712     {   // if the joint is powered, or has joint limits, add in the extra row
    713         btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
    714         btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
    715         J1[srow+0] = ax1[0];
    716         J1[srow+1] = ax1[1];
    717         J1[srow+2] = ax1[2];
    718         if(rotational)
    719         {
    720             J2[srow+0] = -ax1[0];
    721             J2[srow+1] = -ax1[1];
    722             J2[srow+2] = -ax1[2];
    723         }
    724         if((!rotational) && limit)
    725         {
    726                         btVector3 ltd;  // Linear Torque Decoupling vector
    727                         btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
    728                         ltd = c.cross(ax1);
    729             info->m_J1angularAxis[srow+0] = ltd[0];
    730             info->m_J1angularAxis[srow+1] = ltd[1];
    731             info->m_J1angularAxis[srow+2] = ltd[2];
    732 
    733                         c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
    734                         ltd = -c.cross(ax1);
    735                         info->m_J2angularAxis[srow+0] = ltd[0];
    736             info->m_J2angularAxis[srow+1] = ltd[1];
    737             info->m_J2angularAxis[srow+2] = ltd[2];
    738         }
    739         // if we're limited low and high simultaneously, the joint motor is
    740         // ineffective
    741         if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
    742         info->m_constraintError[srow] = btScalar(0.f);
    743         if (powered)
    744         {
    745             info->cfm[srow] = 0.0f;
    746             if(!limit)
    747             {
    748                 info->m_constraintError[srow] += limot->m_targetVelocity;
    749                 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
    750                 info->m_upperLimit[srow] = limot->m_maxMotorForce;
    751             }
    752         }
    753         if(limit)
    754         {
    755             btScalar k = info->fps * limot->m_ERP;
    756                         if(!rotational)
    757                         {
    758                                 info->m_constraintError[srow] += k * limot->m_currentLimitError;
    759                         }
    760                         else
    761                         {
    762                                 info->m_constraintError[srow] += -k * limot->m_currentLimitError;
    763                         }
    764             info->cfm[srow] = 0.0f;
    765             if (limot->m_loLimit == limot->m_hiLimit)
    766             {   // limited low and high simultaneously
    767                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    768                 info->m_upperLimit[srow] = SIMD_INFINITY;
    769             }
    770             else
    771             {
    772                 if (limit == 1)
    773                 {
    774                     info->m_lowerLimit[srow] = 0;
    775                     info->m_upperLimit[srow] = SIMD_INFINITY;
    776                 }
    777                 else
    778                 {
    779                     info->m_lowerLimit[srow] = -SIMD_INFINITY;
    780                     info->m_upperLimit[srow] = 0;
    781                 }
    782                 // deal with bounce
    783                 if (limot->m_bounce > 0)
    784                 {
    785                     // calculate joint velocity
    786                     btScalar vel;
    787                     if (rotational)
    788                     {
    789                         vel = body0->getAngularVelocity().dot(ax1);
    790                         if (body1)
    791                             vel -= body1->getAngularVelocity().dot(ax1);
    792                     }
    793                     else
    794                     {
    795                         vel = body0->getLinearVelocity().dot(ax1);
    796                         if (body1)
    797                             vel -= body1->getLinearVelocity().dot(ax1);
    798                     }
    799                     // only apply bounce if the velocity is incoming, and if the
    800                     // resulting c[] exceeds what we already have.
    801                     if (limit == 1)
    802                     {
    803                         if (vel < 0)
    804                         {
    805                             btScalar newc = -limot->m_bounce* vel;
    806                             if (newc > info->m_constraintError[srow])
    807                                                                 info->m_constraintError[srow] = newc;
    808                         }
    809                     }
    810                     else
    811                     {
    812                         if (vel > 0)
    813                         {
    814                             btScalar newc = -limot->m_bounce * vel;
    815                             if (newc < info->m_constraintError[srow])
    816                                                                 info->m_constraintError[srow] = newc;
    817                         }
    818                     }
    819                 }
    820             }
    821         }
    822         return 1;
    823     }
    824     else return 0;
    825 }
    826 
    827 //-----------------------------------------------------------------------------
    828 //-----------------------------------------------------------------------------
    829 //-----------------------------------------------------------------------------

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

    r2907 r2908  
    2929
    3030class btRigidBody;
    31 
    32 
    3331
    3432
    … …  
    9593    bool isLimited()
    9694    {
    97         if(m_loLimit > m_hiLimit) return false;
     95        if(m_loLimit>=m_hiLimit) return false;
    9896        return true;
    9997    }
    … …  
    113111
    114112        //! apply the correction impulses for two bodies
    115     btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
     113    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
     114
    116115
    117116};
    … …  
    131130    btScalar    m_restitution;//! Bounce parameter for linear limit
    132131    //!@}
    133         bool            m_enableMotor[3];
    134     btVector3   m_targetVelocity;//!< target motor velocity
    135     btVector3   m_maxMotorForce;//!< max force on motor
    136     btVector3   m_currentLimitError;//!  How much is violated this limit
    137     int                 m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
    138132
    139133    btTranslationalLimitMotor()
    … …  
    146140        m_damping = btScalar(1.0f);
    147141        m_restitution = btScalar(0.5f);
    148                 for(int i=0; i < 3; i++)
    149                 {
    150                         m_enableMotor[i] = false;
    151                         m_targetVelocity[i] = btScalar(0.f);
    152                         m_maxMotorForce[i] = btScalar(0.f);
    153                 }
    154142    }
    155143
    … …  
    163151        m_damping = other.m_damping;
    164152        m_restitution = other.m_restitution;
    165                 for(int i=0; i < 3; i++)
    166                 {
    167                         m_enableMotor[i] = other.m_enableMotor[i];
    168                         m_targetVelocity[i] = other.m_targetVelocity[i];
    169                         m_maxMotorForce[i] = other.m_maxMotorForce[i];
    170                 }
    171153    }
    172154
    … …  
    182164       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
    183165    }
    184     inline bool needApplyForce(int limitIndex)
    185     {
    186         if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
    187         return true;
    188     }
    189         int testLimitValue(int limitIndex, btScalar test_value);
    190166
    191167
    … …  
    193169        btScalar timeStep,
    194170        btScalar jacDiagABInv,
    195         btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
    196         btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
     171        btRigidBody& body1,const btVector3 &pointInA,
     172        btRigidBody& body2,const btVector3 &pointInB,
    197173        int limit_index,
    198174        const btVector3 & axis_normal_on_a,
    … …  
    272248    btVector3 m_calculatedAxisAngleDiff;
    273249    btVector3 m_calculatedAxis[3];
    274     btVector3 m_calculatedLinearDiff;
    275250   
    276251        btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
    … …  
    288263
    289264
    290         int setAngularLimits(btConstraintInfo2 *info, int row_offset);
    291 
    292         int setLinearLimits(btConstraintInfo2 *info);
    293265
    294266    void buildLinearJacobian(
    … …  
    298270    void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
    299271
    300         // tests linear limits
    301         void calculateLinearInfo();
    302272
    303273        //! calcs the euler angles between the two bodies.
    … …  
    307277
    308278public:
    309 
    310         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
    311         bool            m_useSolveConstraintObsolete;
    312 
    313279    btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
    314280
    … …  
    365331    virtual void        buildJacobian();
    366332
    367         virtual void getInfo1 (btConstraintInfo1* info);
    368 
    369         virtual void getInfo2 (btConstraintInfo2* info);
    370 
    371     virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     333    virtual     void    solveConstraint(btScalar        timeStep);
    372334
    373335    void        updateRHS(btScalar      timeStep);
    … …  
    471433        virtual void calcAnchorPos(void); // overridable
    472434
    473         int get_limit_motor_info2(      btRotationalLimitMotor * limot,
    474                                                                 btRigidBody * body0, btRigidBody * body1,
    475                                                                 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
    476 
    477 
    478435};
    479436

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

    r2907 r2908  
    2020#include "LinearMath/btMinMax.h"
    2121#include <new>
    22 #include "btSolverBody.h"
    23 
    24 //-----------------------------------------------------------------------------
    25 
    26 #define HINGE_USE_OBSOLETE_SOLVER false
    27 
    28 //-----------------------------------------------------------------------------
    2922
    3023
    3124btHingeConstraint::btHingeConstraint()
    3225: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
    33 m_enableAngularMotor(false),
    34 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    35 m_useReferenceFrameA(false)
    36 {
    37         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
    38 }
    39 
    40 //-----------------------------------------------------------------------------
     26m_enableAngularMotor(false)
     27{
     28}
    4129
    4230btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
    43                                                                          btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
     31                                                                         btVector3& axisInA,btVector3& axisInB)
    4432                                                                         :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
    4533                                                                         m_angularOnly(false),
    46                                                                          m_enableAngularMotor(false),
    47                                                                          m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    48                                                                          m_useReferenceFrameA(useReferenceFrameA)
     34                                                                         m_enableAngularMotor(false)
    4935{
    5036        m_rbAFrame.getOrigin() = pivotInA;
    … …  
    7561       
    7662        m_rbBFrame.getOrigin() = pivotInB;
    77         m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
    78                                                                         rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
    79                                                                         rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
     63        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
     64                                                                        rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
     65                                                                        rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
    8066       
    8167        //start with free
    … …  
    8672        m_limitSoftness = 0.9f;
    8773        m_solveLimit = false;
    88         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
    89 }
    90 
    91 //-----------------------------------------------------------------------------
    92 
    93 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
    94 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
    95 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    96 m_useReferenceFrameA(useReferenceFrameA)
     74
     75}
     76
     77
     78
     79btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
     80:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
    9781{
    9882
    … …  
    10791                                                                        rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
    10892
    109         btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
     93        btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
    11094
    11195        btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
    … …  
    126110        m_limitSoftness = 0.9f;
    127111        m_solveLimit = false;
    128         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
    129 }
    130 
    131 //-----------------------------------------------------------------------------
     112}
    132113
    133114btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
    134                                                                      const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
     115                                                                     const btTransform& rbAFrame, const btTransform& rbBFrame)
    135116:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
    136117m_angularOnly(false),
    137 m_enableAngularMotor(false),
    138 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    139 m_useReferenceFrameA(useReferenceFrameA)
    140 {
     118m_enableAngularMotor(false)
     119{
     120        // flip axis
     121        m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
     122        m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
     123        m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
     124
    141125        //start with free
    142126        m_lowerLimit = btScalar(1e30);
    … …  
    146130        m_limitSoftness = 0.9f;
    147131        m_solveLimit = false;
    148         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
    149132}                       
    150133
    151 //-----------------------------------------------------------------------------
    152 
    153 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
     134
     135
     136btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
    154137:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
    155138m_angularOnly(false),
    156 m_enableAngularMotor(false),
    157 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
    158 m_useReferenceFrameA(useReferenceFrameA)
     139m_enableAngularMotor(false)
    159140{
    160141        ///not providing rigidbody B means implicitly using worldspace for body B
     142
     143        // flip axis
     144        m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
     145        m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
     146        m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
    161147
    162148        m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
    … …  
    169155        m_limitSoftness = 0.9f;
    170156        m_solveLimit = false;
    171         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
    172 }
    173 
    174 //-----------------------------------------------------------------------------
     157}
    175158
    176159void    btHingeConstraint::buildJacobian()
    177160{
    178         if (m_useSolveConstraintObsolete)
     161        m_appliedImpulse = btScalar(0.);
     162
     163        if (!m_angularOnly)
    179164        {
    180                 m_appliedImpulse = btScalar(0.);
    181 
    182                 if (!m_angularOnly)
    183                 {
    184                         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
    185                         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
    186                         btVector3 relPos = pivotBInW - pivotAInW;
    187 
    188                         btVector3 normal[3];
    189                         if (relPos.length2() > SIMD_EPSILON)
    190                         {
    191                                 normal[0] = relPos.normalized();
    192                         }
    193                         else
    194                         {
    195                                 normal[0].setValue(btScalar(1.0),0,0);
    196                         }
    197 
    198                         btPlaneSpace1(normal[0], normal[1], normal[2]);
    199 
    200                         for (int i=0;i<3;i++)
    201                         {
    202                                 new (&m_jac[i]) btJacobianEntry(
     165                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
     166                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
     167                btVector3 relPos = pivotBInW - pivotAInW;
     168
     169                btVector3 normal[3];
     170                if (relPos.length2() > SIMD_EPSILON)
     171                {
     172                        normal[0] = relPos.normalized();
     173                }
     174                else
     175                {
     176                        normal[0].setValue(btScalar(1.0),0,0);
     177                }
     178
     179                btPlaneSpace1(normal[0], normal[1], normal[2]);
     180
     181                for (int i=0;i<3;i++)
     182                {
     183                        new (&m_jac[i]) btJacobianEntry(
    203184                                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    204185                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    210191                                m_rbB.getInvInertiaDiagLocal(),
    211192                                m_rbB.getInvMass());
     193                }
     194        }
     195
     196        //calculate two perpendicular jointAxis, orthogonal to hingeAxis
     197        //these two jointAxis require equal angular velocities for both bodies
     198
     199        //this is unused for now, it's a todo
     200        btVector3 jointAxis0local;
     201        btVector3 jointAxis1local;
     202       
     203        btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
     204
     205        getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
     206        btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
     207        btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
     208        btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
     209               
     210        new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
     211                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
     212                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
     213                m_rbA.getInvInertiaDiagLocal(),
     214                m_rbB.getInvInertiaDiagLocal());
     215
     216        new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
     217                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
     218                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
     219                m_rbA.getInvInertiaDiagLocal(),
     220                m_rbB.getInvInertiaDiagLocal());
     221
     222        new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
     223                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
     224                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
     225                m_rbA.getInvInertiaDiagLocal(),
     226                m_rbB.getInvInertiaDiagLocal());
     227
     228
     229        // Compute limit information
     230        btScalar hingeAngle = getHingeAngle(); 
     231
     232        //set bias, sign, clear accumulator
     233        m_correction = btScalar(0.);
     234        m_limitSign = btScalar(0.);
     235        m_solveLimit = false;
     236        m_accLimitImpulse = btScalar(0.);
     237
     238//      if (m_lowerLimit < m_upperLimit)
     239        if (m_lowerLimit <= m_upperLimit)
     240        {
     241//              if (hingeAngle <= m_lowerLimit*m_limitSoftness)
     242                if (hingeAngle <= m_lowerLimit)
     243                {
     244                        m_correction = (m_lowerLimit - hingeAngle);
     245                        m_limitSign = 1.0f;
     246                        m_solveLimit = true;
     247                }
     248//              else if (hingeAngle >= m_upperLimit*m_limitSoftness)
     249                else if (hingeAngle >= m_upperLimit)
     250                {
     251                        m_correction = m_upperLimit - hingeAngle;
     252                        m_limitSign = -1.0f;
     253                        m_solveLimit = true;
     254                }
     255        }
     256
     257        //Compute K = J*W*J' for hinge axis
     258        btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
     259        m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
     260                                     getRigidBodyB().computeAngularImpulseDenominator(axisA));
     261
     262}
     263
     264void    btHingeConstraint::solveConstraint(btScalar     timeStep)
     265{
     266
     267        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
     268        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
     269
     270        btScalar tau = btScalar(0.3);
     271
     272        //linear part
     273        if (!m_angularOnly)
     274        {
     275                btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
     276                btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
     277
     278                btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
     279                btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
     280                btVector3 vel = vel1 - vel2;
     281
     282                for (int i=0;i<3;i++)
     283                {               
     284                        const btVector3& normal = m_jac[i].m_linearJointAxis;
     285                        btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
     286
     287                        btScalar rel_vel;
     288                        rel_vel = normal.dot(vel);
     289                        //positional error (zeroth order error)
     290                        btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
     291                        btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
     292                        m_appliedImpulse += impulse;
     293                        btVector3 impulse_vector = normal * impulse;
     294                        m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
     295                        m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
     296                }
     297        }
     298
     299       
     300        {
     301                ///solve angular part
     302
     303                // get axes in world space
     304                btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
     305                btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
     306
     307                const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
     308                const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
     309
     310                btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
     311                btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
     312
     313                btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
     314                btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
     315                btVector3 velrelOrthog = angAorthog-angBorthog;
     316                {
     317                        //solve orthogonal angular velocity correction
     318                        btScalar relaxation = btScalar(1.);
     319                        btScalar len = velrelOrthog.length();
     320                        if (len > btScalar(0.00001))
     321                        {
     322                                btVector3 normal = velrelOrthog.normalized();
     323                                btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
     324                                        getRigidBodyB().computeAngularImpulseDenominator(normal);
     325                                // scale for mass and relaxation
     326                                velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
    212327                        }
    213                 }
    214 
    215                 //calculate two perpendicular jointAxis, orthogonal to hingeAxis
    216                 //these two jointAxis require equal angular velocities for both bodies
    217 
    218                 //this is unused for now, it's a todo
    219                 btVector3 jointAxis0local;
    220                 btVector3 jointAxis1local;
    221                
    222                 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
    223 
    224                 getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
    225                 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
    226                 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
    227                 btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
     328
     329                        //solve angular positional correction
     330                        btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
     331                        btScalar len2 = angularError.length();
     332                        if (len2>btScalar(0.00001))
     333                        {
     334                                btVector3 normal2 = angularError.normalized();
     335                                btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
     336                                                getRigidBodyB().computeAngularImpulseDenominator(normal2);
     337                                angularError *= (btScalar(1.)/denom2) * relaxation;
     338                        }
     339
     340                        m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
     341                        m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
     342
     343                        // solve limit
     344                        if (m_solveLimit)
     345                        {
     346                                btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
     347
     348                                btScalar impulseMag = amplitude * m_kHinge;
     349
     350                                // Clamp the accumulated impulse
     351                                btScalar temp = m_accLimitImpulse;
     352                                m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
     353                                impulseMag = m_accLimitImpulse - temp;
     354
     355
     356                                btVector3 impulse = axisA * impulseMag * m_limitSign;
     357                                m_rbA.applyTorqueImpulse(impulse);
     358                                m_rbB.applyTorqueImpulse(-impulse);
     359                        }
     360                }
     361
     362                //apply motor
     363                if (m_enableAngularMotor)
     364                {
     365                        //todo: add limits too
     366                        btVector3 angularLimit(0,0,0);
     367
     368                        btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
     369                        btScalar projRelVel = velrel.dot(axisA);
     370
     371                        btScalar desiredMotorVel = m_motorTargetVelocity;
     372                        btScalar motor_relvel = desiredMotorVel - projRelVel;
     373
     374                        btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
     375                        //todo: should clip against accumulated impulse
     376                        btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
     377                        clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
     378                        btVector3 motorImp = clippedMotorImpulse * axisA;
     379
     380                        m_rbA.applyTorqueImpulse(motorImp+angularLimit);
     381                        m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
    228382                       
    229                 new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
    230                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    231                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    232                         m_rbA.getInvInertiaDiagLocal(),
    233                         m_rbB.getInvInertiaDiagLocal());
    234 
    235                 new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
    236                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    237                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    238                         m_rbA.getInvInertiaDiagLocal(),
    239                         m_rbB.getInvInertiaDiagLocal());
    240 
    241                 new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
    242                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    243                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    244                         m_rbA.getInvInertiaDiagLocal(),
    245                         m_rbB.getInvInertiaDiagLocal());
    246 
    247                         // clear accumulator
    248                         m_accLimitImpulse = btScalar(0.);
    249 
    250                         // test angular limit
    251                         testLimit();
    252 
    253                 //Compute K = J*W*J' for hinge axis
    254                 btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
    255                 m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
    256                                                          getRigidBodyB().computeAngularImpulseDenominator(axisA));
    257 
     383                }
    258384        }
    259 }
    260 
    261 //-----------------------------------------------------------------------------
    262 
    263 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
    264 {
    265         if (m_useSolveConstraintObsolete)
    266         {
    267                 info->m_numConstraintRows = 0;
    268                 info->nub = 0;
    269         }
    270         else
    271         {
    272                 info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
    273                 info->nub = 1;
    274                 //prepare constraint
    275                 testLimit();
    276                 if(getSolveLimit() || getEnableAngularMotor())
    277                 {
    278                         info->m_numConstraintRows++; // limit 3rd anguar as well
    279                         info->nub--;
    280                 }
    281         }
    282 } // btHingeConstraint::getInfo1 ()
    283 
    284 //-----------------------------------------------------------------------------
    285 
    286 void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
    287 {
    288         btAssert(!m_useSolveConstraintObsolete);
    289         int i, s = info->rowskip;
    290         // transforms in world space
    291         btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
    292         btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
    293         // pivot point
    294         btVector3 pivotAInW = trA.getOrigin();
    295         btVector3 pivotBInW = trB.getOrigin();
    296         // linear (all fixed)
    297     info->m_J1linearAxis[0] = 1;
    298     info->m_J1linearAxis[s + 1] = 1;
    299     info->m_J1linearAxis[2 * s + 2] = 1;
    300         btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
    301         {
    302                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
    303                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
    304                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
    305                 btVector3 a1neg = -a1;
    306                 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
    307         }
    308         btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
    309         {
    310                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
    311                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
    312                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
    313                 a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
    314         }
    315         // linear RHS
    316     btScalar k = info->fps * info->erp;
    317         for(i = 0; i < 3; i++)
    318     {
    319         info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
    320     }
    321         // make rotations around X and Y equal
    322         // the hinge axis should be the only unconstrained
    323         // rotational axis, the angular velocity of the two bodies perpendicular to
    324         // the hinge axis should be equal. thus the constraint equations are
    325         //    p*w1 - p*w2 = 0
    326         //    q*w1 - q*w2 = 0
    327         // where p and q are unit vectors normal to the hinge axis, and w1 and w2
    328         // are the angular velocity vectors of the two bodies.
    329         // get hinge axis (Z)
    330         btVector3 ax1 = trA.getBasis().getColumn(2);
    331         // get 2 orthos to hinge axis (X, Y)
    332         btVector3 p = trA.getBasis().getColumn(0);
    333         btVector3 q = trA.getBasis().getColumn(1);
    334         // set the two hinge angular rows
    335     int s3 = 3 * info->rowskip;
    336     int s4 = 4 * info->rowskip;
    337 
    338         info->m_J1angularAxis[s3 + 0] = p[0];
    339         info->m_J1angularAxis[s3 + 1] = p[1];
    340         info->m_J1angularAxis[s3 + 2] = p[2];
    341         info->m_J1angularAxis[s4 + 0] = q[0];
    342         info->m_J1angularAxis[s4 + 1] = q[1];
    343         info->m_J1angularAxis[s4 + 2] = q[2];
    344 
    345         info->m_J2angularAxis[s3 + 0] = -p[0];
    346         info->m_J2angularAxis[s3 + 1] = -p[1];
    347         info->m_J2angularAxis[s3 + 2] = -p[2];
    348         info->m_J2angularAxis[s4 + 0] = -q[0];
    349         info->m_J2angularAxis[s4 + 1] = -q[1];
    350         info->m_J2angularAxis[s4 + 2] = -q[2];
    351     // compute the right hand side of the constraint equation. set relative
    352     // body velocities along p and q to bring the hinge back into alignment.
    353     // if ax1,ax2 are the unit length hinge axes as computed from body1 and
    354     // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
    355     // if `theta' is the angle between ax1 and ax2, we need an angular velocity
    356     // along u to cover angle erp*theta in one step :
    357     //   |angular_velocity| = angle/time = erp*theta / stepsize
    358     //                      = (erp*fps) * theta
    359     //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
    360     //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
    361     // ...as ax1 and ax2 are unit length. if theta is smallish,
    362     // theta ~= sin(theta), so
    363     //    angular_velocity  = (erp*fps) * (ax1 x ax2)
    364     // ax1 x ax2 is in the plane space of ax1, so we project the angular
    365     // velocity to p and q to find the right hand side.
    366     btVector3 ax2 = trB.getBasis().getColumn(2);
    367         btVector3 u = ax1.cross(ax2);
    368         info->m_constraintError[s3] = k * u.dot(p);
    369         info->m_constraintError[s4] = k * u.dot(q);
    370         // check angular limits
    371         int nrow = 4; // last filled row
    372         int srow;
    373         btScalar limit_err = btScalar(0.0);
    374         int limit = 0;
    375         if(getSolveLimit())
    376         {
    377                 limit_err = m_correction * m_referenceSign;
    378                 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
    379         }
    380         // if the hinge has joint limits or motor, add in the extra row
    381         int powered = 0;
    382         if(getEnableAngularMotor())
    383         {
    384                 powered = 1;
    385         }
    386         if(limit || powered)
    387         {
    388                 nrow++;
    389                 srow = nrow * info->rowskip;
    390                 info->m_J1angularAxis[srow+0] = ax1[0];
    391                 info->m_J1angularAxis[srow+1] = ax1[1];
    392                 info->m_J1angularAxis[srow+2] = ax1[2];
    393 
    394                 info->m_J2angularAxis[srow+0] = -ax1[0];
    395                 info->m_J2angularAxis[srow+1] = -ax1[1];
    396                 info->m_J2angularAxis[srow+2] = -ax1[2];
    397 
    398                 btScalar lostop = getLowerLimit();
    399                 btScalar histop = getUpperLimit();
    400                 if(limit && (lostop == histop))
    401                 {  // the joint motor is ineffective
    402                         powered = 0;
    403                 }
    404                 info->m_constraintError[srow] = btScalar(0.0f);
    405                 if(powered)
    406                 {
    407             info->cfm[srow] = btScalar(0.0);
    408                         btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
    409                         info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
    410                         info->m_lowerLimit[srow] = - m_maxMotorImpulse;
    411                         info->m_upperLimit[srow] =   m_maxMotorImpulse;
    412                 }
    413                 if(limit)
    414                 {
    415                         k = info->fps * info->erp;
    416                         info->m_constraintError[srow] += k * limit_err;
    417                         info->cfm[srow] = btScalar(0.0);
    418                         if(lostop == histop)
    419                         {
    420                                 // limited low and high simultaneously
    421                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    422                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    423                         }
    424                         else if(limit == 1)
    425                         { // low limit
    426                                 info->m_lowerLimit[srow] = 0;
    427                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    428                         }
    429                         else
    430                         { // high limit
    431                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    432                                 info->m_upperLimit[srow] = 0;
    433                         }
    434                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
    435                         btScalar bounce = m_relaxationFactor;
    436                         if(bounce > btScalar(0.0))
    437                         {
    438                                 btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
    439                                 vel -= m_rbB.getAngularVelocity().dot(ax1);
    440                                 // only apply bounce if the velocity is incoming, and if the
    441                                 // resulting c[] exceeds what we already have.
    442                                 if(limit == 1)
    443                                 {       // low limit
    444                                         if(vel < 0)
    445                                         {
    446                                                 btScalar newc = -bounce * vel;
    447                                                 if(newc > info->m_constraintError[srow])
    448                                                 {
    449                                                         info->m_constraintError[srow] = newc;
    450                                                 }
    451                                         }
    452                                 }
    453                                 else
    454                                 {       // high limit - all those computations are reversed
    455                                         if(vel > 0)
    456                                         {
    457                                                 btScalar newc = -bounce * vel;
    458                                                 if(newc < info->m_constraintError[srow])
    459                                                 {
    460                                                         info->m_constraintError[srow] = newc;
    461                                                 }
    462                                         }
    463                                 }
    464                         }
    465                         info->m_constraintError[srow] *= m_biasFactor;
    466                 } // if(limit)
    467         } // if angular limit or powered
    468 }
    469 
    470 //-----------------------------------------------------------------------------
    471 
    472 void    btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar     timeStep)
    473 {
    474 
    475         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
    476         if (m_useSolveConstraintObsolete)
    477         {
    478 
    479                 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
    480                 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
    481 
    482                 btScalar tau = btScalar(0.3);
    483 
    484                 //linear part
    485                 if (!m_angularOnly)
    486                 {
    487                         btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
    488                         btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    489 
    490                         btVector3 vel1,vel2;
    491                         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
    492                         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
    493                         btVector3 vel = vel1 - vel2;
    494 
    495                         for (int i=0;i<3;i++)
    496                         {               
    497                                 const btVector3& normal = m_jac[i].m_linearJointAxis;
    498                                 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
    499 
    500                                 btScalar rel_vel;
    501                                 rel_vel = normal.dot(vel);
    502                                 //positional error (zeroth order error)
    503                                 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
    504                                 btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
    505                                 m_appliedImpulse += impulse;
    506                                 btVector3 impulse_vector = normal * impulse;
    507                                 btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
    508                                 btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
    509                                 bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
    510                                 bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
    511                         }
    512                 }
    513 
    514                
    515                 {
    516                         ///solve angular part
    517 
    518                         // get axes in world space
    519                         btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
    520                         btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
    521 
    522                         btVector3 angVelA;
    523                         bodyA.getAngularVelocity(angVelA);
    524                         btVector3 angVelB;
    525                         bodyB.getAngularVelocity(angVelB);
    526 
    527                         btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
    528                         btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
    529 
    530                         btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
    531                         btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
    532                         btVector3 velrelOrthog = angAorthog-angBorthog;
    533                         {
    534                                
    535 
    536                                 //solve orthogonal angular velocity correction
    537                                 btScalar relaxation = btScalar(1.);
    538                                 btScalar len = velrelOrthog.length();
    539                                 if (len > btScalar(0.00001))
    540                                 {
    541                                         btVector3 normal = velrelOrthog.normalized();
    542                                         btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
    543                                                 getRigidBodyB().computeAngularImpulseDenominator(normal);
    544                                         // scale for mass and relaxation
    545                                         //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
    546 
    547                                         bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
    548                                         bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
    549 
    550                                 }
    551 
    552                                 //solve angular positional correction
    553                                 btVector3 angularError =  axisA.cross(axisB) *(btScalar(1.)/timeStep);
    554                                 btScalar len2 = angularError.length();
    555                                 if (len2>btScalar(0.00001))
    556                                 {
    557                                         btVector3 normal2 = angularError.normalized();
    558                                         btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
    559                                                         getRigidBodyB().computeAngularImpulseDenominator(normal2);
    560                                         //angularError *= (btScalar(1.)/denom2) * relaxation;
    561                                        
    562                                         bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
    563                                         bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
    564 
    565                                 }
    566                                
    567                                
    568 
    569 
    570 
    571                                 // solve limit
    572                                 if (m_solveLimit)
    573                                 {
    574                                         btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
    575 
    576                                         btScalar impulseMag = amplitude * m_kHinge;
    577 
    578                                         // Clamp the accumulated impulse
    579                                         btScalar temp = m_accLimitImpulse;
    580                                         m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
    581                                         impulseMag = m_accLimitImpulse - temp;
    582 
    583 
    584                                        
    585                                         bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
    586                                         bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
    587 
    588                                 }
    589                         }
    590 
    591                         //apply motor
    592                         if (m_enableAngularMotor)
    593                         {
    594                                 //todo: add limits too
    595                                 btVector3 angularLimit(0,0,0);
    596 
    597                                 btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
    598                                 btScalar projRelVel = velrel.dot(axisA);
    599 
    600                                 btScalar desiredMotorVel = m_motorTargetVelocity;
    601                                 btScalar motor_relvel = desiredMotorVel - projRelVel;
    602 
    603                                 btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
    604                                 //todo: should clip against accumulated impulse
    605                                 btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
    606                                 clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
    607                                 btVector3 motorImp = clippedMotorImpulse * axisA;
    608                        
    609                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
    610                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
    611                                
    612                         }
    613                 }
    614         }
    615 
    616 }
    617 
    618 //-----------------------------------------------------------------------------
     385
     386}
    619387
    620388void    btHingeConstraint::updateRHS(btScalar   timeStep)
    … …  
    623391
    624392}
    625 
    626 //-----------------------------------------------------------------------------
    627393
    628394btScalar btHingeConstraint::getHingeAngle()
    … …  
    631397        const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
    632398        const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
    633         btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
    634         return m_referenceSign * angle;
    635 }
    636 
    637 //-----------------------------------------------------------------------------
    638 
    639 void btHingeConstraint::testLimit()
    640 {
    641         // Compute limit information
    642         m_hingeAngle = getHingeAngle(); 
    643         m_correction = btScalar(0.);
    644         m_limitSign = btScalar(0.);
    645         m_solveLimit = false;
    646         if (m_lowerLimit <= m_upperLimit)
    647         {
    648                 if (m_hingeAngle <= m_lowerLimit)
    649                 {
    650                         m_correction = (m_lowerLimit - m_hingeAngle);
    651                         m_limitSign = 1.0f;
    652                         m_solveLimit = true;
    653                 }
    654                 else if (m_hingeAngle >= m_upperLimit)
    655                 {
    656                         m_correction = m_upperLimit - m_hingeAngle;
    657                         m_limitSign = -1.0f;
    658                         m_solveLimit = true;
    659                 }
    660         }
    661         return;
    662 } // btHingeConstraint::testLimit()
    663 
    664 //-----------------------------------------------------------------------------
    665 //-----------------------------------------------------------------------------
    666 //-----------------------------------------------------------------------------
     399
     400        return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
     401}
     402

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h

    r2907 r2908  
    5454
    5555        btScalar        m_accLimitImpulse;
    56         btScalar        m_hingeAngle;
    57         btScalar    m_referenceSign;
    5856
    5957        bool            m_angularOnly;
    6058        bool            m_enableAngularMotor;
    6159        bool            m_solveLimit;
    62         bool            m_useSolveConstraintObsolete;
    63         bool            m_useReferenceFrameA;
    6460
    6561       
    6662public:
    6763
    68         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false);
     64        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
    6965
    70         btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
     66        btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
    7167       
    72         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
     68        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
    7369
    74         btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
     70        btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
    7571
    7672        btHingeConstraint();
    … …  
    7874        virtual void    buildJacobian();
    7975
    80         virtual void getInfo1 (btConstraintInfo1* info);
    81 
    82         virtual void getInfo2 (btConstraintInfo2* info);
    83        
    84         virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     76        virtual void    solveConstraint(btScalar        timeStep);
    8577
    8678        void    updateRHS(btScalar      timeStep);
    … …  
    9587        }
    9688
    97         btRigidBody& getRigidBodyA()   
    98         {               
    99                 return m_rbA;   
    100         }       
    101 
    102         btRigidBody& getRigidBodyB()   
    103         {               
    104                 return m_rbB;   
    105         }       
    106        
    10789        void    setAngularOnly(bool angularOnly)
    10890        {
    … …  
    141123        btScalar getHingeAngle();
    142124
    143         void testLimit();
    144 
    145125
    146126        const btTransform& getAFrame() { return m_rbAFrame; }; 

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

    r2907 r2908  
    2222
    2323btPoint2PointConstraint::btPoint2PointConstraint()
    24 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
    25 m_useSolveConstraintObsolete(false)
     24:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
    2625{
    2726}
    2827
    2928btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
    30 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
    31 m_useSolveConstraintObsolete(false)
     29:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
    3230{
    3331
    … …  
    3634
    3735btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
    38 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
    39 m_useSolveConstraintObsolete(false)
     36:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
    4037{
    4138       
    … …  
    4441void    btPoint2PointConstraint::buildJacobian()
    4542{
    46         ///we need it for both methods
     43        m_appliedImpulse = btScalar(0.);
     44
     45        btVector3       normal(0,0,0);
     46
     47        for (int i=0;i<3;i++)
    4748        {
    48                 m_appliedImpulse = btScalar(0.);
    49 
    50                 btVector3       normal(0,0,0);
    51 
    52                 for (int i=0;i<3;i++)
    53                 {
    54                         normal[i] = 1;
    55                         new (&m_jac[i]) btJacobianEntry(
     49                normal[i] = 1;
     50                new (&m_jac[i]) btJacobianEntry(
    5651                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    5752                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    6459                        m_rbB.getInvMass());
    6560                normal[i] = 0;
    66                 }
    6761        }
    6862
    6963}
    7064
     65void    btPoint2PointConstraint::solveConstraint(btScalar       timeStep)
     66{
     67        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
     68        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
    7169
    72 void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
    73 {
    74         if (m_useSolveConstraintObsolete)
    75         {
    76                 info->m_numConstraintRows = 0;
    77                 info->nub = 0;
    78         } else
    79         {
    80                 info->m_numConstraintRows = 3;
    81                 info->nub = 3;
    82         }
    83 }
    8470
    85 void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
    86 {
    87         btAssert(!m_useSolveConstraintObsolete);
     71        btVector3 normal(0,0,0);
     72       
    8873
    89          //retrieve matrices
    90         btTransform body0_trans;
    91         body0_trans = m_rbA.getCenterOfMassTransform();
    92     btTransform body1_trans;
    93         body1_trans = m_rbB.getCenterOfMassTransform();
     74//      btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
     75//      btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
    9476
    95         // anchor points in global coordinates with respect to body PORs.
    96    
    97     // set jacobian
    98     info->m_J1linearAxis[0] = 1;
    99     info->m_J1linearAxis[info->rowskip+1] = 1;
    100     info->m_J1linearAxis[2*info->rowskip+2] = 1;
     77        for (int i=0;i<3;i++)
     78        {               
     79                normal[i] = 1;
     80                btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
    10181
    102         btVector3 a1 = body0_trans.getBasis()*getPivotInA();
    103         {
    104                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
    105                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
    106                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
    107                 btVector3 a1neg = -a1;
    108                 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
    109         }
    110    
    111         /*info->m_J2linearAxis[0] = -1;
    112     info->m_J2linearAxis[s+1] = -1;
    113     info->m_J2linearAxis[2*s+2] = -1;
     82                btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
     83                btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
     84                //this jacobian entry could be re-used for all iterations
     85               
     86                btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
     87                btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
     88                btVector3 vel = vel1 - vel2;
     89               
     90                btScalar rel_vel;
     91                rel_vel = normal.dot(vel);
     92
     93        /*
     94                //velocity error (first order error)
     95                btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
     96                                                                                                                m_rbB.getLinearVelocity(),angvelB);
    11497        */
    11598       
    116         btVector3 a2 = body1_trans.getBasis()*getPivotInB();
    117    
    118         {
    119                 btVector3 a2n = -a2;
    120                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
    121                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
    122                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
    123                 a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
    124         }
    125    
     99                //positional error (zeroth order error)
     100                btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
     101               
     102                btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
    126103
     104                btScalar impulseClamp = m_setting.m_impulseClamp;
     105                if (impulseClamp > 0)
     106                {
     107                        if (impulse < -impulseClamp)
     108                                impulse = -impulseClamp;
     109                        if (impulse > impulseClamp)
     110                                impulse = impulseClamp;
     111                }
    127112
    128     // set right hand side
    129     btScalar k = info->fps * info->erp;
    130     int j;
    131 
    132         for (j=0; j<3; j++)
    133     {
    134         info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] -                     a1[j] - body0_trans.getOrigin()[j]);
    135                 //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
    136     }
    137 
    138         btScalar impulseClamp = m_setting.m_impulseClamp;//
    139         for (j=0; j<3; j++)
    140     {
    141                 if (m_setting.m_impulseClamp > 0)
    142                 {
    143                         info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
    144                         info->m_upperLimit[j*info->rowskip] = impulseClamp;
    145                 }
    146         }
    147        
    148 }
    149 
    150 
    151 void    btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar       timeStep)
    152 {
    153         if (m_useSolveConstraintObsolete)
    154         {
    155                 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
    156                 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
    157 
    158 
    159                 btVector3 normal(0,0,0);
     113                m_appliedImpulse+=impulse;
     114                btVector3 impulse_vector = normal * impulse;
     115                m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
     116                m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
    160117               
    161 
    162         //      btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
    163         //      btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
    164 
    165                 for (int i=0;i<3;i++)
    166                 {               
    167                         normal[i] = 1;
    168                         btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
    169 
    170                         btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
    171                         btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    172                         //this jacobian entry could be re-used for all iterations
    173                        
    174                         btVector3 vel1,vel2;
    175                         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
    176                         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
    177                         btVector3 vel = vel1 - vel2;
    178                        
    179                         btScalar rel_vel;
    180                         rel_vel = normal.dot(vel);
    181 
    182                 /*
    183                         //velocity error (first order error)
    184                         btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
    185                                                                                                                         m_rbB.getLinearVelocity(),angvelB);
    186                 */
    187                
    188                         //positional error (zeroth order error)
    189                         btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
    190                        
    191                         btScalar deltaImpulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
    192 
    193                         btScalar impulseClamp = m_setting.m_impulseClamp;
    194                        
    195                         const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
    196                         if (sum < -impulseClamp)
    197                         {
    198                                 deltaImpulse = -impulseClamp-m_appliedImpulse;
    199                                 m_appliedImpulse = -impulseClamp;
    200                         }
    201                         else if (sum > impulseClamp)
    202                         {
    203                                 deltaImpulse = impulseClamp-m_appliedImpulse;
    204                                 m_appliedImpulse = impulseClamp;
    205                         }
    206                         else
    207                         {
    208                                 m_appliedImpulse = sum;
    209                         }
    210 
    211                        
    212                         btVector3 impulse_vector = normal * deltaImpulse;
    213                        
    214                         btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
    215                         btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
    216                         bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
    217                         bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);
    218 
    219 
    220                         normal[i] = 0;
    221                 }
     118                normal[i] = 0;
    222119        }
    223120}

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h

    r2907 r2908  
    5151public:
    5252
    53         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
    54         bool            m_useSolveConstraintObsolete;
    55 
    5653        btConstraintSetting     m_setting;
    5754
    … …  
    6461        virtual void    buildJacobian();
    6562
    66         virtual void getInfo1 (btConstraintInfo1* info);
    6763
    68         virtual void getInfo2 (btConstraintInfo2* info);
    69 
    70 
    71         virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
     64        virtual void    solveConstraint(btScalar        timeStep);
    7265
    7366        void    updateRHS(btScalar      timeStep);

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

    r2907 r2908  
    1616//#define COMPUTE_IMPULSE_DENOM 1
    1717//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
     18//#define FORCE_REFESH_CONTACT_MANIFOLDS 1
    1819
    1920#include "btSequentialImpulseConstraintSolver.h"
    … …  
    3233#include "btSolverBody.h"
    3334#include "btSolverConstraint.h"
     35
     36
    3437#include "LinearMath/btAlignedObjectArray.h"
    35 #include <string.h> //for memset
     38
     39
     40int totalCpd = 0;
     41
     42int     gTotalContactPoints = 0;
     43
     44struct  btOrderIndex
     45{
     46        int     m_manifoldIndex;
     47        int     m_pointIndex;
     48};
     49
     50
     51
     52#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
     53static btOrderIndex     gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
     54
     55
     56unsigned long btSequentialImpulseConstraintSolver::btRand2()
     57{
     58  m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
     59  return m_btSeed2;
     60}
     61
     62
     63
     64//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
     65int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
     66{
     67  // seems good; xor-fold and modulus
     68  const unsigned long un = static_cast<unsigned long>(n);
     69  unsigned long r = btRand2();
     70
     71  // note: probably more aggressive than it needs to be -- might be
     72  //       able to get away without one or two of the innermost branches.
     73  if (un <= 0x00010000UL) {
     74    r ^= (r >> 16);
     75    if (un <= 0x00000100UL) {
     76      r ^= (r >> 8);
     77      if (un <= 0x00000010UL) {
     78        r ^= (r >> 4);
     79        if (un <= 0x00000004UL) {
     80          r ^= (r >> 2);
     81          if (un <= 0x00000002UL) {
     82            r ^= (r >> 1);
     83          }
     84        }
     85     }
     86    }
     87   }
     88
     89  return (int) (r % un);
     90}
     91
     92
     93
     94
     95bool  MyContactDestroyedCallback(void* userPersistentData);
     96bool  MyContactDestroyedCallback(void* userPersistentData)
     97{
     98        assert (userPersistentData);
     99        btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
     100        btAlignedFree(cpd);
     101        totalCpd--;
     102        //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
     103        return true;
     104}
     105
     106
    36107
    37108btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
    38109:m_btSeed2(0)
    39110{
    40 
     111        gContactDestroyedCallback = &MyContactDestroyedCallback;
     112
     113        //initialize default friction/contact funcs
     114        int i,j;
     115        for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
     116                for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
     117                {
     118
     119                        m_contactDispatch[i][j] = resolveSingleCollision;
     120                        m_frictionDispatch[i][j] = resolveSingleFriction;
     121                }
    41122}
    42123
    43124btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
    44125{
    45 }
    46 
    47 #ifdef USE_SIMD
    48 #include <emmintrin.h>
    49 #define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
    50 static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
    51 {
    52         __m128 result = _mm_mul_ps( vec0, vec1);
    53         return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
    54 }
    55 #endif//USE_SIMD
    56 
    57 // Project Gauss Seidel or the equivalent Sequential Impulse
    58 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
    59 {
    60 #ifdef USE_SIMD
    61         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
    62         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
    63         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
    64         __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
    65         __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
    66         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
    67         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    68         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    69         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
    70         btSimdScalar resultLowerLess,resultUpperLess;
    71         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
    72         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
    73         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
    74         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
    75         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
    76         __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
    77         deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
    78         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
    79         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
    80         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
    81         __m128 impulseMagnitude = deltaImpulse;
    82         body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
    83         body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
    84         body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
    85         body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
    86 #else
    87         resolveSingleConstraintRowGeneric(body1,body2,c);
    88 #endif
    89 }
    90 
    91 // Project Gauss Seidel or the equivalent Sequential Impulse
    92  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
    93 {
    94         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
    95         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
    96         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
    97 
    98         const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
    99         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
    100         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
    101 
    102         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
    103         if (sum < c.m_lowerLimit)
    104         {
    105                 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
    106                 c.m_appliedImpulse = c.m_lowerLimit;
    107         }
    108         else if (sum > c.m_upperLimit)
    109         {
    110                 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
    111                 c.m_appliedImpulse = c.m_upperLimit;
    112         }
    113         else
    114         {
    115                 c.m_appliedImpulse = sum;
    116         }
    117         if (body1.m_invMass)
    118                 body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
    119         if (body2.m_invMass)
    120                 body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
    121 }
    122 
    123  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
    124 {
    125 #ifdef USE_SIMD
    126         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
    127         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
    128         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
    129         __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
    130         __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
    131         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
    132         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    133         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    134         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
    135         btSimdScalar resultLowerLess,resultUpperLess;
    136         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
    137         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
    138         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
    139         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
    140         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
    141         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
    142         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
    143         __m128 impulseMagnitude = deltaImpulse;
    144         body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
    145         body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
    146         body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
    147         body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
    148 #else
    149         resolveSingleConstraintRowLowerLimit(body1,body2,c);
    150 #endif
    151 }
    152 
    153 // Project Gauss Seidel or the equivalent Sequential Impulse
    154  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
    155 {
    156         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
    157         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
    158         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
    159 
    160         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
    161         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
    162         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
    163         if (sum < c.m_lowerLimit)
    164         {
    165                 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
    166                 c.m_appliedImpulse = c.m_lowerLimit;
    167         }
    168         else
    169         {
    170                 c.m_appliedImpulse = sum;
    171         }
    172         if (body1.m_invMass)
    173                 body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
    174         if (body2.m_invMass)
    175                 body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
    176 }
    177 
    178 
    179 
    180 unsigned long btSequentialImpulseConstraintSolver::btRand2()
    181 {
    182         m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
    183         return m_btSeed2;
    184 }
    185 
    186 
    187 
    188 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
    189 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
    190 {
    191         // seems good; xor-fold and modulus
    192         const unsigned long un = static_cast<unsigned long>(n);
    193         unsigned long r = btRand2();
    194 
    195         // note: probably more aggressive than it needs to be -- might be
    196         //       able to get away without one or two of the innermost branches.
    197         if (un <= 0x00010000UL) {
    198                 r ^= (r >> 16);
    199                 if (un <= 0x00000100UL) {
    200                         r ^= (r >> 8);
    201                         if (un <= 0x00000010UL) {
    202                                 r ^= (r >> 4);
    203                                 if (un <= 0x00000004UL) {
    204                                         r ^= (r >> 2);
    205                                         if (un <= 0x00000002UL) {
    206                                                 r ^= (r >> 1);
    207                                         }
    208                                 }
    209                         }
    210                 }
    211         }
    212 
    213         return (int) (r % un);
    214 }
    215 
    216 
    217 
    218 void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
    219 {
    220         btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
    221 
    222         solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
    223         solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
    224 
     126
     127}
     128
     129void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
     130void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
     131{
     132        btRigidBody* rb = btRigidBody::upcast(collisionObject);
    225133        if (rb)
    226134        {
     135                solverBody->m_angularVelocity = rb->getAngularVelocity() ;
     136                solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
     137                solverBody->m_friction = collisionObject->getFriction();
    227138                solverBody->m_invMass = rb->getInvMass();
     139                solverBody->m_linearVelocity = rb->getLinearVelocity();
    228140                solverBody->m_originalBody = rb;
    229141                solverBody->m_angularFactor = rb->getAngularFactor();
    230142        } else
    231143        {
     144                solverBody->m_angularVelocity.setValue(0,0,0);
     145                solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
     146                solverBody->m_friction = collisionObject->getFriction();
    232147                solverBody->m_invMass = 0.f;
     148                solverBody->m_linearVelocity.setValue(0,0,0);
    233149                solverBody->m_originalBody = 0;
    234150                solverBody->m_angularFactor = 1.f;
    235151        }
     152        solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
     153        solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
    236154}
    237155
    … …  
    239157int             gNumSplitImpulseRecoveries = 0;
    240158
    241 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
     159btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
     160btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
    242161{
    243162        btScalar rest = restitution * -rel_vel;
    … …  
    246165
    247166
    248 
    249 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
    250 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
    251 {
    252         if (colObj && colObj->hasAnisotropicFriction())
    253         {
    254                 // transform to local coordinates
    255                 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
    256                 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
    257                 //apply anisotropic friction
    258                 loc_lateral *= friction_scaling;
    259                 // ... and transform it back to global coordinates
    260                 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
    261         }
    262 }
     167void    resolveSplitPenetrationImpulseCacheFriendly(
     168        btSolverBody& body1,
     169        btSolverBody& body2,
     170        const btSolverConstraint& contactConstraint,
     171        const btContactSolverInfo& solverInfo);
     172
     173//SIMD_FORCE_INLINE
     174void    resolveSplitPenetrationImpulseCacheFriendly(
     175        btSolverBody& body1,
     176        btSolverBody& body2,
     177        const btSolverConstraint& contactConstraint,
     178        const btContactSolverInfo& solverInfo)
     179{
     180        (void)solverInfo;
     181
     182                if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
     183        {
     184
     185                                gNumSplitImpulseRecoveries++;
     186                btScalar normalImpulse;
     187
     188                //  Optimized version of projected relative velocity, use precomputed cross products with normal
     189                //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
     190                //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
     191                //      btVector3 vel = vel1 - vel2;
     192                //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
     193
     194                btScalar rel_vel;
     195                btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
     196                + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
     197                btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
     198                + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
     199
     200                rel_vel = vel1Dotn-vel2Dotn;
     201
     202
     203                                btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
     204                //      btScalar positionalError = contactConstraint.m_penetration;
     205
     206                btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
     207
     208                btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
     209                btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
     210                normalImpulse = penetrationImpulse+velocityImpulse;
     211
     212                // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
     213                btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
     214                btScalar sum = oldNormalImpulse + normalImpulse;
     215                contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
     216
     217                normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
     218
     219                                body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
     220               
     221                                body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
     222               
     223        }
     224
     225}
     226
     227
     228//velocity + friction
     229//response  between two dynamic objects with friction
     230
     231btScalar resolveSingleCollisionCombinedCacheFriendly(
     232        btSolverBody& body1,
     233        btSolverBody& body2,
     234        const btSolverConstraint& contactConstraint,
     235        const btContactSolverInfo& solverInfo);
     236
     237//SIMD_FORCE_INLINE
     238btScalar resolveSingleCollisionCombinedCacheFriendly(
     239        btSolverBody& body1,
     240        btSolverBody& body2,
     241        const btSolverConstraint& contactConstraint,
     242        const btContactSolverInfo& solverInfo)
     243{
     244        (void)solverInfo;
     245
     246        btScalar normalImpulse;
     247
     248        {
     249
     250               
     251                //  Optimized version of projected relative velocity, use precomputed cross products with normal
     252                //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
     253                //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
     254                //      btVector3 vel = vel1 - vel2;
     255                //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
     256
     257                btScalar rel_vel;
     258                btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
     259                                        + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
     260                btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
     261                                        + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
     262
     263                rel_vel = vel1Dotn-vel2Dotn;
     264
     265                btScalar positionalError = 0.f;
     266                if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
     267                {
     268                        positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
     269                }
     270
     271                btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
     272
     273                btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
     274                btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
     275                normalImpulse = penetrationImpulse+velocityImpulse;
     276               
     277               
     278                // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
     279                btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
     280                btScalar sum = oldNormalImpulse + normalImpulse;
     281                contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
     282
     283                normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
     284
     285                body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
     286                                contactConstraint.m_angularComponentA,normalImpulse);
     287               
     288                body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
     289                                contactConstraint.m_angularComponentB,-normalImpulse);
     290        }
     291
     292        return normalImpulse;
     293}
     294
     295
     296#ifndef NO_FRICTION_TANGENTIALS
     297
     298btScalar resolveSingleFrictionCacheFriendly(
     299        btSolverBody& body1,
     300        btSolverBody& body2,
     301        const btSolverConstraint& contactConstraint,
     302        const btContactSolverInfo& solverInfo,
     303        btScalar appliedNormalImpulse);
     304
     305//SIMD_FORCE_INLINE
     306btScalar resolveSingleFrictionCacheFriendly(
     307        btSolverBody& body1,
     308        btSolverBody& body2,
     309        const btSolverConstraint& contactConstraint,
     310        const btContactSolverInfo& solverInfo,
     311        btScalar appliedNormalImpulse)
     312{
     313        (void)solverInfo;
     314
     315       
     316        const btScalar combinedFriction = contactConstraint.m_friction;
     317       
     318        const btScalar limit = appliedNormalImpulse * combinedFriction;
     319       
     320        if (appliedNormalImpulse>btScalar(0.))
     321        //friction
     322        {
     323               
     324                btScalar j1;
     325                {
     326
     327                        btScalar rel_vel;
     328                        const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
     329                                                + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
     330                        const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
     331                                + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
     332                        rel_vel = vel1Dotn-vel2Dotn;
     333
     334                        // calculate j that moves us to zero relative velocity
     335                        j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
     336#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
     337#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
     338                        btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
     339                        contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
     340                       
     341                        if (limit < contactConstraint.m_appliedImpulse)
     342                        {
     343                                contactConstraint.m_appliedImpulse = limit;
     344                        } else
     345                        {
     346                                if (contactConstraint.m_appliedImpulse < -limit)
     347                                        contactConstraint.m_appliedImpulse = -limit;
     348                        }
     349                        j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
     350#else
     351                        if (limit < j1)
     352                        {
     353                                j1 = limit;
     354                        } else
     355                        {
     356                                if (j1 < -limit)
     357                                        j1 = -limit;
     358                        }
     359
     360#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
     361
     362                        //GEN_set_min(contactConstraint.m_appliedImpulse, limit);
     363                        //GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
     364
     365                       
     366
     367                }
     368       
     369                body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
     370               
     371                body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
     372
     373        }
     374        return 0.f;
     375}
     376
     377
     378#else
     379
     380//velocity + friction
     381//response  between two dynamic objects with friction
     382btScalar resolveSingleFrictionCacheFriendly(
     383        btSolverBody& body1,
     384        btSolverBody& body2,
     385        btSolverConstraint& contactConstraint,
     386        const btContactSolverInfo& solverInfo)
     387{
     388
     389        btVector3 vel1;
     390        btVector3 vel2;
     391        btScalar normalImpulse(0.f);
     392
     393        {
     394                const btVector3& normal = contactConstraint.m_contactNormal;
     395                if (contactConstraint.m_penetration < 0.f)
     396                        return 0.f;
     397
     398
     399                body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
     400                body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
     401                btVector3 vel = vel1 - vel2;
     402                btScalar rel_vel;
     403                rel_vel = normal.dot(vel);
     404
     405                btVector3 lat_vel = vel - normal * rel_vel;
     406                btScalar lat_rel_vel = lat_vel.length2();
     407
     408                btScalar combinedFriction = contactConstraint.m_friction;
     409                const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
     410                const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
     411
     412
     413                if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
     414                {
     415                        lat_rel_vel = btSqrt(lat_rel_vel);
     416
     417                        lat_vel /= lat_rel_vel;
     418                        btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
     419                        btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
     420                        btScalar friction_impulse = lat_rel_vel /
     421                                (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
     422                        btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
     423
     424                        GEN_set_min(friction_impulse, normal_impulse);
     425                        GEN_set_max(friction_impulse, -normal_impulse);
     426                        body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
     427                        body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
     428                }
     429        }
     430
     431        return normalImpulse;
     432}
     433
     434#endif //NO_FRICTION_TANGENTIALS
     435
     436
    263437
    264438
    … …  
    266440btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
    267441{
    268 
    269442
    270443        btRigidBody* body0=btRigidBody::upcast(colObj0);
    271444        btRigidBody* body1=btRigidBody::upcast(colObj1);
    272445
    273         btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
    274         memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
     446        btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
    275447        solverConstraint.m_contactNormal = normalAxis;
    276448
    277449        solverConstraint.m_solverBodyIdA = solverBodyIdA;
    278450        solverConstraint.m_solverBodyIdB = solverBodyIdB;
     451        solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
    279452        solverConstraint.m_frictionIndex = frictionIndex;
    280453
    … …  
    282455        solverConstraint.m_originalContactPoint = 0;
    283456
    284         solverConstraint.m_appliedImpulse = 0.f;
    285         //      solverConstraint.m_appliedPushImpulse = 0.f;
    286 
     457        solverConstraint.m_appliedImpulse = btScalar(0.);
     458        solverConstraint.m_appliedPushImpulse = 0.f;
     459        solverConstraint.m_penetration = 0.f;
    287460        {
    288461                btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
    289462                solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
    290                 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
    291         }
    292         {
    293                 btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
     463                solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
     464        }
     465        {
     466                btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
    294467                solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
    295                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
     468                solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
    296469        }
    297470
    … …  
    310483        if (body1)
    311484        {
    312                 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
     485                vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
    313486                denom1 = body1->getInvMass() + normalAxis.dot(vec);
    314487        }
    … …  
    319492        solverConstraint.m_jacDiagABInv = denom;
    320493
    321 #ifdef _USE_JACOBIAN
    322         solverConstraint.m_jac =  btJacobianEntry (
    323                 rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
    324                 body0->getInvInertiaDiagLocal(),
    325                 body0->getInvMass(),
    326                 body1->getInvInertiaDiagLocal(),
    327                 body1->getInvMass());
    328 #endif //_USE_JACOBIAN
    329 
    330 
    331         {
    332                 btScalar rel_vel;
    333                 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0))
    334                         + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
    335                 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0))
    336                         + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
    337 
    338                 rel_vel = vel1Dotn+vel2Dotn;
    339 
    340                 btScalar positionalError = 0.f;
    341 
    342                 btSimdScalar velocityError =  - rel_vel;
    343                 btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
    344                 solverConstraint.m_rhs = velocityImpulse;
    345                 solverConstraint.m_cfm = 0.f;
    346                 solverConstraint.m_lowerLimit = 0;
    347                 solverConstraint.m_upperLimit = 1e10f;
    348         }
    349 
    350494        return solverConstraint;
    351495}
    352496
    353 int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
    354 {
    355         int solverBodyIdA = -1;
    356 
    357         if (body.getCompanionId() >= 0)
    358         {
    359                 //body has already been converted
    360                 solverBodyIdA = body.getCompanionId();
    361         } else
    362         {
    363                 btRigidBody* rb = btRigidBody::upcast(&body);
    364                 if (rb && rb->getInvMass())
    365                 {
    366                         solverBodyIdA = m_tmpSolverBodyPool.size();
    367                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
    368                         initSolverBody(&solverBody,&body);
    369                         body.setCompanionId(solverBodyIdA);
    370                 } else
    371                 {
    372                         return 0;//assume first one is a fixed solver body
    373                 }
    374         }
    375         return solverBodyIdA;
    376 }
    377 #include <stdio.h>
    378 
    379 
    380 
    381 void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
    382 {
     497
     498
     499btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
     500{
     501        BT_PROFILE("solveGroupCacheFriendlySetup");
     502        (void)stackAlloc;
     503        (void)debugDrawer;
     504
     505
     506        if (!(numConstraints + numManifolds))
     507        {
     508//              printf("empty\n");
     509                return 0.f;
     510        }
     511        btPersistentManifold* manifold = 0;
    383512        btCollisionObject* colObj0=0,*colObj1=0;
    384513
    385         colObj0 = (btCollisionObject*)manifold->getBody0();
    386         colObj1 = (btCollisionObject*)manifold->getBody1();
    387 
    388         int solverBodyIdA=-1;
    389         int solverBodyIdB=-1;
    390 
    391         if (manifold->getNumContacts())
    392         {
    393                 solverBodyIdA = getOrInitSolverBody(*colObj0);
    394                 solverBodyIdB = getOrInitSolverBody(*colObj1);
    395         }
    396 
    397         ///avoid collision response between two static objects
    398         if (!solverBodyIdA && !solverBodyIdB)
    399                 return;
    400 
    401         btVector3 rel_pos1;
    402         btVector3 rel_pos2;
    403         btScalar relaxation;
    404 
    405         for (int j=0;j<manifold->getNumContacts();j++)
    406         {
    407 
    408                 btManifoldPoint& cp = manifold->getContactPoint(j);
    409 
    410                 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
    411                 {
    412 
    413                         const btVector3& pos1 = cp.getPositionWorldOnA();
    414                         const btVector3& pos2 = cp.getPositionWorldOnB();
    415 
    416                         rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
    417                         rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
    418 
    419 
    420                         relaxation = 1.f;
    421                         btScalar rel_vel;
    422                         btVector3 vel;
    423 
    424                         int frictionIndex = m_tmpSolverContactConstraintPool.size();
    425 
    426                         {
    427                                 btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
    428                                 btRigidBody* rb0 = btRigidBody::upcast(colObj0);
    429                                 btRigidBody* rb1 = btRigidBody::upcast(colObj1);
    430 
    431                                 solverConstraint.m_solverBodyIdA = solverBodyIdA;
    432                                 solverConstraint.m_solverBodyIdB = solverBodyIdB;
    433 
    434                                 solverConstraint.m_originalContactPoint = &cp;
    435 
    436                                 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
    437                                 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
    438                                 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
    439                                 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
    440                                 {
    441 #ifdef COMPUTE_IMPULSE_DENOM
    442                                         btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
    443                                         btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
    444 #else                                                   
    445                                         btVector3 vec;
    446                                         btScalar denom0 = 0.f;
    447                                         btScalar denom1 = 0.f;
    448                                         if (rb0)
     514        //btRigidBody* rb0=0,*rb1=0;
     515
     516
     517#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
     518
     519        BEGIN_PROFILE("refreshManifolds");
     520
     521        int i;
     522       
     523       
     524
     525        for (i=0;i<numManifolds;i++)
     526        {
     527                manifold = manifoldPtr[i];
     528                rb1 = (btRigidBody*)manifold->getBody1();
     529                rb0 = (btRigidBody*)manifold->getBody0();
     530               
     531                manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
     532
     533        }
     534
     535        END_PROFILE("refreshManifolds");
     536#endif //FORCE_REFESH_CONTACT_MANIFOLDS
     537
     538       
     539
     540
     541
     542        //int sizeofSB = sizeof(btSolverBody);
     543        //int sizeofSC = sizeof(btSolverConstraint);
     544
     545
     546        //if (1)
     547        {
     548                //if m_stackAlloc, try to pack bodies/constraints to speed up solving
     549//              btBlock*                                        sablock;
     550//              sablock = stackAlloc->beginBlock();
     551
     552        //      int memsize = 16;
     553//              unsigned char* stackMemory = stackAlloc->allocate(memsize);
     554
     555               
     556                //todo: use stack allocator for this temp memory
     557//              int minReservation = numManifolds*2;
     558
     559                //m_tmpSolverBodyPool.reserve(minReservation);
     560
     561                //don't convert all bodies, only the one we need so solver the constraints
     562/*
     563                {
     564                        for (int i=0;i<numBodies;i++)
     565                        {
     566                                btRigidBody* rb = btRigidBody::upcast(bodies[i]);
     567                                if (rb &&       (rb->getIslandTag() >= 0))
     568                                {
     569                                        btAssert(rb->getCompanionId() < 0);
     570                                        int solverBodyId = m_tmpSolverBodyPool.size();
     571                                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     572                                        initSolverBody(&solverBody,rb);
     573                                        rb->setCompanionId(solverBodyId);
     574                                }
     575                        }
     576                }
     577*/
     578               
     579                //m_tmpSolverConstraintPool.reserve(minReservation);
     580                //m_tmpSolverFrictionConstraintPool.reserve(minReservation);
     581
     582                {
     583                        int i;
     584
     585                        for (i=0;i<numManifolds;i++)
     586                        {
     587                                manifold = manifoldPtr[i];
     588                                colObj0 = (btCollisionObject*)manifold->getBody0();
     589                                colObj1 = (btCollisionObject*)manifold->getBody1();
     590                       
     591                                int solverBodyIdA=-1;
     592                                int solverBodyIdB=-1;
     593
     594                                if (manifold->getNumContacts())
     595                                {
     596
     597                                       
     598
     599                                        if (colObj0->getIslandTag() >= 0)
    449600                                        {
    450                                                 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
    451                                                 denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
    452                                         }
    453                                         if (rb1)
    454                                         {
    455                                                 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
    456                                                 denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
    457                                         }
    458 #endif //COMPUTE_IMPULSE_DENOM         
    459 
    460                                         btScalar denom = relaxation/(denom0+denom1);
    461                                         solverConstraint.m_jacDiagABInv = denom;
    462                                 }
    463 
    464                                 solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
    465                                 solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
    466                                 solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
    467 
    468 
    469                                 btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
    470                                 btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
    471 
    472                                 vel  = vel1 - vel2;
    473 
    474                                 rel_vel = cp.m_normalWorldOnB.dot(vel);
    475 
    476                                 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
    477 
    478 
    479                                 solverConstraint.m_friction = cp.m_combinedFriction;
    480 
    481                                 btScalar restitution = 0.f;
    482                                
    483                                 if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
    484                                 {
    485                                         restitution = 0.f;
    486                                 } else
    487                                 {
    488                                         restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
    489                                         if (restitution <= btScalar(0.))
    490                                         {
    491                                                 restitution = 0.f;
    492                                         };
    493                                 }
    494 
    495 
    496                                 ///warm starting (or zero if disabled)
    497                                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
    498                                 {
    499                                         solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
    500                                         if (rb0)
    501                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
    502                                         if (rb1)
    503                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
    504                                 } else
    505                                 {
    506                                         solverConstraint.m_appliedImpulse = 0.f;
    507                                 }
    508 
    509                                 //                                                      solverConstraint.m_appliedPushImpulse = 0.f;
    510 
    511                                 {
    512                                         btScalar rel_vel;
    513                                         btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
    514                                                 + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
    515                                         btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
    516                                                 + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
    517 
    518                                         rel_vel = vel1Dotn+vel2Dotn;
    519 
    520                                         btScalar positionalError = 0.f;
    521                                         positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
    522                                         btScalar        velocityError = restitution - rel_vel;// * damping;
    523                                         btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
    524                                         btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
    525                                         solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
    526                                         solverConstraint.m_cfm = 0.f;
    527                                         solverConstraint.m_lowerLimit = 0;
    528                                         solverConstraint.m_upperLimit = 1e10f;
    529                                 }
    530 
    531 
    532                                 /////setup the friction constraints
    533 
    534 
    535 
    536                                 if (1)
    537                                 {
    538                                         solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
    539                                         if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
    540                                         {
    541                                                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
    542                                                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
    543                                                 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
     601                                                if (colObj0->getCompanionId() >= 0)
    544602                                                {
    545                                                         cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
    546                                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
    547                                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
    548                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    549                                                         if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
    550                                                         {
    551                                                                 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
    552                                                                 cp.m_lateralFrictionDir2.normalize();//??
    553                                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
    554                                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
    555                                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    556                                                         }
    557                                                         cp.m_lateralFrictionInitialized = true;
     603                                                        //body has already been converted
     604                                                        solverBodyIdA = colObj0->getCompanionId();
    558605                                                } else
    559606                                                {
    560                                                         //re-calculate friction direction every frame, todo: check if this is really needed
    561                                                         btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
    562                                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
    563                                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
    564 
    565                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    566                                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
    567                                                         {
    568                                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
    569                                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
    570                                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    571                                                         }
    572                                                         cp.m_lateralFrictionInitialized = true;
    573                                                 }
    574 
    575                                         } else
    576                                         {
    577                                                 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    578                                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
    579                                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    580                                         }
    581 
    582                                         if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    583                                         {
    584                                                 {
    585                                                         btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
    586                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
    587                                                         {
    588                                                                 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
    589                                                                 if (rb0)
    590                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
    591                                                                 if (rb1)
    592                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
    593                                                         } else
    594                                                         {
    595                                                                 frictionConstraint1.m_appliedImpulse = 0.f;
    596                                                         }
    597                                                 }
    598 
    599                                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
    600                                                 {
    601                                                         btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
    602                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
    603                                                         {
    604                                                                 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
    605                                                                 if (rb0)
    606                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
    607                                                                 if (rb1)
    608                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
    609                                                         } else
    610                                                         {
    611                                                                 frictionConstraint2.m_appliedImpulse = 0.f;
    612                                                         }
     607                                                        solverBodyIdA = m_tmpSolverBodyPool.size();
     608                                                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     609                                                        initSolverBody(&solverBody,colObj0);
     610                                                        colObj0->setCompanionId(solverBodyIdA);
    613611                                                }
    614612                                        } else
    615613                                        {
    616                                                 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
    617                                                 frictionConstraint1.m_appliedImpulse = 0.f;
    618                                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
     614                                                //create a static body
     615                                                solverBodyIdA = m_tmpSolverBodyPool.size();
     616                                                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     617                                                initSolverBody(&solverBody,colObj0);
     618                                        }
     619
     620                                        if (colObj1->getIslandTag() >= 0)
     621                                        {
     622                                                if (colObj1->getCompanionId() >= 0)
    619623                                                {
    620                                                         btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
    621                                                         frictionConstraint2.m_appliedImpulse = 0.f;
     624                                                        solverBodyIdB = colObj1->getCompanionId();
     625                                                } else
     626                                                {
     627                                                        solverBodyIdB = m_tmpSolverBodyPool.size();
     628                                                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     629                                                        initSolverBody(&solverBody,colObj1);
     630                                                        colObj1->setCompanionId(solverBodyIdB);
    622631                                                }
     632                                        } else
     633                                        {
     634                                                //create a static body
     635                                                solverBodyIdB = m_tmpSolverBodyPool.size();
     636                                                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     637                                                initSolverBody(&solverBody,colObj1);
    623638                                        }
    624639                                }
    625                         }
    626 
    627 
    628                 }
    629         }
    630 }
    631 
    632 
    633 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
    634 {
    635         BT_PROFILE("solveGroupCacheFriendlySetup");
    636         (void)stackAlloc;
    637         (void)debugDrawer;
    638 
    639 
    640         if (!(numConstraints + numManifolds))
    641         {
    642                 //              printf("empty\n");
    643                 return 0.f;
    644         }
    645 
    646         if (1)
     640
     641                                btVector3 rel_pos1;
     642                                btVector3 rel_pos2;
     643                                btScalar relaxation;
     644
     645                                for (int j=0;j<manifold->getNumContacts();j++)
     646                                {
     647                                       
     648                                        btManifoldPoint& cp = manifold->getContactPoint(j);
     649                                       
     650                                        if (cp.getDistance() <= btScalar(0.))
     651                                        {
     652                                               
     653                                                const btVector3& pos1 = cp.getPositionWorldOnA();
     654                                                const btVector3& pos2 = cp.getPositionWorldOnB();
     655
     656                                                 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
     657                                                 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
     658
     659                                               
     660                                                relaxation = 1.f;
     661                                                btScalar rel_vel;
     662                                                btVector3 vel;
     663
     664                                                int frictionIndex = m_tmpSolverConstraintPool.size();
     665
     666                                                {
     667                                                        btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
     668                                                        btRigidBody* rb0 = btRigidBody::upcast(colObj0);
     669                                                        btRigidBody* rb1 = btRigidBody::upcast(colObj1);
     670
     671                                                        solverConstraint.m_solverBodyIdA = solverBodyIdA;
     672                                                        solverConstraint.m_solverBodyIdB = solverBodyIdB;
     673                                                        solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
     674
     675                                                        solverConstraint.m_originalContactPoint = &cp;
     676
     677                                                        btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
     678                                                        solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
     679                                                        btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
     680                                                        solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
     681                                                        {
     682#ifdef COMPUTE_IMPULSE_DENOM
     683                                                                btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
     684                                                                btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
     685#else                                                   
     686                                                                btVector3 vec;
     687                                                                btScalar denom0 = 0.f;
     688                                                                btScalar denom1 = 0.f;
     689                                                                if (rb0)
     690                                                                {
     691                                                                        vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
     692                                                                        denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
     693                                                                }
     694                                                                if (rb1)
     695                                                                {
     696                                                                        vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
     697                                                                        denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
     698                                                                }
     699#endif //COMPUTE_IMPULSE_DENOM         
     700                                                               
     701                                                                btScalar denom = relaxation/(denom0+denom1);
     702                                                                solverConstraint.m_jacDiagABInv = denom;
     703                                                        }
     704
     705                                                        solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
     706                                                        solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
     707                                                        solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
     708
     709
     710                                                        btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
     711                                                        btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
     712                               
     713                                                        vel  = vel1 - vel2;
     714                                                       
     715                                                        rel_vel = cp.m_normalWorldOnB.dot(vel);
     716                                                       
     717                                                        solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
     718                                                        //solverConstraint.m_penetration = cp.getDistance();
     719
     720                                                        solverConstraint.m_friction = cp.m_combinedFriction;
     721
     722                                                       
     723                                                        if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
     724                                                        {
     725                                                                solverConstraint.m_restitution = 0.f;
     726                                                        } else
     727                                                        {
     728                                                                solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
     729                                                                if (solverConstraint.m_restitution <= btScalar(0.))
     730                                                                {
     731                                                                        solverConstraint.m_restitution = 0.f;
     732                                                                };
     733                                                        }
     734
     735                                                       
     736                                                        btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
     737
     738                                                       
     739
     740                                                        if (solverConstraint.m_restitution > penVel)
     741                                                        {
     742                                                                solverConstraint.m_penetration = btScalar(0.);
     743                                                        }
     744                                                         
     745                                                       
     746                                                       
     747                                                        ///warm starting (or zero if disabled)
     748                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
     749                                                        {
     750                                                                solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
     751                                                                if (rb0)
     752                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
     753                                                                if (rb1)
     754                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
     755                                                        } else
     756                                                        {
     757                                                                solverConstraint.m_appliedImpulse = 0.f;
     758                                                        }
     759
     760                                                        solverConstraint.m_appliedPushImpulse = 0.f;
     761                                                       
     762                                                        solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
     763                                                        if (!cp.m_lateralFrictionInitialized)
     764                                                        {
     765                                                                cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
     766                                                                btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
     767                                                                if (lat_rel_vel > SIMD_EPSILON)//0.0f)
     768                                                                {
     769                                                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
     770                                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     771                                                                        if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     772                                                                        {
     773                                                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
     774                                                                                cp.m_lateralFrictionDir2.normalize();//??
     775                                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     776                                                                                cp.m_lateralFrictionInitialized = true;
     777                                                                        }
     778                                                                } else
     779                                                                {
     780                                                                        //re-calculate friction direction every frame, todo: check if this is really needed
     781                                                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
     782                                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     783                                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     784                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     785                                                                        {
     786                                                                                cp.m_lateralFrictionInitialized = true;
     787                                                                        }
     788                                                                }
     789                                                               
     790                                                        } else
     791                                                        {
     792                                                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     793                                                                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     794                                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     795                                                        }
     796
     797                                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     798                                                        {
     799                                                                {
     800                                                                        btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
     801                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
     802                                                                        {
     803                                                                                frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
     804                                                                                if (rb0)
     805                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
     806                                                                                if (rb1)
     807                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
     808                                                                        } else
     809                                                                        {
     810                                                                                frictionConstraint1.m_appliedImpulse = 0.f;
     811                                                                        }
     812                                                                }
     813                                                                {
     814                                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
     815                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
     816                                                                        {
     817                                                                                frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
     818                                                                                if (rb0)
     819                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
     820                                                                                if (rb1)
     821                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
     822                                                                        } else
     823                                                                        {
     824                                                                                frictionConstraint2.m_appliedImpulse = 0.f;
     825                                                                        }
     826                                                                }
     827                                                        }
     828                                                }
     829
     830
     831                                        }
     832                                }
     833                        }
     834                }
     835        }
     836       
     837        btContactSolverInfo info = infoGlobal;
     838
    647839        {
    648840                int j;
    … …  
    653845                }
    654846        }
    655 
    656         btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
    657         initSolverBody(&fixedBody,0);
    658 
    659         //btRigidBody* rb0=0,*rb1=0;
    660 
    661         //if (1)
    662         {
    663                 {
    664 
    665                         int totalNumRows = 0;
    666                         int i;
    667                         //calculate the total number of contraint rows
    668                         for (i=0;i<numConstraints;i++)
    669                         {
    670 
    671                                 btTypedConstraint::btConstraintInfo1 info1;
    672                                 constraints[i]->getInfo1(&info1);
    673                                 totalNumRows += info1.m_numConstraintRows;
    674                         }
    675                         m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
    676 
    677                         btTypedConstraint::btConstraintInfo1 info1;
    678                         info1.m_numConstraintRows = 0;
    679 
    680 
    681                         ///setup the btSolverConstraints
    682                         int currentRow = 0;
    683 
    684                         for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
    685                         {
    686                                 constraints[i]->getInfo1(&info1);
    687                                 if (info1.m_numConstraintRows)
    688                                 {
    689                                         btAssert(currentRow<totalNumRows);
    690 
    691                                         btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
    692                                         btTypedConstraint* constraint = constraints[i];
    693 
    694 
    695 
    696                                         btRigidBody& rbA = constraint->getRigidBodyA();
    697                                         btRigidBody& rbB = constraint->getRigidBodyB();
    698 
    699                                         int solverBodyIdA = getOrInitSolverBody(rbA);
    700                                         int solverBodyIdB = getOrInitSolverBody(rbB);
    701 
    702                                         btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
    703                                         btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
    704 
    705                                         int j;
    706                                         for ( j=0;j<info1.m_numConstraintRows;j++)
    707                                         {
    708                                                 memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
    709                                                 currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
    710                                                 currentConstraintRow[j].m_upperLimit = FLT_MAX;
    711                                                 currentConstraintRow[j].m_appliedImpulse = 0.f;
    712                                                 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
    713                                                 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
    714                                                 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
    715                                         }
    716 
    717                                         bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
    718                                         bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
    719                                         bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
    720                                         bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
    721 
    722 
    723 
    724                                         btTypedConstraint::btConstraintInfo2 info2;
    725                                         info2.fps = 1.f/infoGlobal.m_timeStep;
    726                                         info2.erp = infoGlobal.m_erp;
    727                                         info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
    728                                         info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
    729                                         info2.m_J2linearAxis = 0;
    730                                         info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
    731                                         info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
    732                                         ///the size of btSolverConstraint needs be a multiple of btScalar
    733                                         btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
    734                                         info2.m_constraintError = &currentConstraintRow->m_rhs;
    735                                         info2.cfm = &currentConstraintRow->m_cfm;
    736                                         info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
    737                                         info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
    738                                         constraints[i]->getInfo2(&info2);
    739 
    740                                         ///finalize the constraint setup
    741                                         for ( j=0;j<info1.m_numConstraintRows;j++)
    742                                         {
    743                                                 btSolverConstraint& solverConstraint = currentConstraintRow[j];
    744 
    745                                                 {
    746                                                         const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
    747                                                         solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
    748                                                 }
    749                                                 {
    750                                                         const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
    751                                                         solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
    752                                                 }
    753 
    754                                                 {
    755                                                         btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
    756                                                         btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
    757                                                         btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
    758                                                         btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
    759 
    760                                                         btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
    761                                                         sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
    762                                                         sum += iMJlB.dot(solverConstraint.m_contactNormal);
    763                                                         sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
    764 
    765                                                         solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
    766                                                 }
    767 
    768 
    769                                                 ///fix rhs
    770                                                 ///todo: add force/torque accelerators
    771                                                 {
    772                                                         btScalar rel_vel;
    773                                                         btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
    774                                                         btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
    775 
    776                                                         rel_vel = vel1Dotn+vel2Dotn;
    777 
    778                                                         btScalar restitution = 0.f;
    779                                                         btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
    780                                                         btScalar        velocityError = restitution - rel_vel;// * damping;
    781                                                         btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
    782                                                         btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
    783                                                         solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
    784                                                         solverConstraint.m_appliedImpulse = 0.f;
    785 
    786                                                 }
    787                                         }
    788                                 }
    789                         }
    790                 }
    791 
    792                 {
    793                         int i;
    794                         btPersistentManifold* manifold = 0;
    795                         btCollisionObject* colObj0=0,*colObj1=0;
    796 
    797 
    798                         for (i=0;i<numManifolds;i++)
    799                         {
    800                                 manifold = manifoldPtr[i];
    801                                 convertContact(manifold,infoGlobal);
    802                         }
    803                 }
    804         }
    805 
    806         btContactSolverInfo info = infoGlobal;
    807 
    808 
    809 
    810         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
    811         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
     847       
     848       
     849
     850        int numConstraintPool = m_tmpSolverConstraintPool.size();
     851        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
    812852
    813853        ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
    … …  
    826866        }
    827867
     868
     869
    828870        return 0.f;
    829871
    … …  
    833875{
    834876        BT_PROFILE("solveGroupCacheFriendlyIterations");
    835 
    836         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
    837         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
     877        int numConstraintPool = m_tmpSolverConstraintPool.size();
     878        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
    838879
    839880        //should traverse the contacts random order...
    … …  
    863904                        }
    864905
    865                         if (infoGlobal.m_solverMode & SOLVER_SIMD)
    866                         {
    867                                 ///solve all joint constraints, using SIMD, if available
    868                                 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
    869                                 {
    870                                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
    871                                         resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
    872                                 }
    873 
    874                                 for (j=0;j<numConstraints;j++)
    875                                 {
    876                                         int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
    877                                         int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
    878                                         btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
    879                                         btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
    880                                         constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
    881                                 }
    882 
    883                                 ///solve all contact constraints using SIMD, if available
    884                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     906                        for (j=0;j<numConstraints;j++)
     907                        {
     908                                btTypedConstraint* constraint = constraints[j];
     909                                ///todo: use solver bodies, so we don't need to copy from/to btRigidBody
     910
     911                                if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
     912                                {
     913                                        m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
     914                                }
     915                                if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
     916                                {
     917                                        m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
     918                                }
     919
     920                                constraint->solveConstraint(infoGlobal.m_timeStep);
     921
     922                                if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
     923                                {
     924                                        m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
     925                                }
     926                                if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
     927                                {
     928                                        m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
     929                                }
     930
     931                        }
     932
     933                        {
     934                                int numPoolConstraints = m_tmpSolverConstraintPool.size();
    885935                                for (j=0;j<numPoolConstraints;j++)
    886936                                {
    887                                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
    888                                         resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
    889 
    890                                 }
    891                                 ///solve all friction constraints, using SIMD, if available
    892                                 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
    893                                 for (j=0;j<numFrictionPoolConstraints;j++)
    894                                 {
    895                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
    896                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    897 
    898                                         if (totalImpulse>btScalar(0))
     937                                       
     938                                        const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
     939                                        resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
     940                                                m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
     941                                }
     942                        }
     943
     944                        {
     945                                 int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
     946                               
     947                                 for (j=0;j<numFrictionPoolConstraints;j++)
     948                                {
     949                                        const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
     950                                        btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
     951                                                                m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;                 
     952
     953                                                resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
     954                                                        m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
     955                                                        totalImpulse);
     956                                }
     957                        }
     958                       
     959
     960
     961                }
     962       
     963                if (infoGlobal.m_splitImpulse)
     964                {
     965                       
     966                        for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
     967                        {
     968                                {
     969                                        int numPoolConstraints = m_tmpSolverConstraintPool.size();
     970                                        int j;
     971                                        for (j=0;j<numPoolConstraints;j++)
    899972                                        {
    900                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
    901                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
    902 
    903                                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],       m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
     973                                                const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
     974
     975                                                resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
     976                                                        m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
    904977                                        }
    905978                                }
    906                         } else
    907                         {
    908 
    909                                 ///solve all joint constraints
    910                                 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
    911                                 {
    912                                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
    913                                         resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
    914                                 }
    915 
    916                                 for (j=0;j<numConstraints;j++)
    917                                 {
    918                                         int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
    919                                         int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
    920                                         btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
    921                                         btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
    922 
    923                                         constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
    924                                 }
    925 
    926                                 ///solve all contact constraints
    927                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
    928                                 for (j=0;j<numPoolConstraints;j++)
    929                                 {
    930                                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
    931                                         resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
    932                                 }
    933                                 ///solve all friction constraints
    934                                 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
    935                                 for (j=0;j<numFrictionPoolConstraints;j++)
    936                                 {
    937                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
    938                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    939 
    940                                         if (totalImpulse>btScalar(0))
    941                                         {
    942                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
    943                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
    944 
    945                                                 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],                                                   m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
    946                                         }
    947                                 }
    948                         }
    949 
    950 
    951 
    952                 }
    953         }
     979                        }
     980
     981                }
     982
     983        }
     984
    954985        return 0.f;
    955986}
    956987
    957988
    958 
    959 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
    960 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
    961 {
    962 
    963        
    964 
    965         BT_PROFILE("solveGroup");
    966         //we only implement SOLVER_CACHE_FRIENDLY now
    967         //you need to provide at least some bodies
    968         btAssert(bodies);
    969         btAssert(numBodies);
    970 
     989btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
     990{
    971991        int i;
    972992
    … …  
    974994        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
    975995
    976         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     996        int numPoolConstraints = m_tmpSolverConstraintPool.size();
    977997        int j;
    978 
    979998        for (j=0;j<numPoolConstraints;j++)
    980999        {
    981 
    982                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
     1000               
     1001                const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
    9831002                btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
    9841003                btAssert(pt);
    … …  
    9861005                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    9871006                {
    988                         pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    989                         pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
     1007                        pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
     1008                        pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
    9901009                }
    9911010
    9921011                //do a callback here?
     1012
    9931013        }
    9941014
    … …  
    10021022        {
    10031023                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
    1004                 {
    1005                         m_tmpSolverBodyPool[i].writebackVelocity();
    1006                 }
    1007         }
    1008 
     1024        {
     1025                m_tmpSolverBodyPool[i].writebackVelocity();
     1026        }
     1027        }
     1028
     1029//      printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
     1030
     1031/*
     1032        printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
     1033        printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
     1034        printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
     1035
     1036       
     1037        printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
     1038        printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
     1039        printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
     1040*/
    10091041
    10101042        m_tmpSolverBodyPool.resize(0);
    1011         m_tmpSolverContactConstraintPool.resize(0);
    1012         m_tmpSolverNonContactConstraintPool.resize(0);
    1013         m_tmpSolverContactFrictionConstraintPool.resize(0);
     1043        m_tmpSolverConstraintPool.resize(0);
     1044        m_tmpSolverFrictionConstraintPool.resize(0);
     1045
    10141046
    10151047        return 0.f;
    10161048}
    10171049
    1018 
    1019 
    1020 
    1021 
    1022 
    1023 
     1050/// btSequentialImpulseConstraintSolver Sequentially applies impulses
     1051btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
     1052{
     1053        BT_PROFILE("solveGroup");
     1054        if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
     1055        {
     1056                //you need to provide at least some bodies
     1057                //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
     1058                btAssert(bodies);
     1059                btAssert(numBodies);
     1060                return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
     1061        }
     1062
     1063       
     1064
     1065        btContactSolverInfo info = infoGlobal;
     1066
     1067        int numiter = infoGlobal.m_numIterations;
     1068
     1069        int totalPoints = 0;
     1070
     1071
     1072        {
     1073                short j;
     1074                for (j=0;j<numManifolds;j++)
     1075                {
     1076                        btPersistentManifold* manifold = manifoldPtr[j];
     1077                        prepareConstraints(manifold,info,debugDrawer);
     1078
     1079                        for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
     1080                        {
     1081                                gOrder[totalPoints].m_manifoldIndex = j;
     1082                                gOrder[totalPoints].m_pointIndex = p;
     1083                                totalPoints++;
     1084                        }
     1085                }
     1086        }
     1087
     1088        {
     1089                int j;
     1090                for (j=0;j<numConstraints;j++)
     1091                {
     1092                        btTypedConstraint* constraint = constraints[j];
     1093                        constraint->buildJacobian();
     1094                }
     1095        }
     1096       
     1097
     1098        //should traverse the contacts random order...
     1099        int iteration;
     1100
     1101        {
     1102                for ( iteration = 0;iteration<numiter;iteration++)
     1103                {
     1104                        int j;
     1105                        if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
     1106                        {
     1107                                if ((iteration & 7) == 0) {
     1108                                        for (j=0; j<totalPoints; ++j) {
     1109                                                btOrderIndex tmp = gOrder[j];
     1110                                                int swapi = btRandInt2(j+1);
     1111                                                gOrder[j] = gOrder[swapi];
     1112                                                gOrder[swapi] = tmp;
     1113                                        }
     1114                                }
     1115                        }
     1116
     1117                        for (j=0;j<numConstraints;j++)
     1118                        {
     1119                                btTypedConstraint* constraint = constraints[j];
     1120                                constraint->solveConstraint(info.m_timeStep);
     1121                        }
     1122
     1123                        for (j=0;j<totalPoints;j++)
     1124                        {
     1125                                btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
     1126                                solve( (btRigidBody*)manifold->getBody0(),
     1127                                                                        (btRigidBody*)manifold->getBody1()
     1128                                ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
     1129                        }
     1130               
     1131                        for (j=0;j<totalPoints;j++)
     1132                        {
     1133                                btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
     1134                                solveFriction((btRigidBody*)manifold->getBody0(),
     1135                                        (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
     1136                        }
     1137                       
     1138                }
     1139        }
     1140               
     1141
     1142
     1143
     1144        return btScalar(0.);
     1145}
     1146
     1147
     1148
     1149
     1150
     1151
     1152
     1153void    btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
     1154{
     1155
     1156        (void)debugDrawer;
     1157
     1158        btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
     1159        btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
     1160
     1161
     1162        //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
     1163        {
     1164#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
     1165                manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
     1166#endif //FORCE_REFESH_CONTACT_MANIFOLDS         
     1167                int numpoints = manifoldPtr->getNumContacts();
     1168
     1169                gTotalContactPoints += numpoints;
     1170
     1171               
     1172                for (int i=0;i<numpoints ;i++)
     1173                {
     1174                        btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
     1175                        if (cp.getDistance() <= btScalar(0.))
     1176                        {
     1177                                const btVector3& pos1 = cp.getPositionWorldOnA();
     1178                                const btVector3& pos2 = cp.getPositionWorldOnB();
     1179
     1180                                btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition();
     1181                                btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
     1182                               
     1183
     1184                                //this jacobian entry is re-used for all iterations
     1185                                btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
     1186                                        body1->getCenterOfMassTransform().getBasis().transpose(),
     1187                                        rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
     1188                                        body1->getInvInertiaDiagLocal(),body1->getInvMass());
     1189
     1190                               
     1191                                btScalar jacDiagAB = jac.getDiagonal();
     1192
     1193                                btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
     1194                                if (cpd)
     1195                                {
     1196                                        //might be invalid
     1197                                        cpd->m_persistentLifeTime++;
     1198                                        if (cpd->m_persistentLifeTime != cp.getLifeTime())
     1199                                        {
     1200                                                //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
     1201                                                new (cpd) btConstraintPersistentData;
     1202                                                cpd->m_persistentLifeTime = cp.getLifeTime();
     1203
     1204                                        } else
     1205                                        {
     1206                                                //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
     1207                                               
     1208                                        }
     1209                                } else
     1210                                {
     1211                                               
     1212                                        //todo: should this be in a pool?
     1213                                        void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
     1214                                        cpd = new (mem)btConstraintPersistentData;
     1215                                        assert(cpd);
     1216
     1217                                        totalCpd ++;
     1218                                        //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
     1219                                        cp.m_userPersistentData = cpd;
     1220                                        cpd->m_persistentLifeTime = cp.getLifeTime();
     1221                                        //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
     1222                                       
     1223                                }
     1224                                assert(cpd);
     1225
     1226                                cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
     1227
     1228                                //Dependent on Rigidbody A and B types, fetch the contact/friction response func
     1229                                //perhaps do a similar thing for friction/restutution combiner funcs...
     1230                               
     1231                                cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
     1232                                cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
     1233                               
     1234                                btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
     1235                                btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
     1236                                btVector3 vel = vel1 - vel2;
     1237                                btScalar rel_vel;
     1238                                rel_vel = cp.m_normalWorldOnB.dot(vel);
     1239                               
     1240                                btScalar combinedRestitution = cp.m_combinedRestitution;
     1241                               
     1242                                cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
     1243                                cpd->m_friction = cp.m_combinedFriction;
     1244                                if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold)
     1245                                {
     1246                                        cpd->m_restitution = 0.f;
     1247                                } else
     1248                                {
     1249                                        cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
     1250                                        if (cpd->m_restitution <= btScalar(0.))
     1251                                        {
     1252                                                cpd->m_restitution = btScalar(0.0);
     1253                                        };
     1254                                }
     1255                               
     1256                                //restitution and penetration work in same direction so
     1257                                //rel_vel
     1258                               
     1259                                btScalar penVel = -cpd->m_penetration/info.m_timeStep;
     1260
     1261                                if (cpd->m_restitution > penVel)
     1262                                {
     1263                                        cpd->m_penetration = btScalar(0.);
     1264                                }                               
     1265                               
     1266
     1267                                btScalar relaxation = info.m_damping;
     1268                                if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
     1269                                {
     1270                                        cpd->m_appliedImpulse *= relaxation;
     1271                                } else
     1272                                {
     1273                                        cpd->m_appliedImpulse =btScalar(0.);
     1274                                }
     1275       
     1276                                //for friction
     1277                                cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
     1278                               
     1279                                //re-calculate friction direction every frame, todo: check if this is really needed
     1280                                btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
     1281
     1282
     1283#define NO_FRICTION_WARMSTART 1
     1284
     1285        #ifdef NO_FRICTION_WARMSTART
     1286                                cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
     1287                                cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
     1288        #endif //NO_FRICTION_WARMSTART
     1289                                btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
     1290                                btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
     1291                                btScalar denom = relaxation/(denom0+denom1);
     1292                                cpd->m_jacDiagABInvTangent0 = denom;
     1293
     1294
     1295                                denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
     1296                                denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
     1297                                denom = relaxation/(denom0+denom1);
     1298                                cpd->m_jacDiagABInvTangent1 = denom;
     1299
     1300                                btVector3 totalImpulse =
     1301        #ifndef NO_FRICTION_WARMSTART
     1302                                        cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
     1303                                        cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
     1304        #endif //NO_FRICTION_WARMSTART
     1305                                        cp.m_normalWorldOnB*cpd->m_appliedImpulse;
     1306
     1307
     1308
     1309                                ///
     1310                                {
     1311                                btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
     1312                                cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
     1313                                btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
     1314                                cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
     1315                                }
     1316                                {
     1317                                        btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
     1318                                        cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
     1319                                }
     1320                                {
     1321                                        btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
     1322                                        cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
     1323                                }
     1324                                {
     1325                                        btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
     1326                                        cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
     1327                                }
     1328                                {
     1329                                        btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
     1330                                        cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
     1331                                }
     1332                               
     1333                                ///
     1334
     1335
     1336
     1337                                //apply previous frames impulse on both bodies
     1338                                body0->applyImpulse(totalImpulse, rel_pos1);
     1339                                body1->applyImpulse(-totalImpulse, rel_pos2);
     1340                        }
     1341                       
     1342                }
     1343        }
     1344}
     1345
     1346
     1347btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
     1348{
     1349        btScalar maxImpulse = btScalar(0.);
     1350
     1351        {
     1352
     1353               
     1354                {
     1355                        if (cp.getDistance() <= btScalar(0.))
     1356                        {
     1357
     1358                               
     1359
     1360                                {
     1361
     1362                                        //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
     1363                                        btScalar impulse = resolveSingleCollisionCombined(
     1364                                                *body0,*body1,
     1365                                                cp,
     1366                                                info);
     1367
     1368                                        if (maxImpulse < impulse)
     1369                                                maxImpulse  = impulse;
     1370
     1371                                }
     1372                        }
     1373                }
     1374        }
     1375        return maxImpulse;
     1376}
     1377
     1378
     1379
     1380btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
     1381{
     1382
     1383        btScalar maxImpulse = btScalar(0.);
     1384
     1385        {
     1386
     1387               
     1388                {
     1389                        if (cp.getDistance() <= btScalar(0.))
     1390                        {
     1391
     1392                               
     1393
     1394                                {
     1395
     1396                                        btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
     1397                                        btScalar impulse = cpd->m_contactSolverFunc(
     1398                                                *body0,*body1,
     1399                                                cp,
     1400                                                info);
     1401
     1402                                        if (maxImpulse < impulse)
     1403                                                maxImpulse  = impulse;
     1404
     1405                                }
     1406                        }
     1407                }
     1408        }
     1409        return maxImpulse;
     1410}
     1411
     1412btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
     1413{
     1414
     1415        (void)debugDrawer;
     1416        (void)iter;
     1417
     1418
     1419        {
     1420
     1421               
     1422                {
     1423                       
     1424                        if (cp.getDistance() <= btScalar(0.))
     1425                        {
     1426
     1427                                btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
     1428                                cpd->m_frictionSolverFunc(
     1429                                        *body0,*body1,
     1430                                        cp,
     1431                                        info);
     1432
     1433                               
     1434                        }
     1435                }
     1436
     1437       
     1438        }
     1439        return btScalar(0.);
     1440}
    10241441
    10251442

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

    r2907 r2908  
    2424
    2525
    26 
    27 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
     26///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
     27///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
     28///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
     29///Applies impulses for combined restitution and penetration recovery and to simulate friction
    2830class btSequentialImpulseConstraintSolver : public btConstraintSolver
    2931{
    30 protected:
    3132
    3233        btAlignedObjectArray<btSolverBody>      m_tmpSolverBodyPool;
    33         btConstraintArray                       m_tmpSolverContactConstraintPool;
    34         btConstraintArray                       m_tmpSolverNonContactConstraintPool;
    35         btConstraintArray                       m_tmpSolverContactFrictionConstraintPool;
     34        btAlignedObjectArray<btSolverConstraint>        m_tmpSolverConstraintPool;
     35        btAlignedObjectArray<btSolverConstraint>        m_tmpSolverFrictionConstraintPool;
    3636        btAlignedObjectArray<int>       m_orderTmpConstraintPool;
    3737        btAlignedObjectArray<int>       m_orderFrictionConstraintPool;
    3838
     39
     40protected:
     41        btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
     42        btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
     43        void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
    3944        btSolverConstraint&     addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
     45
     46        ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
     47        ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
     48
    4049       
    4150        ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
    4251        unsigned long   m_btSeed2;
    4352
    44         void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
    45         btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
    46 
    47         void    convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
    48 
    49         void    resolveSplitPenetrationImpulseCacheFriendly(
    50         btSolverBody& body1,
    51         btSolverBody& body2,
    52         const btSolverConstraint& contactConstraint,
    53         const btContactSolverInfo& solverInfo);
    54 
    55         //internal method
    56         int     getOrInitSolverBody(btCollisionObject& body);
    57 
    58         void    resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
    59 
    60         void    resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
    61        
    62         void    resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
    63        
    64         void    resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
    65                
    6653public:
    6754
    6855       
    6956        btSequentialImpulseConstraintSolver();
     57
     58        ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
     59        ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
     60        void    setContactSolverFunc(ContactSolverFunc func,int type0,int type1)
     61        {
     62                m_contactDispatch[type0][type1] = func;
     63        }
     64       
     65        ///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
     66        ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
     67        void    SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1)
     68        {
     69                m_frictionDispatch[type0][type1] = func;
     70        }
     71
    7072        virtual ~btSequentialImpulseConstraintSolver();
     73       
     74        virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
    7175
    72         virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
    73        
     76        virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
     77        btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
    7478        btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
    75         btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
     79
    7680
    7781        ///clear internal cached data and reset random seed
    7882        virtual void    reset();
     83
     84        btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
     85
     86
    7987       
    8088        unsigned long btRand2();

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

    r2907 r2908  
    6969btSliderConstraint::btSliderConstraint()
    7070        :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
    71                 m_useLinearReferenceFrameA(true),
    72                 m_useSolveConstraintObsolete(false)
    73 //              m_useSolveConstraintObsolete(true)
     71                m_useLinearReferenceFrameA(true)
    7472{
    7573        initParams();
    … …  
    8280        , m_frameInA(frameInA)
    8381        , m_frameInB(frameInB),
    84                 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
    85                 m_useSolveConstraintObsolete(false)
    86 //              m_useSolveConstraintObsolete(true)
     82                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
    8783{
    8884        initParams();
    … …  
    9389void btSliderConstraint::buildJacobian()
    9490{
    95         if (!m_useSolveConstraintObsolete)
    96         {
    97                 return;
    98         }
    9991        if(m_useLinearReferenceFrameA)
    10092        {
    … …  
    164156//-----------------------------------------------------------------------------
    165157
    166 void btSliderConstraint::getInfo1(btConstraintInfo1* info)
    167 {
    168         if (m_useSolveConstraintObsolete)
    169         {
    170                 info->m_numConstraintRows = 0;
    171                 info->nub = 0;
     158void btSliderConstraint::solveConstraint(btScalar timeStep)
     159{
     160    m_timeStep = timeStep;
     161        if(m_useLinearReferenceFrameA)
     162        {
     163                solveConstraintInt(m_rbA, m_rbB);
    172164        }
    173165        else
    174166        {
    175                 info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
    176                 info->nub = 2;
    177                 //prepare constraint
    178                 calculateTransforms();
    179                 testLinLimits();
    180                 if(getSolveLinLimit() || getPoweredLinMotor())
    181                 {
    182                         info->m_numConstraintRows++; // limit 3rd linear as well
    183                         info->nub--;
    184                 }
    185                 testAngLimits();
    186                 if(getSolveAngLimit() || getPoweredAngMotor())
    187                 {
    188                         info->m_numConstraintRows++; // limit 3rd angular as well
    189                         info->nub--;
    190                 }
    191         }
    192 } // btSliderConstraint::getInfo1()
    193 
    194 //-----------------------------------------------------------------------------
    195 
    196 void btSliderConstraint::getInfo2(btConstraintInfo2* info)
    197 {
    198         btAssert(!m_useSolveConstraintObsolete);
    199         int i, s = info->rowskip;
    200         const btTransform& trA = getCalculatedTransformA();
    201         const btTransform& trB = getCalculatedTransformB();
    202         btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
    203         // make rotations around Y and Z equal
    204         // the slider axis should be the only unconstrained
    205         // rotational axis, the angular velocity of the two bodies perpendicular to
    206         // the slider axis should be equal. thus the constraint equations are
    207         //    p*w1 - p*w2 = 0
    208         //    q*w1 - q*w2 = 0
    209         // where p and q are unit vectors normal to the slider axis, and w1 and w2
    210         // are the angular velocity vectors of the two bodies.
    211         // get slider axis (X)
    212         btVector3 ax1 = trA.getBasis().getColumn(0);
    213         // get 2 orthos to slider axis (Y, Z)
    214         btVector3 p = trA.getBasis().getColumn(1);
    215         btVector3 q = trA.getBasis().getColumn(2);
    216         // set the two slider rows
    217         info->m_J1angularAxis[0] = p[0];
    218         info->m_J1angularAxis[1] = p[1];
    219         info->m_J1angularAxis[2] = p[2];
    220         info->m_J1angularAxis[s+0] = q[0];
    221         info->m_J1angularAxis[s+1] = q[1];
    222         info->m_J1angularAxis[s+2] = q[2];
    223 
    224         info->m_J2angularAxis[0] = -p[0];
    225         info->m_J2angularAxis[1] = -p[1];
    226         info->m_J2angularAxis[2] = -p[2];
    227         info->m_J2angularAxis[s+0] = -q[0];
    228         info->m_J2angularAxis[s+1] = -q[1];
    229         info->m_J2angularAxis[s+2] = -q[2];
    230         // compute the right hand side of the constraint equation. set relative
    231         // body velocities along p and q to bring the slider back into alignment.
    232         // if ax1,ax2 are the unit length slider axes as computed from body1 and
    233         // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
    234         // if "theta" is the angle between ax1 and ax2, we need an angular velocity
    235         // along u to cover angle erp*theta in one step :
    236         //   |angular_velocity| = angle/time = erp*theta / stepsize
    237         //                      = (erp*fps) * theta
    238         //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
    239         //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
    240         // ...as ax1 and ax2 are unit length. if theta is smallish,
    241         // theta ~= sin(theta), so
    242         //    angular_velocity  = (erp*fps) * (ax1 x ax2)
    243         // ax1 x ax2 is in the plane space of ax1, so we project the angular
    244         // velocity to p and q to find the right hand side.
    245         btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
    246     btVector3 ax2 = trB.getBasis().getColumn(0);
    247         btVector3 u = ax1.cross(ax2);
    248         info->m_constraintError[0] = k * u.dot(p);
    249         info->m_constraintError[s] = k * u.dot(q);
    250         // pull out pos and R for both bodies. also get the connection
    251         // vector c = pos2-pos1.
    252         // next two rows. we want: vel2 = vel1 + w1 x c ... but this would
    253         // result in three equations, so we project along the planespace vectors
    254         // so that sliding along the slider axis is disregarded. for symmetry we
    255         // also consider rotation around center of mass of two bodies (factA and factB).
    256         btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
    257         btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
    258         int s2 = 2 * s, s3 = 3 * s;
    259         btVector3 c;
    260         btScalar miA = m_rbA.getInvMass();
    261         btScalar miB = m_rbB.getInvMass();
    262         btScalar miS = miA + miB;
    263         btScalar factA, factB;
    264         if(miS > btScalar(0.f))
    265         {
    266                 factA = miB / miS;
    267         }
    268         else
    269         {
    270                 factA = btScalar(0.5f);
    271         }
    272         if(factA > 0.99f) factA = 0.99f;
    273         if(factA < 0.01f) factA = 0.01f;
    274         factB = btScalar(1.0f) - factA;
    275         c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
    276         btVector3 tmp = c.cross(p);
    277         for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
    278         for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
    279         tmp = c.cross(q);
    280         for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
    281         for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
    282 
    283         for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
    284         for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
    285         // compute two elements of right hand side. we want to align the offset
    286         // point (in body 2's frame) with the center of body 1.
    287         btVector3 ofs; // offset point in global coordinates
    288         ofs = trB.getOrigin() - trA.getOrigin();
    289         k = info->fps * info->erp * getSoftnessOrthoLin();
    290         info->m_constraintError[s2] = k * p.dot(ofs);
    291         info->m_constraintError[s3] = k * q.dot(ofs);
    292         int nrow = 3; // last filled row
    293         int srow;
    294         // check linear limits linear
    295         btScalar limit_err = btScalar(0.0);
    296         int limit = 0;
    297         if(getSolveLinLimit())
    298         {
    299                 limit_err = getLinDepth() *  signFact;
    300                 limit = (limit_err > btScalar(0.0)) ? 2 : 1;
    301         }
    302         int powered = 0;
    303         if(getPoweredLinMotor())
    304         {
    305                 powered = 1;
    306         }
    307         // if the slider has joint limits or motor, add in the extra row
    308         if (limit || powered)
    309         {
    310                 nrow++;
    311                 srow = nrow * info->rowskip;
    312                 info->m_J1linearAxis[srow+0] = ax1[0];
    313                 info->m_J1linearAxis[srow+1] = ax1[1];
    314                 info->m_J1linearAxis[srow+2] = ax1[2];
    315                 // linear torque decoupling step:
    316                 //
    317                 // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
    318                 // do not create a torque couple. in other words, the points that the
    319                 // constraint force is applied at must lie along the same ax1 axis.
    320                 // a torque couple will result in limited slider-jointed free
    321                 // bodies from gaining angular momentum.
    322                 // the solution used here is to apply the constraint forces at the center of mass of the two bodies
    323                 btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
    324 //              c = btScalar(0.5) * c;
    325                 ltd = c.cross(ax1);
    326                 info->m_J1angularAxis[srow+0] = factA*ltd[0];
    327                 info->m_J1angularAxis[srow+1] = factA*ltd[1];
    328                 info->m_J1angularAxis[srow+2] = factA*ltd[2];
    329                 info->m_J2angularAxis[srow+0] = factB*ltd[0];
    330                 info->m_J2angularAxis[srow+1] = factB*ltd[1];
    331                 info->m_J2angularAxis[srow+2] = factB*ltd[2];
    332                 // right-hand part
    333                 btScalar lostop = getLowerLinLimit();
    334                 btScalar histop = getUpperLinLimit();
    335                 if(limit && (lostop == histop))
    336                 {  // the joint motor is ineffective
    337                         powered = 0;
    338                 }
    339                 info->m_constraintError[srow] = 0.;
    340                 info->m_lowerLimit[srow] = 0.;
    341                 info->m_upperLimit[srow] = 0.;
    342                 if(powered)
    343                 {
    344             info->cfm[nrow] = btScalar(0.0);
    345                         btScalar tag_vel = getTargetLinMotorVelocity();
    346                         btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
    347 //                      info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
    348                         info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
    349                         info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
    350                         info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
    351                 }
    352                 if(limit)
    353                 {
    354                         k = info->fps * info->erp;
    355                         info->m_constraintError[srow] += k * limit_err;
    356                         info->cfm[srow] = btScalar(0.0); // stop_cfm;
    357                         if(lostop == histop)
    358                         {       // limited low and high simultaneously
    359                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    360                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    361                         }
    362                         else if(limit == 1)
    363                         { // low limit
    364                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    365                                 info->m_upperLimit[srow] = 0;
    366                         }
    367                         else
    368                         { // high limit
    369                                 info->m_lowerLimit[srow] = 0;
    370                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    371                         }
    372                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
    373                         btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
    374                         if(bounce > btScalar(0.0))
    375                         {
    376                                 btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
    377                                 vel -= m_rbB.getLinearVelocity().dot(ax1);
    378                                 vel *= signFact;
    379                                 // only apply bounce if the velocity is incoming, and if the
    380                                 // resulting c[] exceeds what we already have.
    381                                 if(limit == 1)
    382                                 {       // low limit
    383                                         if(vel < 0)
    384                                         {
    385                                                 btScalar newc = -bounce * vel;
    386                                                 if (newc > info->m_constraintError[srow])
    387                                                 {
    388                                                         info->m_constraintError[srow] = newc;
    389                                                 }
    390                                         }
    391                                 }
    392                                 else
    393                                 { // high limit - all those computations are reversed
    394                                         if(vel > 0)
    395                                         {
    396                                                 btScalar newc = -bounce * vel;
    397                                                 if(newc < info->m_constraintError[srow])
    398                                                 {
    399                                                         info->m_constraintError[srow] = newc;
    400                                                 }
    401                                         }
    402                                 }
    403                         }
    404                         info->m_constraintError[srow] *= getSoftnessLimLin();
    405                 } // if(limit)
    406         } // if linear limit
    407         // check angular limits
    408         limit_err = btScalar(0.0);
    409         limit = 0;
    410         if(getSolveAngLimit())
    411         {
    412                 limit_err = getAngDepth();
    413                 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
    414         }
    415         // if the slider has joint limits, add in the extra row
    416         powered = 0;
    417         if(getPoweredAngMotor())
    418         {
    419                 powered = 1;
    420         }
    421         if(limit || powered)
    422         {
    423                 nrow++;
    424                 srow = nrow * info->rowskip;
    425                 info->m_J1angularAxis[srow+0] = ax1[0];
    426                 info->m_J1angularAxis[srow+1] = ax1[1];
    427                 info->m_J1angularAxis[srow+2] = ax1[2];
    428 
    429                 info->m_J2angularAxis[srow+0] = -ax1[0];
    430                 info->m_J2angularAxis[srow+1] = -ax1[1];
    431                 info->m_J2angularAxis[srow+2] = -ax1[2];
    432 
    433                 btScalar lostop = getLowerAngLimit();
    434                 btScalar histop = getUpperAngLimit();
    435                 if(limit && (lostop == histop))
    436                 {  // the joint motor is ineffective
    437                         powered = 0;
    438                 }
    439                 if(powered)
    440                 {
    441             info->cfm[srow] = btScalar(0.0);
    442                         btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
    443                         info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
    444                         info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
    445                         info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
    446                 }
    447                 if(limit)
    448                 {
    449                         k = info->fps * info->erp;
    450                         info->m_constraintError[srow] += k * limit_err;
    451                         info->cfm[srow] = btScalar(0.0); // stop_cfm;
    452                         if(lostop == histop)
    453                         {
    454                                 // limited low and high simultaneously
    455                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    456                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    457                         }
    458                         else if(limit == 1)
    459                         { // low limit
    460                                 info->m_lowerLimit[srow] = 0;
    461                                 info->m_upperLimit[srow] = SIMD_INFINITY;
    462                         }
    463                         else
    464                         { // high limit
    465                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
    466                                 info->m_upperLimit[srow] = 0;
    467                         }
    468                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
    469                         btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
    470                         if(bounce > btScalar(0.0))
    471                         {
    472                                 btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
    473                                 vel -= m_rbB.getAngularVelocity().dot(ax1);
    474                                 // only apply bounce if the velocity is incoming, and if the
    475                                 // resulting c[] exceeds what we already have.
    476                                 if(limit == 1)
    477                                 {       // low limit
    478                                         if(vel < 0)
    479                                         {
    480                                                 btScalar newc = -bounce * vel;
    481                                                 if(newc > info->m_constraintError[srow])
    482                                                 {
    483                                                         info->m_constraintError[srow] = newc;
    484                                                 }
    485                                         }
    486                                 }
    487                                 else
    488                                 {       // high limit - all those computations are reversed
    489                                         if(vel > 0)
    490                                         {
    491                                                 btScalar newc = -bounce * vel;
    492                                                 if(newc < info->m_constraintError[srow])
    493                                                 {
    494                                                         info->m_constraintError[srow] = newc;
    495                                                 }
    496                                         }
    497                                 }
    498                         }
    499                         info->m_constraintError[srow] *= getSoftnessLimAng();
    500                 } // if(limit)
    501         } // if angular limit or powered
    502 } // btSliderConstraint::getInfo2()
    503 
    504 //-----------------------------------------------------------------------------
    505 
    506 void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
    507 {
    508         if (m_useSolveConstraintObsolete)
    509         {
    510                 m_timeStep = timeStep;
    511                 if(m_useLinearReferenceFrameA)
    512                 {
    513                         solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
    514                 }
    515                 else
    516                 {
    517                         solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
    518                 }
     167                solveConstraintInt(m_rbB, m_rbA);
    519168        }
    520169} // btSliderConstraint::solveConstraint()
    … …  
    522171//-----------------------------------------------------------------------------
    523172
    524 void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
     173void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
    525174{
    526175    int i;
    527176    // linear
    528     btVector3 velA;
    529         bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
    530     btVector3 velB;
    531         bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
     177    btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
     178    btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
    532179    btVector3 vel = velA - velB;
    533180        for(i = 0; i < 3; i++)
    … …  
    544191                btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
    545192                btVector3 impulse_vector = normal * normalImpulse;
    546                
    547                 //rbA.applyImpulse( impulse_vector, m_relPosA);
    548                 //rbB.applyImpulse(-impulse_vector, m_relPosB);
    549                 {
    550                         btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
    551                         btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
    552                         bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
    553                         bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
    554                 }
    555 
    556 
    557 
     193                rbA.applyImpulse( impulse_vector, m_relPosA);
     194                rbB.applyImpulse(-impulse_vector, m_relPosB);
    558195                if(m_poweredLinMotor && (!i))
    559196                { // apply linear motor
    … …  
    581218                                // apply clamped impulse
    582219                                impulse_vector = normal * normalImpulse;
    583                                 //rbA.applyImpulse( impulse_vector, m_relPosA);
    584                                 //rbB.applyImpulse(-impulse_vector, m_relPosB);
    585 
    586                                 {
    587                                         btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
    588                                         btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
    589                                         bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
    590                                         bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
    591                                 }
    592 
    593 
    594 
     220                                rbA.applyImpulse( impulse_vector, m_relPosA);
     221                                rbB.applyImpulse(-impulse_vector, m_relPosB);
    595222                        }
    596223                }
    … …  
    601228        btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
    602229
    603         btVector3 angVelA;
    604         bodyA.getAngularVelocity(angVelA);
    605         btVector3 angVelB;
    606         bodyB.getAngularVelocity(angVelB);
     230        const btVector3& angVelA = rbA.getAngularVelocity();
     231        const btVector3& angVelB = rbB.getAngularVelocity();
    607232
    608233        btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
    … …  
    614239        //solve orthogonal angular velocity correction
    615240        btScalar len = velrelOrthog.length();
    616         btScalar orthorImpulseMag = 0.f;
    617 
    618241        if (len > btScalar(0.00001))
    619242        {
    620243                btVector3 normal = velrelOrthog.normalized();
    621244                btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
    622                 //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
    623                 orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
     245                velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
    624246        }
    625247        //solve angular positional correction
    626248        btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
    627         btVector3 angularAxis = angularError;
    628         btScalar angularImpulseMag = 0;
    629 
    630249        btScalar len2 = angularError.length();
    631250        if (len2>btScalar(0.00001))
    … …  
    633252                btVector3 normal2 = angularError.normalized();
    634253                btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
    635                 angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
    636                 angularError *= angularImpulseMag;
     254                angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
    637255        }
    638256        // apply impulse
    639         //rbA.applyTorqueImpulse(-velrelOrthog+angularError);
    640         //rbB.applyTorqueImpulse(velrelOrthog-angularError);
    641 
    642         bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag);
    643         bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag);
    644         bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag);
    645         bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag);
    646 
    647 
     257        rbA.applyTorqueImpulse(-velrelOrthog+angularError);
     258        rbB.applyTorqueImpulse(velrelOrthog-angularError);
    648259        btScalar impulseMag;
    649260        //solve angular limits
    … …  
    659270        }
    660271        btVector3 impulse = axisA * impulseMag;
    661         //rbA.applyTorqueImpulse(impulse);
    662         //rbB.applyTorqueImpulse(-impulse);
    663 
    664         bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag);
    665         bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag);
    666 
    667 
    668 
     272        rbA.applyTorqueImpulse(impulse);
     273        rbB.applyTorqueImpulse(-impulse);
    669274        //apply angular motor
    670275        if(m_poweredAngMotor)
    … …  
    697302                        // apply clamped impulse
    698303                        btVector3 motorImp = angImpulse * axisA;
    699                         //rbA.applyTorqueImpulse(motorImp);
    700                         //rbB.applyTorqueImpulse(-motorImp);
    701 
    702                         bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse);
    703                         bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
     304                        m_rbA.applyTorqueImpulse(motorImp);
     305                        m_rbB.applyTorqueImpulse(-motorImp);
    704306                }
    705307        }
    … …  
    711313
    712314void btSliderConstraint::calculateTransforms(void){
    713         if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
     315        if(m_useLinearReferenceFrameA)
    714316        {
    715317                m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
    … …  
    724326        m_realPivotBInW = m_calculatedTransformB.getOrigin();
    725327        m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
    726         if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
    727         {
    728                 m_delta = m_realPivotBInW - m_realPivotAInW;
    729         }
    730         else
    731         {
    732                 m_delta = m_realPivotAInW - m_realPivotBInW;
    733         }
     328        m_delta = m_realPivotBInW - m_realPivotAInW;
    734329        m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
    735330    btVector3 normalWorld;
    … …  
    773368
    774369//-----------------------------------------------------------------------------
     370 
    775371
    776372void btSliderConstraint::testAngLimits(void)
    … …  
    784380                const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
    785381                btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); 
    786                 m_angPos = rot;
    787382                if(rot < m_lowerAngLimit)
    788383                {
    … …  
    797392        }
    798393} // btSliderConstraint::testAngLimits()
     394
    799395       
    800396//-----------------------------------------------------------------------------
     397
     398
    801399
    802400btVector3 btSliderConstraint::getAncorInA(void)

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h

    r2907 r2908  
    4747{
    4848protected:
    49         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
    50         bool            m_useSolveConstraintObsolete;
    5149        btTransform     m_frameInA;
    5250    btTransform m_frameInB;
    … …  
    107105
    108106        btScalar m_linPos;
    109         btScalar m_angPos;
    110107
    111108        btScalar m_angDepth;
    … …  
    130127        // overrides
    131128    virtual void        buildJacobian();
    132     virtual void getInfo1 (btConstraintInfo1* info);
    133        
    134         virtual void getInfo2 (btConstraintInfo2* info);
    135 
    136     virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
    137        
    138 
     129    virtual     void    solveConstraint(btScalar        timeStep);
    139130        // access
    140131    const btRigidBody& getRigidBodyA() const { return m_rbA; }
    … …  
    204195        btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
    205196        btScalar getLinearPos() { return m_linPos; }
    206        
    207197
    208198        // access for ODE solver
    … …  
    213203        // internal
    214204    void        buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
    215     void        solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
     205    void        solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB);
    216206        // shared code used by ODE solver
    217207        void    calculateTransforms(void);
    218208        void    testLinLimits(void);
    219         void    testLinLimits2(btConstraintInfo2* info);
    220209        void    testAngLimits(void);
    221210        // access for PE Solver

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h

    r2907 r2908  
    2424#include "LinearMath/btTransformUtil.h"
    2525
    26 ///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
    27 #ifdef BT_USE_SSE
    28 #define USE_SIMD 1
    29 #endif //
    30 
    31 
    32 #ifdef USE_SIMD
    33 
    34 struct  btSimdScalar
    35 {
    36         SIMD_FORCE_INLINE       btSimdScalar()
    37         {
    38 
    39         }
    40 
    41         SIMD_FORCE_INLINE       btSimdScalar(float      fl)
    42         :m_vec128 (_mm_set1_ps(fl))
    43         {
    44         }
    45 
    46         SIMD_FORCE_INLINE       btSimdScalar(__m128 v128)
    47                 :m_vec128(v128)
    48         {
    49         }
    50         union
    51         {
    52                 __m128          m_vec128;
    53                 float           m_floats[4];
    54                 int                     m_ints[4];
    55                 btScalar        m_unusedPadding;
    56         };
    57         SIMD_FORCE_INLINE       __m128  get128()
    58         {
    59                 return m_vec128;
    60         }
    61 
    62         SIMD_FORCE_INLINE       const __m128    get128() const
    63         {
    64                 return m_vec128;
    65         }
    66 
    67         SIMD_FORCE_INLINE       void    set128(__m128 v128)
    68         {
    69                 m_vec128 = v128;
    70         }
    71 
    72         SIMD_FORCE_INLINE       operator       __m128()       
    73         {
    74                 return m_vec128;
    75         }
    76         SIMD_FORCE_INLINE       operator const __m128() const
    77         {
    78                 return m_vec128;
    79         }
    80        
    81         SIMD_FORCE_INLINE       operator float() const
    82         {
    83                 return m_floats[0];
    84         }
    85 
    86 };
    87 
    88 ///@brief Return the elementwise product of two btSimdScalar
    89 SIMD_FORCE_INLINE btSimdScalar
    90 operator*(const btSimdScalar& v1, const btSimdScalar& v2)
    91 {
    92         return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
    93 }
    94 
    95 ///@brief Return the elementwise product of two btSimdScalar
    96 SIMD_FORCE_INLINE btSimdScalar
    97 operator+(const btSimdScalar& v1, const btSimdScalar& v2)
    98 {
    99         return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
    100 }
    101 
    102 
    103 #else
    104 #define btSimdScalar btScalar
    105 #endif
    10626
    10727///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
    … …  
    10929{
    11030        BT_DECLARE_ALIGNED_ALLOCATOR();
    111         btVector3               m_deltaLinearVelocity;
    112         btVector3               m_deltaAngularVelocity;
    113         btScalar                m_angularFactor;
    114         btScalar                m_invMass;
    115         btScalar                m_friction;
     31       
     32        btMatrix3x3             m_worldInvInertiaTensor;
     33
     34        btVector3               m_angularVelocity;
     35        btVector3               m_linearVelocity;
     36        btVector3               m_centerOfMassPosition;
     37        btVector3               m_pushVelocity;
     38        btVector3               m_turnVelocity;
     39
     40        float                   m_angularFactor;
     41        float                   m_invMass;
     42        float                   m_friction;
    11643        btRigidBody*    m_originalBody;
    117         btVector3               m_pushVelocity;
    118         //btVector3             m_turnVelocity;
    119 
    12044       
    121         SIMD_FORCE_INLINE void  getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
     45       
     46        SIMD_FORCE_INLINE void  getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
    12247        {
    123                 if (m_originalBody)
    124                         velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
    125                 else
    126                         velocity.setValue(0,0,0);
     48                velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
    12749        }
    12850
    129         SIMD_FORCE_INLINE void  getAngularVelocity(btVector3& angVel) const
     51        //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
     52        SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
    13053        {
    131                 if (m_originalBody)
    132                         angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
    133                 else
    134                         angVel.setValue(0,0,0);
     54                if (m_invMass)
     55                {
     56                        m_linearVelocity += linearComponent*impulseMagnitude;
     57                        m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
     58                }
    13559        }
    136 
    137 
    138         //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
    139         SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
     60       
     61        SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
    14062        {
    141                 //if (m_invMass)
     63                if (m_invMass)
    14264                {
    143                         m_deltaLinearVelocity += linearComponent*impulseMagnitude;
    144                         m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
     65                        m_pushVelocity += linearComponent*impulseMagnitude;
     66                        m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
    14567                }
    14668        }
    14769
    148        
    149 /*
    15070       
    15171        void    writebackVelocity()
    … …  
    15373                if (m_invMass)
    15474                {
    155                         m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
    156                         m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
     75                        m_originalBody->setLinearVelocity(m_linearVelocity);
     76                        m_originalBody->setAngularVelocity(m_angularVelocity);
    15777                       
    15878                        //m_originalBody->setCompanionId(-1);
    15979                }
    16080        }
    161         */
     81       
    16282
    163         void    writebackVelocity(btScalar timeStep=0)
     83        void    writebackVelocity(btScalar timeStep)
    16484        {
    16585                if (m_invMass)
    16686                {
    167                         m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
    168                         m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
     87                        m_originalBody->setLinearVelocity(m_linearVelocity);
     88                        m_originalBody->setAngularVelocity(m_angularVelocity);
     89                       
     90                        //correct the position/orientation based on push/turn recovery
     91                        btTransform newTransform;
     92                        btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
     93                        m_originalBody->setWorldTransform(newTransform);
     94                       
    16995                        //m_originalBody->setCompanionId(-1);
    17096                }
    17197        }
     98
     99        void    readVelocity()
     100        {
     101                if (m_invMass)
     102                {
     103                        m_linearVelocity = m_originalBody->getLinearVelocity();
     104                        m_angularVelocity = m_originalBody->getAngularVelocity();
     105                }
     106        }
     107
    172108       
    173109

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h

    r2907 r2908  
    2020#include "LinearMath/btVector3.h"
    2121#include "LinearMath/btMatrix3x3.h"
    22 #include "btJacobianEntry.h"
    2322
    2423//#define NO_FRICTION_TANGENTIALS 1
    25 #include "btSolverBody.h"
    26 
    2724
    2825///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
    … …  
    3128        BT_DECLARE_ALIGNED_ALLOCATOR();
    3229
    33         btVector3               m_relpos1CrossNormal;
    34         btVector3               m_contactNormal;
     30        btVector3       m_relpos1CrossNormal;
     31        btVector3       m_contactNormal;
    3532
    36         btVector3               m_relpos2CrossNormal;
    37         //btVector3             m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
     33        btVector3       m_relpos2CrossNormal;
     34        btVector3       m_angularComponentA;
    3835
    39         btVector3               m_angularComponentA;
    40         btVector3               m_angularComponentB;
     36        btVector3       m_angularComponentB;
     37
     38        mutable btScalar        m_appliedPushImpulse;
    4139       
    42         mutable btSimdScalar    m_appliedPushImpulse;
    43         mutable btSimdScalar    m_appliedImpulse;
    44        
     40        mutable btScalar        m_appliedImpulse;
     41        int                     m_solverBodyIdA;
     42        int                     m_solverBodyIdB;
    4543       
    4644        btScalar        m_friction;
     45        btScalar        m_restitution;
    4746        btScalar        m_jacDiagABInv;
    48         union
    49         {
    50                 int     m_numConsecutiveRowsPerKernel;
    51                 btScalar        m_unusedPadding0;
    52         };
     47        btScalar        m_penetration;
     48       
    5349
    54         union
    55         {
    56                 int                     m_frictionIndex;
    57                 btScalar        m_unusedPadding1;
    58         };
    59         union
    60         {
    61                 int                     m_solverBodyIdA;
    62                 btScalar        m_unusedPadding2;
    63         };
    64         union
    65         {
    66                 int                     m_solverBodyIdB;
    67                 btScalar        m_unusedPadding3;
    68         };
    6950       
    70         union
    71         {
    72                 void*           m_originalContactPoint;
    73                 btScalar        m_unusedPadding4;
    74         };
     51        int                     m_constraintType;
     52        int                     m_frictionIndex;
     53        void*           m_originalContactPoint;
     54        int                     m_unusedPadding[1];
    7555
    76         btScalar                m_rhs;
    77         btScalar                m_cfm;
    78         btScalar                m_lowerLimit;
    79         btScalar                m_upperLimit;
    8056
    8157        enum            btSolverConstraintType
    … …  
    8662};
    8763
    88 typedef btAlignedObjectArray<btSolverConstraint>        btConstraintArray;
     64
     65
     66
    8967
    9068

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

    r2907 r2908  
    2020static btRigidBody s_fixed(0, 0,0);
    2121
    22 #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
    23 
    2422btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
    2523:m_userConstraintType(-1),
    … …  
    2826m_rbA(s_fixed),
    2927m_rbB(s_fixed),
    30 m_appliedImpulse(btScalar(0.)),
    31 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
     28m_appliedImpulse(btScalar(0.))
    3229{
    3330        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    … …  
    3936m_rbA(rbA),
    4037m_rbB(s_fixed),
    41 m_appliedImpulse(btScalar(0.)),
    42 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
     38m_appliedImpulse(btScalar(0.))
    4339{
    4440                s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    … …  
    5349m_rbA(rbA),
    5450m_rbB(rbB),
    55 m_appliedImpulse(btScalar(0.)),
    56 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
     51m_appliedImpulse(btScalar(0.))
    5752{
    5853                s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    … …  
    6055}
    6156
    62 
    63 //-----------------------------------------------------------------------------
    64 
    65 btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
    66 {
    67         if(lowLim > uppLim)
    68         {
    69                 return btScalar(1.0f);
    70         }
    71         else if(lowLim == uppLim)
    72         {
    73                 return btScalar(0.0f);
    74         }
    75         btScalar lim_fact = btScalar(1.0f);
    76         btScalar delta_max = vel / timeFact;
    77         if(delta_max < btScalar(0.0f))
    78         {
    79                 if((pos >= lowLim) && (pos < (lowLim - delta_max)))
    80                 {
    81                         lim_fact = (lowLim - pos) / delta_max;
    82                 }
    83                 else if(pos  < lowLim)
    84                 {
    85                         lim_fact = btScalar(0.0f);
    86                 }
    87                 else
    88                 {
    89                         lim_fact = btScalar(1.0f);
    90                 }
    91         }
    92         else if(delta_max > btScalar(0.0f))
    93         {
    94                 if((pos <= uppLim) && (pos > (uppLim - delta_max)))
    95                 {
    96                         lim_fact = (uppLim - pos) / delta_max;
    97                 }
    98                 else if(pos  > uppLim)
    99                 {
    100                         lim_fact = btScalar(0.0f);
    101                 }
    102                 else
    103                 {
    104                         lim_fact = btScalar(1.0f);
    105                 }
    106         }
    107         else
    108         {
    109                         lim_fact = btScalar(0.0f);
    110         }
    111         return lim_fact;
    112 } // btTypedConstraint::getMotorFactor()
    113 
    114 

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h

    r2907 r2908  
    1919class btRigidBody;
    2020#include "LinearMath/btScalar.h"
    21 #include "btSolverConstraint.h"
    22 struct  btSolverBody;
    23 
    24 
    25 
    2621
    2722enum btTypedConstraintType
    … …  
    3126        CONETWIST_CONSTRAINT_TYPE,
    3227        D6_CONSTRAINT_TYPE,
     28        VEHICLE_CONSTRAINT_TYPE,
    3329        SLIDER_CONSTRAINT_TYPE
    3430};
    … …  
    5349        btRigidBody&    m_rbB;
    5450        btScalar        m_appliedImpulse;
    55         btScalar        m_dbgDrawSize;
    5651
    5752
    … …  
    6156        virtual ~btTypedConstraint() {};
    6257        btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
     58
    6359        btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
    64 
    65         struct btConstraintInfo1 {
    66                 int m_numConstraintRows,nub;
    67         };
    68 
    69         struct btConstraintInfo2 {
    70                 // integrator parameters: frames per second (1/stepsize), default error
    71                 // reduction parameter (0..1).
    72                 btScalar fps,erp;
    73 
    74                 // for the first and second body, pointers to two (linear and angular)
    75                 // n*3 jacobian sub matrices, stored by rows. these matrices will have
    76                 // been initialized to 0 on entry. if the second body is zero then the
    77                 // J2xx pointers may be 0.
    78                 btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
    79 
    80                 // elements to jump from one row to the next in J's
    81                 int rowskip;
    82 
    83                 // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
    84                 // "constraint force mixing" vector. c is set to zero on entry, cfm is
    85                 // set to a constant value (typically very small or zero) value on entry.
    86                 btScalar *m_constraintError,*cfm;
    87 
    88                 // lo and hi limits for variables (set to -/+ infinity on entry).
    89                 btScalar *m_lowerLimit,*m_upperLimit;
    90 
    91                 // findex vector for variables. see the LCP solver interface for a
    92                 // description of what this does. this is set to -1 on entry.
    93                 // note that the returned indexes are relative to the first index of
    94                 // the constraint.
    95                 int *findex;
    96         };
    97 
    9860
    9961        virtual void    buildJacobian() = 0;
    10062
    101         virtual void    setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
    102         {
    103         }
    104         virtual void getInfo1 (btConstraintInfo1* info)=0;
     63        virtual void    solveConstraint(btScalar        timeStep) = 0;
    10564
    106         virtual void getInfo2 (btConstraintInfo2* info)=0;
    107 
    108         virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep) = 0;
    109 
    110         btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
    111        
    11265        const btRigidBody& getRigidBodyA() const
    11366        {
    … …  
    14295                m_userConstraintId = uid;
    14396        }
    144 
     97       
    14598        int getUserConstraintId() const
    14699        {
    … …  
    162115                return m_constraintType;
    163116        }
    164        
    165         void setDbgDrawSize(btScalar dbgDrawSize)
    166         {
    167                 m_dbgDrawSize = dbgDrawSize;
    168         }
    169         btScalar getDbgDrawSize()
    170         {
    171                 return m_dbgDrawSize;
    172         }
    173        
     117
    174118};
    175119

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

    r2907 r2908  
    111111{
    112112        btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
    113         btAssert(dynamicsWorld);
     113        assert(dynamicsWorld);
    114114        dynamicsWorld->stepSimulation(timeStep);
    115115}
    … …  
    118118{
    119119        btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
    120         btAssert(dynamicsWorld);
    121         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
    122         btAssert(body);
     120        assert(dynamicsWorld);
     121        btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
     122        assert(body);
    123123
    124124        dynamicsWorld->addRigidBody(body);
    … …  
    128128{
    129129        btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
    130         btAssert(dynamicsWorld);
    131         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
    132         btAssert(body);
     130        assert(dynamicsWorld);
     131        btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
     132        assert(body);
    133133
    134134        dynamicsWorld->removeRigidBody(body);
    … …  
    143143        btVector3 localInertia(0,0,0);
    144144        btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
    145         btAssert(shape);
     145        assert(shape);
    146146        if (mass)
    147147        {
    … …  
    159159{
    160160        btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
    161         btAssert(body);
     161        assert(body);
    162162        btAlignedFree( body);
    163163}
    … …  
    263263{
    264264        btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
    265         btAssert(shape);
     265        assert(shape);
    266266        btAlignedFree(shape);
    267267}
    … …  
    269269{
    270270        btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
    271         btAssert(shape);
     271        assert(shape);
    272272        btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
    273273        shape->setLocalScaling(scaling);       

  • code/branches/questsystem5/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp

    r2907 r2908  
    9191
    9292        ///update vehicle simulation
    93         updateActions(timeStep);
     93        updateVehicles(timeStep);
     94
    9495
    9596        updateActivationState( timeStep );

  • code/branches/questsystem5/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

    r2907 r2908  
    3030#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
    3131#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
    32 #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
    33 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
    34 #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
    35 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
    36 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
    3732
    3833//for debug rendering
    … …  
    5247
    5348
    54 #include "BulletDynamics/Dynamics/btActionInterface.h"
     49
     50//vehicle
     51#include "BulletDynamics/Vehicle/btRaycastVehicle.h"
     52#include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
     53#include "BulletDynamics/Vehicle/btWheelInfo.h"
     54//character
     55#include "BulletDynamics/Character/btCharacterControllerInterface.h"
     56
     57#include "LinearMath/btIDebugDraw.h"
    5558#include "LinearMath/btQuickprof.h"
    5659#include "LinearMath/btMotionState.h"
    … …  
    111114                if (body)
    112115                {
     116                                btTransform predictedTrans;
    113117                                if (body->getActivationState() != ISLAND_SLEEPING)
    114118                                {
    … …  
    145149                }
    146150        }
    147         bool drawConstraints = false;
    148         if (getDebugDrawer())
    149         {
    150                 int mode = getDebugDrawer()->getDebugMode();
    151                 if(mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
    152                 {
    153                         drawConstraints = true;
    154                 }
    155         }
    156         if(drawConstraints)
    157         {
    158                 for(int i = getNumConstraints()-1; i>=0 ;i--)
    159                 {
    160                         btTypedConstraint* constraint = getConstraint(i);
    161                         debugDrawConstraint(constraint);
    162                 }
    163         }
    164 
    165151
    166152
    … …  
    205191                }
    206192       
    207                 if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
    208                 {
    209                         for (i=0;i<m_actions.size();i++)
    210                         {
    211                                 m_actions[i]->debugDraw(m_debugDrawer);
     193                for (  i=0;i<this->m_vehicles.size();i++)
     194                {
     195                        for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
     196                        {
     197                                btVector3 wheelColor(0,255,255);
     198                                if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
     199                                {
     200                                        wheelColor.setValue(0,0,255);
     201                                } else
     202                                {
     203                                        wheelColor.setValue(255,0,255);
     204                                }
     205               
     206                                btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
     207
     208                                btVector3 axle = btVector3(     
     209                                        m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
     210                                        m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
     211                                        m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
     212
     213
     214                                //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
     215                                //debug wheels (cylinders)
     216                                m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
     217                                m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
     218
    212219                        }
    213220                }
    … …  
    281288                }
    282289        }
    283 /*
     290
    284291        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
    285292        {
    … …  
    293300                }
    294301        }
    295         */
    296 
    297302
    298303}
    … …  
    335340        if (getDebugDrawer())
    336341        {
    337                 btIDebugDraw* debugDrawer = getDebugDrawer ();
    338                 gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
     342                gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
    339343        }
    340344        if (numSimulationSubSteps)
    … …  
    400404
    401405        ///update vehicle simulation
    402         updateActions(timeStep);
    403        
     406        updateVehicles(timeStep);
     407       
     408        updateCharacters(timeStep);
     409
    404410        updateActivationState( timeStep );
    405411
    … …  
    465471
    466472
    467 void    btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
    468 {
    469         BT_PROFILE("updateActions");
    470        
    471         for ( int i=0;i<m_actions.size();i++)
    472         {
    473                 m_actions[i]->updateAction( this, timeStep);
    474         }
    475 }
     473void    btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
     474{
     475        BT_PROFILE("updateVehicles");
     476       
     477        for ( int i=0;i<m_vehicles.size();i++)
     478        {
     479                btRaycastVehicle* vehicle = m_vehicles[i];
     480                vehicle->updateVehicle( timeStep);
     481        }
     482}
     483
     484void    btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep)
     485{
     486        BT_PROFILE("updateCharacters");
     487       
     488        for ( int i=0;i<m_characters.size();i++)
     489        {
     490                btCharacterControllerInterface* character = m_characters[i];
     491                character->preStep (this);
     492                character->playerStep (this,timeStep);
     493        }
     494}
     495
    476496       
    477497       
    … …  
    530550}
    531551
    532 void    btDiscreteDynamicsWorld::addAction(btActionInterface* action)
    533 {
    534         m_actions.push_back(action);
    535 }
    536 
    537 void    btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
    538 {
    539         m_actions.remove(action);
    540 }
    541 
    542 
    543 void    btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
    544 {
    545         addAction(vehicle);
    546 }
    547 
    548 void    btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
    549 {
    550         removeAction(vehicle);
    551 }
    552 
    553 void    btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
    554 {
    555         addAction(character);
    556 }
    557 
    558 void    btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
    559 {
    560         removeAction(character);
     552void    btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
     553{
     554        m_vehicles.push_back(vehicle);
     555}
     556
     557void    btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
     558{
     559        m_vehicles.remove(vehicle);
     560}
     561
     562void    btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character)
     563{
     564        m_characters.push_back(character);
     565}
     566
     567void    btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character)
     568{
     569        m_characters.remove(character);
    561570}
    562571
    … …  
    634643                        if (islandId<0)
    635644                        {
    636                                 if (numManifolds + m_numConstraints)
    637                                 {
    638                                         ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
    639                                         m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
    640                                 }
     645                                ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
     646                                m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
    641647                        } else
    642648                        {
    … …  
    684690        }
    685691
    686 //      btAssert(0);
     692//      assert(0);
    687693               
    688694       
    … …  
    748754        btScalar m_allowedPenetration;
    749755        btOverlappingPairCache* m_pairCache;
    750         btDispatcher* m_dispatcher;
    751756
    752757
    753758public:
    754         btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
     759        btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) :
    755760          btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
    756761                m_allowedPenetration(0.0f),
    757762                m_me(me),
    758                 m_pairCache(pairCache),
    759                 m_dispatcher(dispatcher)
     763                m_pairCache(pairCache)
    760764        {
    761765        }
    … …  
    792796                        return false;
    793797
    794                 btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
    795 
    796                 //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
    797                 if (m_dispatcher->needsResponse(m_me,otherObj))
    798                 {
    799                         ///don't do CCD when there are already contact points (touching contact/penetration)
    800                         btAlignedObjectArray<btPersistentManifold*> manifoldArray;
    801                         btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
    802                         if (collisionPair)
    803                         {
    804                                 if (collisionPair->m_algorithm)
    805                                 {
    806                                         manifoldArray.resize(0);
    807                                         collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
    808                                         for (int j=0;j<manifoldArray.size();j++)
    809                                         {
    810                                                 btPersistentManifold* manifold = manifoldArray[j];
    811                                                 if (manifold->getNumContacts()>0)
    812                                                         return false;
    813                                         }
     798                ///don't do CCD when there are already contact points (touching contact/penetration)
     799                btAlignedObjectArray<btPersistentManifold*> manifoldArray;
     800                btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
     801                if (collisionPair)
     802                {
     803                        if (collisionPair->m_algorithm)
     804                        {
     805                                manifoldArray.resize(0);
     806                                collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
     807                                for (int j=0;j<manifoldArray.size();j++)
     808                                {
     809                                        btPersistentManifold* manifold = manifoldArray[j];
     810                                        if (manifold->getNumContacts()>0)
     811                                                return false;
    814812                                }
    815813                        }
    … …  
    849847                                                gNumClampedCcdMotions++;
    850848                                               
    851                                                 btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
     849                                                btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache());
    852850                                                btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
    853851                                                btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
    … …  
    11901188
    11911189
    1192 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
    1193 {
    1194         bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
    1195         bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
    1196         btScalar dbgDrawSize = constraint->getDbgDrawSize();
    1197         if(dbgDrawSize <= btScalar(0.f))
    1198         {
    1199                 return;
    1200         }
    1201 
    1202         switch(constraint->getConstraintType())
    1203         {
    1204                 case POINT2POINT_CONSTRAINT_TYPE:
    1205                         {
    1206                                 btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
    1207                                 btTransform tr;
    1208                                 tr.setIdentity();
    1209                                 btVector3 pivot = p2pC->getPivotInA();
    1210                                 pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
    1211                                 tr.setOrigin(pivot);
    1212                                 getDebugDrawer()->drawTransform(tr, dbgDrawSize);
    1213                                 // that ideally should draw the same frame     
    1214                                 pivot = p2pC->getPivotInB();
    1215                                 pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
    1216                                 tr.setOrigin(pivot);
    1217                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
    1218                         }
    1219                         break;
    1220                 case HINGE_CONSTRAINT_TYPE:
    1221                         {
    1222                                 btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
    1223                                 btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
    1224                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
    1225                                 tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
    1226                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
    1227                                 btScalar minAng = pHinge->getLowerLimit();
    1228                                 btScalar maxAng = pHinge->getUpperLimit();
    1229                                 if(minAng == maxAng)
    1230                                 {
    1231                                         break;
    1232                                 }
    1233                                 bool drawSect = true;
    1234                                 if(minAng > maxAng)
    1235                                 {
    1236                                         minAng = btScalar(0.f);
    1237                                         maxAng = SIMD_2_PI;
    1238                                         drawSect = false;
    1239                                 }
    1240                                 if(drawLimits)
    1241                                 {
    1242                                         btVector3& center = tr.getOrigin();
    1243                                         btVector3 normal = tr.getBasis().getColumn(2);
    1244                                         btVector3 axis = tr.getBasis().getColumn(0);
    1245                                         getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
    1246                                 }
    1247                         }
    1248                         break;
    1249                 case CONETWIST_CONSTRAINT_TYPE:
    1250                         {
    1251                                 btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
    1252                                 btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
    1253                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
    1254                                 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
    1255                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
    1256                                 if(drawLimits)
    1257                                 {
    1258                                         //const btScalar length = btScalar(5);
    1259                                         const btScalar length = dbgDrawSize;
    1260                                         static int nSegments = 8*4;
    1261                                         btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
    1262                                         btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
    1263                                         pPrev = tr * pPrev;
    1264                                         for (int i=0; i<nSegments; i++)
    1265                                         {
    1266                                                 fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
    1267                                                 btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
    1268                                                 pCur = tr * pCur;
    1269                                                 getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
    1270 
    1271                                                 if (i%(nSegments/8) == 0)
    1272                                                         getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
    1273 
    1274                                                 pPrev = pCur;
    1275                                         }                                               
    1276                                         btScalar tws = pCT->getTwistSpan();
    1277                                         btScalar twa = pCT->getTwistAngle();
    1278                                         bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
    1279                                         if(useFrameB)
    1280                                         {
    1281                                                 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
    1282                                         }
    1283                                         else
    1284                                         {
    1285                                                 tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
    1286                                         }
    1287                                         btVector3 pivot = tr.getOrigin();
    1288                                         btVector3 normal = tr.getBasis().getColumn(0);
    1289                                         btVector3 axis1 = tr.getBasis().getColumn(1);
    1290                                         getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
    1291 
    1292                                 }
    1293                         }
    1294                         break;
    1295                 case D6_CONSTRAINT_TYPE:
    1296                         {
    1297                                 btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
    1298                                 btTransform tr = p6DOF->getCalculatedTransformA();
    1299                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
    1300                                 tr = p6DOF->getCalculatedTransformB();
    1301                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
    1302                                 if(drawLimits)
    1303                                 {
    1304                                         tr = p6DOF->getCalculatedTransformA();
    1305                                         const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
    1306                                         btVector3 up = tr.getBasis().getColumn(2);
    1307                                         btVector3 axis = tr.getBasis().getColumn(0);
    1308                                         btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
    1309                                         btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
    1310                                         btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
    1311                                         btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
    1312                                         getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
    1313                                         axis = tr.getBasis().getColumn(1);
    1314                                         btScalar ay = p6DOF->getAngle(1);
    1315                                         btScalar az = p6DOF->getAngle(2);
    1316                                         btScalar cy = btCos(ay);
    1317                                         btScalar sy = btSin(ay);
    1318                                         btScalar cz = btCos(az);
    1319                                         btScalar sz = btSin(az);
    1320                                         btVector3 ref;
    1321                                         ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
    1322                                         ref[1] = -sz*axis[0] + cz*axis[1];
    1323                                         ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
    1324                                         tr = p6DOF->getCalculatedTransformB();
    1325                                         btVector3 normal = -tr.getBasis().getColumn(0);
    1326                                         btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
    1327                                         btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
    1328                                         if(minFi > maxFi)
    1329                                         {
    1330                                                 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
    1331                                         }
    1332                                         else if(minFi < maxFi)
    1333                                         {
    1334                                                 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
    1335                                         }
    1336                                         tr = p6DOF->getCalculatedTransformA();
    1337                                         btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
    1338                                         btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
    1339                                         getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
    1340                                 }
    1341                         }
    1342                         break;
    1343                 case SLIDER_CONSTRAINT_TYPE:
    1344                         {
    1345                                 btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
    1346                                 btTransform tr = pSlider->getCalculatedTransformA();
    1347                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
    1348                                 tr = pSlider->getCalculatedTransformB();
    1349                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
    1350                                 if(drawLimits)
    1351                                 {
    1352                                         btTransform tr = pSlider->getCalculatedTransformA();
    1353                                         btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
    1354                                         btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
    1355                                         getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
    1356                                         btVector3 normal = tr.getBasis().getColumn(0);
    1357                                         btVector3 axis = tr.getBasis().getColumn(1);
    1358                                         btScalar a_min = pSlider->getLowerAngLimit();
    1359                                         btScalar a_max = pSlider->getUpperAngLimit();
    1360                                         const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
    1361                                         getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
    1362                                 }
    1363                         }
    1364                         break;
    1365                 default :
    1366                         break;
    1367         }
    1368         return;
    1369 } // btDiscreteDynamicsWorld::debugDrawConstraint()
    1370 
    1371 
    1372 
    1373 
    1374 
    13751190void    btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
    13761191{
    … …  
    14011216        return m_constraints[index];
    14021217}
    1403 
    1404 

  • code/branches/questsystem5/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

    r2907 r2908  
    2424class btSimulationIslandManager;
    2525class btTypedConstraint;
    26 class btActionInterface;
    2726
     27
     28class btRaycastVehicle;
     29class btCharacterControllerInterface;
    2830class btIDebugDraw;
    2931#include "LinearMath/btAlignedObjectArray.h"
    … …  
    5153        bool    m_ownsConstraintSolver;
    5254
    53         btAlignedObjectArray<btActionInterface*>        m_actions;
    5455       
     56        btAlignedObjectArray<btRaycastVehicle*> m_vehicles;
     57       
     58        btAlignedObjectArray<btCharacterControllerInterface*> m_characters;
     59       
     60
    5561        int     m_profileTimings;
    5662
    … …  
    6571        void    updateActivationState(btScalar timeStep);
    6672
    67         void    updateActions(btScalar timeStep);
     73        void    updateVehicles(btScalar timeStep);
     74
     75        void    updateCharacters(btScalar timeStep);
    6876
    6977        void    startProfiling(btScalar timeStep);
    … …  
    98106        virtual void    removeConstraint(btTypedConstraint* constraint);
    99107
    100         virtual void    addAction(btActionInterface*);
     108        virtual void    addVehicle(btRaycastVehicle* vehicle);
    101109
    102         virtual void    removeAction(btActionInterface*);
     110        virtual void    removeVehicle(btRaycastVehicle* vehicle);
    103111       
     112        virtual void    addCharacter(btCharacterControllerInterface* character);
     113
     114        virtual void    removeCharacter(btCharacterControllerInterface* character);
     115               
     116
    104117        btSimulationIslandManager*      getSimulationIslandManager()
    105118        {
    … …  
    118131
    119132        virtual void    setGravity(const btVector3& gravity);
    120 
    121133        virtual btVector3 getGravity () const;
    122134
    … …  
    128140
    129141        void    debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
    130 
    131         void    debugDrawConstraint(btTypedConstraint* constraint);
    132142
    133143        virtual void    debugDrawWorld();
    … …  
    160170        }
    161171
    162         ///obsolete, use updateActions instead
    163         virtual void updateVehicles(btScalar timeStep)
    164         {
    165                 updateActions(timeStep);
    166         }
    167 
    168         ///obsolete, use addAction instead
    169         virtual void    addVehicle(btActionInterface* vehicle);
    170         ///obsolete, use removeAction instead
    171         virtual void    removeVehicle(btActionInterface* vehicle);
    172         ///obsolete, use addAction instead
    173         virtual void    addCharacter(btActionInterface* character);
    174         ///obsolete, use removeAction instead
    175         virtual void    removeCharacter(btActionInterface* character);
    176 
    177172};
    178173

  • code/branches/questsystem5/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h

    r2907 r2908  
    2121
    2222class btTypedConstraint;
    23 class btActionInterface;
     23class btRaycastVehicle;
    2424class btConstraintSolver;
    2525class btDynamicsWorld;
    26 
     26class btCharacterControllerInterface;
    2727
    2828/// Type for the callback for each tick
    … …  
    7373                virtual void    removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
    7474
    75                 virtual void    addAction(btActionInterface* action) = 0;
     75                virtual void    addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
    7676
    77                 virtual void    removeAction(btActionInterface* action) = 0;
     77                virtual void    removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
     78
     79                virtual void    addCharacter(btCharacterControllerInterface* character) {(void)character;}
     80
     81                virtual void    removeCharacter(btCharacterControllerInterface* character) {(void)character;}
     82
    7883
    7984                //once a rigidbody is added to the dynamics world, it will get this gravity assigned
    … …  
    125130
    126131
    127                 ///obsolete, use addAction instead.
    128                 virtual void    addVehicle(btActionInterface* vehicle) {(void)vehicle;}
    129                 ///obsolete, use removeAction instead
    130                 virtual void    removeVehicle(btActionInterface* vehicle) {(void)vehicle;}
    131                 ///obsolete, use addAction instead.
    132                 virtual void    addCharacter(btActionInterface* character) {(void)character;}
    133                 ///obsolete, use removeAction instead
    134                 virtual void    removeCharacter(btActionInterface* character) {(void)character;}
    135 
    136 
    137132};
    138133

  • code/branches/questsystem5/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp

    r2907 r2908  
    4747        m_angularFactor = btScalar(1.);
    4848        m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
    49         m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
    5049        m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
    5150        m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
    … …  
    126125                m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
    127126        }
    128         m_gravity_acceleration = acceleration;
    129127}
    130128

  • code/branches/questsystem5/src/bullet/BulletDynamics/Dynamics/btRigidBody.h

    r2907 r2908  
    4949
    5050        btVector3               m_gravity;     
    51         btVector3               m_gravity_acceleration;
    5251        btVector3               m_invInertiaLocal;
    5352        btVector3               m_totalForce;
    … …  
    183182        const btVector3&        getGravity() const
    184183        {
    185                 return m_gravity_acceleration;
     184                return m_gravity;
    186185        }
    187186
    … …  
    233232                m_totalForce += force;
    234233        }
    235 
    236         const btVector3& getTotalForce()
    237         {
    238                 return m_totalForce;
    239         };
    240 
    241         const btVector3& getTotalTorque()
    242         {
    243                 return m_totalTorque;
    244         };
    245234   
    246         const btVector3& getInvInertiaDiagLocal() const
     235        const btVector3& getInvInertiaDiagLocal()
    247236        {
    248237                return m_invInertiaLocal;

  • code/branches/questsystem5/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp

    r2907 r2908  
    2020#include "btWheelInfo.h"
    2121#include "LinearMath/btMinMax.h"
    22 #include "LinearMath/btIDebugDraw.h"
     22
     23
    2324#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
    2425
    … …  
    2627
    2728btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,  btVehicleRaycaster* raycaster )
    28 :m_vehicleRaycaster(raycaster),
     29: btTypedConstraint(VEHICLE_CONSTRAINT_TYPE),
     30m_vehicleRaycaster(raycaster),
    2931m_pitchControl(btScalar(0.))
    3032{
    … …  
    8688const btTransform&      btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
    8789{
    88         btAssert(wheelIndex < getNumWheels());
     90        assert(wheelIndex < getNumWheels());
    8991        const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
    9092        return wheel.m_worldTransform;
    … …  
    174176        btVehicleRaycaster::btVehicleRaycasterResult    rayResults;
    175177
    176         btAssert(m_vehicleRaycaster);
     178        assert(m_vehicleRaycaster);
    177179
    178180        void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
    … …  
    358360void    btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
    359361{
    360         btAssert(wheel>=0 && wheel < getNumWheels());
     362        assert(wheel>=0 && wheel < getNumWheels());
    361363
    362364        btWheelInfo& wheelInfo = getWheelInfo(wheel);
    … …  
    374376void    btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
    375377{
    376         btAssert(wheel>=0 && wheel < getNumWheels());
     378        assert(wheel>=0 && wheel < getNumWheels());
    377379        btWheelInfo& wheelInfo = getWheelInfo(wheel);
    378380        wheelInfo.m_engineForce = force;
    … …  
    703705
    704706
    705 
    706 void    btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
    707 {
    708 
    709         for (int v=0;v<this->getNumWheels();v++)
    710         {
    711                 btVector3 wheelColor(0,255,255);
    712                 if (getWheelInfo(v).m_raycastInfo.m_isInContact)
    713                 {
    714                         wheelColor.setValue(0,0,255);
    715                 } else
    716                 {
    717                         wheelColor.setValue(255,0,255);
    718                 }
    719 
    720                 btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
    721 
    722                 btVector3 axle = btVector3(     
    723                         getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
    724                         getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
    725                         getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);
    726 
    727                 //debug wheels (cylinders)
    728                 debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
    729                 debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
    730 
    731         }
    732 }
    733 
    734 
    735707void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
    736708{
    … …  
    756728        return 0;
    757729}
    758 

  • code/branches/questsystem5/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h

    r2907 r2908  
    1818#include "LinearMath/btAlignedObjectArray.h"
    1919#include "btWheelInfo.h"
    20 #include "BulletDynamics/Dynamics/btActionInterface.h"
    2120
    2221class btVehicleTuning;
    2322
    2423///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
    25 class btRaycastVehicle : public btActionInterface
     24class btRaycastVehicle : public btTypedConstraint
    2625{
    2726
    … …  
    7574        virtual ~btRaycastVehicle() ;
    7675
    77 
    78         ///btActionInterface interface
    79         virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
    80         {
    81                 updateVehicle(step);
    82         }
    83        
    84 
    85         ///btActionInterface interface
    86         void    debugDraw(btIDebugDraw* debugDrawer);
    87                        
     76               
    8877        const btTransform& getChassisWorldTransform() const;
    8978       
    … …  
    9180
    9281        virtual void updateVehicle(btScalar step);
    93        
    94        
     82
    9583        void resetSuspension();
    9684
    … …  
    188176        }
    189177
     178        virtual void    buildJacobian()
     179        {
     180                //not yet
     181        }
     182
     183        virtual void    solveConstraint(btScalar        timeStep)
     184        {
     185                (void)timeStep;
     186                //not yet
     187        }
    190188
    191189

  • code/branches/questsystem5/src/bullet/LinearMath/btAlignedAllocator.cpp

    r2907 r2908  
    2020int gTotalBytesAlignedAllocs = 0;//detect memory leaks
    2121
    22 static void *btAllocDefault(size_t size)
    23 {
    24         return malloc(size);
    25 }
    26 
    27 static void btFreeDefault(void *ptr)
    28 {
    29         free(ptr);
    30 }
    31 
    32 static btAllocFunc *sAllocFunc = btAllocDefault;
    33 static btFreeFunc *sFreeFunc = btFreeDefault;
    34 
    35 
    36 
    3722#if defined (BT_HAS_ALIGNED_ALLOCATOR)
    3823#include <malloc.h>
    … …  
    6550  unsigned long offset;
    6651
    67   real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1));
     52  real = (char *)malloc(size + sizeof(void *) + (alignment-1));
    6853  if (real) {
    6954    offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1);
    … …  
    8267  if (ptr) {
    8368    real = *((void **)(ptr)-1);
    84     sFreeFunc(real);
     69    free(real);
    8570  }
    8671}
    8772#endif
    8873
     74static void *btAllocDefault(size_t size)
     75{
     76        return malloc(size);
     77}
     78
     79static void btFreeDefault(void *ptr)
     80{
     81        free(ptr);
     82}
    8983
    9084static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault;
    9185static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault;
     86static btAllocFunc *sAllocFunc = btAllocDefault;
     87static btFreeFunc *sFreeFunc = btFreeDefault;
    9288
    9389void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc)

  • code/branches/questsystem5/src/bullet/LinearMath/btAlignedAllocator.h

    r2907 r2908  
    3939        void    btAlignedFreeInternal   (void* ptr);
    4040
    41         #define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment)
     41        #define btAlignedAlloc(a,b) btAlignedAllocInternal(a,b)
    4242        #define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
    4343
    … …  
    5050typedef void (btFreeFunc)(void *memblock);
    5151
    52 ///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom
     52void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);
    5353void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
    54 ///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
    55 void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);
    56 
    5754
    5855///The btAlignedAllocator is a portable class for aligned memory allocations.

  • code/branches/questsystem5/src/bullet/LinearMath/btAlignedObjectArray.h

    r2907 r2908  
    5959                        return (size ? size*2 : 1);
    6060                }
    61                 SIMD_FORCE_INLINE       void    copy(int start,int end, T* dest) const
     61                SIMD_FORCE_INLINE       void    copy(int start,int end, T* dest)
    6262                {
    6363                        int i;
    … …  
    121121                }
    122122
    123                 ///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
    124                 btAlignedObjectArray(const btAlignedObjectArray& otherArray)
    125                 {
    126                         init();
    127 
    128                         int otherSize = otherArray.size();
    129                         resize (otherSize);
    130                         otherArray.copy(0, otherSize, m_data);
    131                 }
    132 
     123                SIMD_FORCE_INLINE       int capacity() const
     124                {       // return current length of allocated storage
     125                        return m_capacity;
     126                }
    133127               
    134                
    135                 /// return the number of elements in the array
    136128                SIMD_FORCE_INLINE       int size() const
    137                 {       
     129                {       // return length of sequence
    138130                        return m_size;
    139131                }
    … …  
    150142               
    151143
    152                 ///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
    153144                SIMD_FORCE_INLINE       void    clear()
    154145                {
    … …  
    166157                }
    167158
    168                 ///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
    169                 ///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
    170159                SIMD_FORCE_INLINE       void    resize(int newsize, const T& fillData=T())
    171160                {
    … …  
    231220
    232221       
    233                 /// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
    234                 SIMD_FORCE_INLINE       int capacity() const
    235                 {       
    236                         return m_capacity;
    237                 }
    238222               
    239223                SIMD_FORCE_INLINE       void reserve(int _Count)

  • code/branches/questsystem5/src/bullet/LinearMath/btConvexHull.cpp

    r2907 r2908  
    263263                for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
    264264                {
    265                         btScalar s = btSin(SIMD_RADS_PER_DEG*(x));
    266                         btScalar c = btCos(SIMD_RADS_PER_DEG*(x));
     265                        btScalar s = sinf(SIMD_RADS_PER_DEG*(x));
     266                        btScalar c = cosf(SIMD_RADS_PER_DEG*(x));
    267267                        int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
    268268                        if(ma==m && mb==m)
    … …  
    276276                                for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
    277277                                {
    278                                         btScalar s = btSin(SIMD_RADS_PER_DEG*(xx));
    279                                         btScalar c = btCos(SIMD_RADS_PER_DEG*(xx));
     278                                        btScalar s = sinf(SIMD_RADS_PER_DEG*(xx));
     279                                        btScalar c = cosf(SIMD_RADS_PER_DEG*(xx));
    280280                                        int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
    281281                                        if(mc==m && md==m)

  • code/branches/questsystem5/src/bullet/LinearMath/btIDebugDraw.h

    r2907 r2908  
    3030
    3131#include "btVector3.h"
    32 #include "btTransform.h"
    3332
    3433
    … …  
    5453                DBG_DisableBulletLCP = 512,
    5554                DBG_EnableCCD = 1024,
    56                 DBG_DrawConstraints = (1 << 11),
    57                 DBG_DrawConstraintLimits = (1 << 12),
    5855                DBG_MAX_DEBUG_DRAW_MODE
    5956        };
    6057
    6158        virtual ~btIDebugDraw() {};
    62 
    63         virtual void    drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
    64         {
    65                 drawLine (from, to, fromColor);
    66         }
    67 
    68         virtual void    drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha)
    69         {
    70         }
    71 
    72         virtual void    drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
    73         {
    74         }
    7559
    7660        virtual void    drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
    … …  
    126110                }
    127111        }
    128         void drawTransform(const btTransform& transform, btScalar orthoLen)
    129         {
    130                 btVector3 start = transform.getOrigin();
    131                 drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0));
    132                 drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0));
    133                 drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f));
    134         }
    135 
    136         void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
    137                                 const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
    138         {
    139                 const btVector3& vx = axis;
    140                 btVector3 vy = normal.cross(axis);
    141                 btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
    142                 int nSteps = (int)((maxAngle - minAngle) / step);
    143                 if(!nSteps) nSteps = 1;
    144                 btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle);
    145                 if(drawSect)
    146                 {
    147                         drawLine(center, prev, color);
    148                 }
    149                 for(int i = 1; i <= nSteps; i++)
    150                 {
    151                         btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps);
    152                         btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle);
    153                         drawLine(prev, next, color);
    154                         prev = next;
    155                 }
    156                 if(drawSect)
    157                 {
    158                         drawLine(center, prev, color);
    159                 }
    160         }
    161         void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
    162                 btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f))
    163         {
    164                 btVector3 vA[74];
    165                 btVector3 vB[74];
    166                 btVector3 *pvA = vA, *pvB = vB, *pT;
    167                 btVector3 npole = center + up * radius;
    168                 btVector3 spole = center - up * radius;
    169                 btVector3 arcStart;
    170                 btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
    171                 const btVector3& kv = up;
    172                 const btVector3& iv = axis;
    173                 btVector3 jv = kv.cross(iv);
    174                 bool drawN = false;
    175                 bool drawS = false;
    176                 if(minTh <= -SIMD_HALF_PI)
    177                 {
    178                         minTh = -SIMD_HALF_PI + step;
    179                         drawN = true;
    180                 }
    181                 if(maxTh >= SIMD_HALF_PI)
    182                 {
    183                         maxTh = SIMD_HALF_PI - step;
    184                         drawS = true;
    185                 }
    186                 if(minTh > maxTh)
    187                 {
    188                         minTh = -SIMD_HALF_PI + step;
    189                         maxTh =  SIMD_HALF_PI - step;
    190                         drawN = drawS = true;
    191                 }
    192                 int n_hor = (int)((maxTh - minTh) / step) + 1;
    193                 if(n_hor < 2) n_hor = 2;
    194                 btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1);
    195                 bool isClosed = false;
    196                 if(minPs > maxPs)
    197                 {
    198                         minPs = -SIMD_PI + step;
    199                         maxPs =  SIMD_PI;
    200                         isClosed = true;
    201                 }
    202                 else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f))
    203                 {
    204                         isClosed = true;
    205                 }
    206                 else
    207                 {
    208                         isClosed = false;
    209                 }
    210                 int n_vert = (int)((maxPs - minPs) / step) + 1;
    211                 if(n_vert < 2) n_vert = 2;
    212                 btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1);
    213                 for(int i = 0; i < n_hor; i++)
    214                 {
    215                         btScalar th = minTh + btScalar(i) * step_h;
    216                         btScalar sth = radius * btSin(th);
    217                         btScalar cth = radius * btCos(th);
    218                         for(int j = 0; j < n_vert; j++)
    219                         {
    220                                 btScalar psi = minPs + btScalar(j) * step_v;
    221                                 btScalar sps = btSin(psi);
    222                                 btScalar cps = btCos(psi);
    223                                 pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv;
    224                                 if(i)
    225                                 {
    226                                         drawLine(pvA[j], pvB[j], color);
    227                                 }
    228                                 else if(drawS)
    229                                 {
    230                                         drawLine(spole, pvB[j], color);
    231                                 }
    232                                 if(j)
    233                                 {
    234                                         drawLine(pvB[j-1], pvB[j], color);
    235                                 }
    236                                 else
    237                                 {
    238                                         arcStart = pvB[j];
    239                                 }
    240                                 if((i == (n_hor - 1)) && drawN)
    241                                 {
    242                                         drawLine(npole, pvB[j], color);
    243                                 }
    244                                 if(isClosed)
    245                                 {
    246                                         if(j == (n_vert-1))
    247                                         {
    248                                                 drawLine(arcStart, pvB[j], color);
    249                                         }
    250                                 }
    251                                 else
    252                                 {
    253                                         if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1))))
    254                                         {
    255                                                 drawLine(center, pvB[j], color);
    256                                         }
    257                                 }
    258                         }
    259                         pT = pvA; pvA = pvB; pvB = pT;
    260                 }
    261         }
    262        
    263         void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color)
    264         {
    265                 drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
    266                 drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
    267                 drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
    268                 drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
    269                 drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
    270                 drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
    271                 drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
    272                 drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
    273                 drawLine(btVector3(bbMin[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
    274                 drawLine(btVector3(bbMax[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
    275                 drawLine(btVector3(bbMax[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
    276                 drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
    277         }
    278         void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color)
    279         {
    280                 drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
    281                 drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
    282                 drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
    283                 drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
    284                 drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
    285                 drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
    286                 drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
    287                 drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
    288                 drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
    289                 drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
    290                 drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
    291                 drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
    292         }
    293112};
    294113

  • code/branches/questsystem5/src/bullet/LinearMath/btMatrix3x3.h

    r2907 r2908  
    193193                                         btScalar(0.0), btScalar(0.0), btScalar(1.0));
    194194                }
    195 
    196                 static const btMatrix3x3&       getIdentity()
    197                 {
    198                         static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0),
    199                                          btScalar(0.0), btScalar(1.0), btScalar(0.0),
    200                                          btScalar(0.0), btScalar(0.0), btScalar(1.0));
    201                         return identityMatrix;
    202                 }
    203 
    204195  /**@brief Fill the values of the matrix into a 9 element array
    205196   * @param m The array to be filled */

  • code/branches/questsystem5/src/bullet/LinearMath/btQuadWord.h

    r2907 r2908  
    2525#endif
    2626
    27 /**@brief The btQuadWord class is base class for btVector3 and btQuaternion.
     27/**@brief The btQuadWordStorage class is base class for btVector3 and btQuaternion.
    2828 * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
    2929 */
    3030#ifndef USE_LIBSPE2
    31 ATTRIBUTE_ALIGNED16(class) btQuadWord
     31ATTRIBUTE_ALIGNED16(class) btQuadWordStorage
    3232#else
    33 class btQuadWord
     33class btQuadWordStorage
    3434#endif
    3535{
    … …  
    4646                return mVec128;
    4747        }
    48 protected:
    4948#else //__CELLOS_LV2__ __SPU__
    5049        btScalar        m_floats[4];
    5150#endif //__CELLOS_LV2__ __SPU__
    5251
     52};
     53
     54/** @brief The btQuadWord is base-class for vectors, points */
     55class   btQuadWord : public btQuadWordStorage
     56{
    5357        public:
    5458 
    … …  
    131135                {
    132136                }
    133  
     137  /**@brief Copy constructor */
     138                SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q)
     139                {
     140                        *((btQuadWordStorage*)this) = q;
     141                }
    134142  /**@brief Three argument constructor (zeros w)
    135143   * @param x Value of x

  • code/branches/questsystem5/src/bullet/LinearMath/btQuaternion.h

    r2907 r2908  
    2020
    2121#include "btVector3.h"
    22 #include "btQuadWord.h"
    2322
    2423/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
    … …  
    5958        {
    6059                btScalar d = axis.length();
    61                 btAssert(d != btScalar(0.0));
     60                assert(d != btScalar(0.0));
    6261                btScalar s = btSin(angle * btScalar(0.5)) / d;
    6362                setValue(axis.x() * s, axis.y() * s, axis.z() * s,
    … …  
    178177        btQuaternion operator/(const btScalar& s) const
    179178        {
    180                 btAssert(s != btScalar(0.0));
     179                assert(s != btScalar(0.0));
    181180                return *this * (btScalar(1.0) / s);
    182181        }
    … …  
    186185        btQuaternion& operator/=(const btScalar& s)
    187186        {
    188                 btAssert(s != btScalar(0.0));
     187                assert(s != btScalar(0.0));
    189188                return *this *= btScalar(1.0) / s;
    190189        }
    … …  
    200199        {
    201200                btScalar s = btSqrt(length2() * q.length2());
    202                 btAssert(s != btScalar(0.0));
     201                assert(s != btScalar(0.0));
    203202                return btAcos(dot(q) / s);
    204203        }
    … …  
    276275        }
    277276
    278         static const btQuaternion&      getIdentity()
    279         {
    280                 static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
    281                 return identityQuat;
    282         }
    283 
    284277        SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
    285278

  • code/branches/questsystem5/src/bullet/LinearMath/btScalar.h

    r2907 r2908  
    2626#include <float.h>
    2727
    28 #define BT_BULLET_VERSION 274
     28#define BT_BULLET_VERSION 273
    2929
    3030inline int      btGetVersion()
    … …  
    4646                        #define ATTRIBUTE_ALIGNED128(a) a
    4747                #else
    48                         //#define BT_HAS_ALIGNED_ALLOCATOR
     48                        #define BT_HAS_ALIGNED_ALLOCATOR
    4949                        #pragma warning(disable : 4324) // disable padding warning
    5050//                      #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
    … …  
    6262                        #define btFsel(a,b,c) __fsel((a),(b),(c))
    6363                #else
    64 
    65 #if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
    6664                        #define BT_USE_SSE
    67                         #include <emmintrin.h>
    68 #endif
    69 
    70                 #endif//_XBOX
    71 
     65                #endif
    7266                #endif //__MINGW32__
    7367
    … …  
    131125
    132126                #define SIMD_FORCE_INLINE inline
    133                 ///@todo: check out alignment methods for other platforms/compilers
    134                 ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
    135                 ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
    136127                #define ATTRIBUTE_ALIGNED16(a) a
    137128                #define ATTRIBUTE_ALIGNED128(a) a
    … …  
    242233  SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
    243234  #endif
    244        
     235
    245236#endif
    246237

  • code/branches/questsystem5/src/bullet/LinearMath/btTransform.h

    r2907 r2908  
    191191
    192192  /**@brief Return an identity transform */
    193         static const btTransform&       getIdentity()
    194         {
    195                 static const btTransform identityTransform(btMatrix3x3::getIdentity());
    196                 return identityTransform;
     193        static btTransform      getIdentity()
     194        {
     195                btTransform tr;
     196                tr.setIdentity();
     197                return tr;
    197198        }
    198199       

  • code/branches/questsystem5/src/bullet/LinearMath/btVector3.h

    r2907 r2908  
    1818#define SIMD__VECTOR3_H
    1919
    20 
    21 #include "btScalar.h"
    22 #include "btScalar.h"
    23 #include "btMinMax.h"
     20#include "btQuadWord.h"
     21
    2422/**@brief btVector3 can be used to represent 3D points and vectors.
    2523 * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
    2624 * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
    2725 */
    28 
    29 ATTRIBUTE_ALIGNED16(class) btVector3
    30 {
     26class   btVector3 : public btQuadWord {
     27
    3128public:
    32 
    33 #if defined (__SPU__) && defined (__CELLOS_LV2__)
    34         union {
    35                 vec_float4 mVec128;
    36                 btScalar        m_floats[4];
    37         };
    38 public:
    39         vec_float4      get128() const
    40         {
    41                 return mVec128;
    42         }
    43 public:
    44 #else //__CELLOS_LV2__ __SPU__
    45 #ifdef BT_USE_SSE // WIN32
    46         union {
    47                 __m128 mVec128;
    48                 btScalar        m_floats[4];
    49         };
    50         SIMD_FORCE_INLINE       __m128  get128() const
    51         {
    52                 return mVec128;
    53         }
    54         SIMD_FORCE_INLINE       void    set128(__m128 v128)
    55         {
    56                 mVec128 = v128;
    57         }
    58 #else
    59         btScalar        m_floats[4];
    60 #endif
    61 #endif //__CELLOS_LV2__ __SPU__
    62 
    63         public:
    64 
    6529  /**@brief No initialization constructor */
    6630        SIMD_FORCE_INLINE btVector3() {}
    6731
    68  
     32  /**@brief Constructor from btQuadWordStorage (btVector3 inherits from this so is also valid)
     33   * Note: Vector3 derives from btQuadWordStorage*/
     34        SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q)
     35                : btQuadWord(q)
     36        {
     37        }
    6938       
    7039  /**@brief Constructor from scalars
    … …  
    7342   * @param z Z value
    7443   */
    75         SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
    76         {
    77                 m_floats[0] = x;
    78                 m_floats[1] = y;
    79                 m_floats[2] = z;
    80                 m_floats[3] = btScalar(0.);
    81         }
     44        SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
     45                :btQuadWord(x,y,z,btScalar(0.))
     46        {
     47        }
     48
     49//      SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
     50//              : btQuadWord(x,y,z,w)
     51//      {
     52//      }
    8253
    8354       
    … …  
    8758        {
    8859
    89                 m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
     60                m_floats[0] += v.x(); m_floats[1] += v.y(); m_floats[2] += v.z();
    9061                return *this;
    9162        }
    … …  
    9667        SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
    9768        {
    98                 m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
     69                m_floats[0] -= v.x(); m_floats[1] -= v.y(); m_floats[2] -= v.z();
    9970                return *this;
    10071        }
    … …  
    10374        SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
    10475        {
    105                 m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
     76                m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s;
    10677                return *this;
    10778        }
    … …  
    11990        SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
    12091        {
    121                 return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
     92                return m_floats[0] * v.x() + m_floats[1] * v.y() + m_floats[2] * v.z();
    12293        }
    12394
    … …  
    178149        {
    179150                return btVector3(
    180                         m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
    181                         m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
    182                         m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
     151                        m_floats[1] * v.z() - m_floats[2] * v.y(),
     152                        m_floats[2] * v.x() - m_floats[0] * v.z(),
     153                        m_floats[0] * v.y() - m_floats[1] * v.x());
    183154        }
    184155
    185156        SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
    186157        {
    187                 return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
    188                         m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
    189                         m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
     158                return m_floats[0] * (v1.y() * v2.z() - v1.z() * v2.y()) +
     159                        m_floats[1] * (v1.z() * v2.x() - v1.x() * v2.z()) +
     160                        m_floats[2] * (v1.x() * v2.y() - v1.y() * v2.x());
    190161        }
    191162
    … …  
    194165        SIMD_FORCE_INLINE int minAxis() const
    195166        {
    196                 return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
     167                return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
    197168        }
    198169
    … …  
    201172        SIMD_FORCE_INLINE int maxAxis() const
    202173        {
    203                 return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
     174                return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
    204175        }
    205176
    … …  
    217188        {
    218189                btScalar s = btScalar(1.0) - rt;
    219                 m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
    220                 m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
    221                 m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
     190                m_floats[0] = s * v0.x() + rt * v1.x();
     191                m_floats[1] = s * v0.y() + rt * v1.y();
     192                m_floats[2] = s * v0.z() + rt * v1.z();
    222193                //don't do the unused w component
    223194                //              m_co[3] = s * v0[3] + rt * v1[3];
    … …  
    229200        SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
    230201        {
    231                 return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
    232                         m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
    233                         m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
     202                return btVector3(m_floats[0] + (v.x() - m_floats[0]) * t,
     203                        m_floats[1] + (v.y() - m_floats[1]) * t,
     204                        m_floats[2] + (v.z() - m_floats[2]) * t);
    234205        }
    235206
    … …  
    238209        SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
    239210        {
    240                 m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
     211                m_floats[0] *= v.x(); m_floats[1] *= v.y(); m_floats[2] *= v.z();
    241212                return *this;
    242213        }
    243214
    244          /**@brief Return the x value */
    245                 SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
    246   /**@brief Return the y value */
    247                 SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
    248   /**@brief Return the z value */
    249                 SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
    250   /**@brief Set the x value */
    251                 SIMD_FORCE_INLINE void  setX(btScalar x) { m_floats[0] = x;};
    252   /**@brief Set the y value */
    253                 SIMD_FORCE_INLINE void  setY(btScalar y) { m_floats[1] = y;};
    254   /**@brief Set the z value */
    255                 SIMD_FORCE_INLINE void  setZ(btScalar z) {m_floats[2] = z;};
    256   /**@brief Set the w value */
    257                 SIMD_FORCE_INLINE void  setW(btScalar w) { m_floats[3] = w;};
    258   /**@brief Return the x value */
    259                 SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
    260   /**@brief Return the y value */
    261                 SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
    262   /**@brief Return the z value */
    263                 SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
    264   /**@brief Return the w value */
    265                 SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
    266 
    267         //SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_floats[0])[i]; }     
    268         //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
    269         ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
    270         SIMD_FORCE_INLINE       operator       btScalar *()       { return &m_floats[0]; }
    271         SIMD_FORCE_INLINE       operator const btScalar *() const { return &m_floats[0]; }
    272 
    273         SIMD_FORCE_INLINE       bool    operator==(const btVector3& other) const
    274         {
    275                 return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
    276         }
    277 
    278         SIMD_FORCE_INLINE       bool    operator!=(const btVector3& other) const
    279         {
    280                 return !(*this == other);
    281         }
    282 
    283          /**@brief Set each element to the max of the current values and the values of another btVector3
    284    * @param other The other btVector3 to compare with
    285    */
    286                 SIMD_FORCE_INLINE void  setMax(const btVector3& other)
    287                 {
    288                         btSetMax(m_floats[0], other.m_floats[0]);
    289                         btSetMax(m_floats[1], other.m_floats[1]);
    290                         btSetMax(m_floats[2], other.m_floats[2]);
    291                         btSetMax(m_floats[3], other.w());
    292                 }
    293   /**@brief Set each element to the min of the current values and the values of another btVector3
    294    * @param other The other btVector3 to compare with
    295    */
    296                 SIMD_FORCE_INLINE void  setMin(const btVector3& other)
    297                 {
    298                         btSetMin(m_floats[0], other.m_floats[0]);
    299                         btSetMin(m_floats[1], other.m_floats[1]);
    300                         btSetMin(m_floats[2], other.m_floats[2]);
    301                         btSetMin(m_floats[3], other.w());
    302                 }
    303 
    304                 SIMD_FORCE_INLINE void  setValue(const btScalar& x, const btScalar& y, const btScalar& z)
    305                 {
    306                         m_floats[0]=x;
    307                         m_floats[1]=y;
    308                         m_floats[2]=z;
    309                         m_floats[3] = 0.f;
    310                 }
    311 
    312                 void    getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
    313                 {
    314                         v0->setValue(0.         ,-z()           ,y());
    315                         v1->setValue(z()        ,0.                     ,-x());
    316                         v2->setValue(-y()       ,x()    ,0.);
    317                 }
     215       
    318216
    319217};
    … …  
    323221operator+(const btVector3& v1, const btVector3& v2)
    324222{
    325         return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
     223        return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
    326224}
    327225
    … …  
    330228operator*(const btVector3& v1, const btVector3& v2)
    331229{
    332         return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
     230        return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
    333231}
    334232
    … …  
    337235operator-(const btVector3& v1, const btVector3& v2)
    338236{
    339         return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
     237        return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
    340238}
    341239/**@brief Return the negative of the vector */
    … …  
    343241operator-(const btVector3& v)
    344242{
    345         return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
     243        return btVector3(-v.x(), -v.y(), -v.z());
    346244}
    347245
    … …  
    350248operator*(const btVector3& v, const btScalar& s)
    351249{
    352         return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
     250        return btVector3(v.x() * s, v.y() * s, v.z() * s);
    353251}
    354252
    … …  
    372270operator/(const btVector3& v1, const btVector3& v2)
    373271{
    374         return btVector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]);
     272        return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
    375273}
    376274
    … …  
    428326}
    429327
    430 
     328/**@brief Test if each element of the vector is equivalent */
     329SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2)
     330{
     331        return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z();
     332}
    431333
    432334SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
    … …  
    503405                {
    504406                        maxIndex = 2;
    505                         maxVal =m_floats[2];
     407                        maxVal = m_floats[2];
    506408                }
    507409                if (m_floats[3] > maxVal)
    … …  
    536438                {
    537439                        minIndex = 2;
    538                         minVal =m_floats[2];
     440                        minVal = m_floats[2];
    539441                }
    540442                if (m_floats[3] < minVal)
    … …  
    553455                return absolute4().maxAxis4();
    554456        }
    555 
    556        
    557  
    558 
    559   /**@brief Set x,y,z and zero w
    560    * @param x Value of x
    561    * @param y Value of y
    562    * @param z Value of z
    563    */
    564                
    565 
    566 /*              void getValue(btScalar *m) const
    567                 {
    568                         m[0] = m_floats[0];
    569                         m[1] = m_floats[1];
    570                         m[2] =m_floats[2];
    571                 }
    572 */
    573 /**@brief Set the values
    574    * @param x Value of x
    575    * @param y Value of y
    576    * @param z Value of z
    577    * @param w Value of w
    578    */
    579                 SIMD_FORCE_INLINE void  setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
    580                 {
    581                         m_floats[0]=x;
    582                         m_floats[1]=y;
    583                         m_floats[2]=z;
    584                         m_floats[3]=w;
    585                 }
    586 
    587 
    588  
    589457
    590458};

  • code/branches/questsystem5/src/bullet/changes_orxonox.diff

    r2907 r2908  
    1 Index: btScalar.h
    2 ===================================================================
    3 --- btScalar.h  (revision 2882)
    4 +++ btScalar.h  (working copy)
    5 @@ -236,7 +236,11 @@
     1--- LinearMath\btScalar.h       So Feb 15 18:28:13 2009
     2+++ LinearMath\btScalar.h       Sa Feb 14 23:04:53 2009
     3@@ -227,7 +227,11 @@
    64 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
    75 SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
    … …  
    1311+  SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
    1412+  #endif
    15 
     13 
    1614 #endif
    17 
     15 
Note: See TracChangeset for help on using the changeset viewer.