Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Changeset 2882


Ignore:
Timestamp:
Mar 31, 2009, 8:05:51 PM (16 years ago)
Author:
rgrieder
Message:

Update from Bullet 2.73 to 2.74.

Location:
code/trunk/src/bullet
Files:
1 added
110 edited

Legend:

Unmodified
Added
Removed

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp

    r2662 r2882  
    2020#include "btAxisSweep3.h"
    2121
    22 #include <assert.h>
    2322
    2423btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h

    r2662 r2882  
    4343public:
    4444       
     45 BT_DECLARE_ALIGNED_ALLOCATOR();
    4546
    4647        class Edge
    … …  
    139140        SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
    140141
    141         void resetPool();
     142        virtual void resetPool(btDispatcher* dispatcher);
    142143
    143144        void    processAllOverlappingPairs(btOverlapCallback* callback);
    … …  
    221222
    222223        if (checkCardinality)
    223                 assert(numEdges == m_numHandles*2+1);
     224                btAssert(numEdges == m_numHandles*2+1);
    224225}
    225226#endif //DEBUG_BROADPHASE
    … …  
    347348        }
    348349
    349         //assert(bounds.HasVolume());
     350        //btAssert(bounds.HasVolume());
    350351
    351352        // init bounds
    … …  
    453454BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
    454455{
    455         assert(m_firstFreeHandle);
     456        btAssert(m_firstFreeHandle);
    456457
    457458        BP_FP_INT_TYPE handle = m_firstFreeHandle;
    … …  
    465466void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
    466467{
    467         assert(handle > 0 && handle < m_maxHandles);
     468        btAssert(handle > 0 && handle < m_maxHandles);
    468469
    469470        getHandle(handle)->SetNextFree(m_firstFreeHandle);
    … …  
    588589
    589590template <typename BP_FP_INT_TYPE>
    590 void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool()
     591void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* dispatcher)
    591592{
    592593        if (m_numHandles == 0)
    … …  
    642643                        if (!isDuplicate)
    643644                        {
     645                                ///important to use an AABB test that is consistent with the broadphase
    644646                                bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
    645647
    … …  
    687689        }
    688690
    689 
    690 
    691        
    692 
    693691}
    694692
    … …  
    731729void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
    732730{
    733 //      assert(bounds.IsFinite());
    734         //assert(bounds.HasVolume());
     731//      btAssert(bounds.IsFinite());
     732        //btAssert(bounds.HasVolume());
    735733
    736734        Handle* pHandle = getHandle(handle);

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h

    r2662 r2882  
    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
    6770        virtual void    printStats() = 0;
    6871

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h

    r2662 r2882  
    7070
    7171        SOFTBODY_SHAPE_PROXYTYPE,
    72 
     72        HFFLUID_SHAPE_PROXYTYPE,
     73        HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE,
    7374        INVALID_SHAPE_PROXYTYPE,
    7475
    … …  
    188189
    189190                //keep them sorted, so the std::set operations work
    190                 if (&proxy0 < &proxy1)
     191                if (proxy0.m_uniqueId < proxy1.m_uniqueId)
    191192        {
    192193            m_pProxy0 = &proxy0;
    … …  
    229230                bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b )
    230231                {
    231                          return a.m_pProxy0 > b.m_pProxy0 ||
    232                                 (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 > b.m_pProxy1) ||
     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) ||
    233239                                (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm);
    234240                }

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp

    r2662 r2882  
    394394}
    395395
    396 //
     396#if 0
    397397static DBVT_INLINE btDbvtNode*  walkup(btDbvtNode* n,int count)
    398398{
    … …  
    400400        return(n);
    401401}
     402#endif
    402403
    403404//
    … …  
    424425void                    btDbvt::clear()
    425426{
    426         if(m_root)      recursedeletenode(this,m_root);
     427        if(m_root)     
     428                recursedeletenode(this,m_root);
    427429        btAlignedFree(m_free);
    428430        m_free=0;
     431        m_lkhd          =       -1;
     432        m_stkStack.clear();
     433        m_opath         =       0;
     434       
    429435}
    430436

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h

    r2662 r2882  
    5858
    5959//SSE gives errors on a MSVC 7.1
    60 #if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
     60#ifdef BT_USE_SSE
    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*)0;
     85#define DBVT_CHECKTYPE                          static const ICollide&  typechecker=*(T*)1;(void)typechecker;
    8686#else
    8787#define DBVT_VIRTUAL_DTOR(a)            virtual ~a() {}
    … …  
    147147        DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
    148148                const btDbvtAabbMm& b);
    149         DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
    150                 const btDbvtAabbMm& b,
    151                 const btTransform& xform);
     149       
    152150        DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
    153151                const btVector3& b);
    … …  
    305303                  const btDbvtNode* root1,
    306304                  DBVT_IPOLICY);
    307 
     305#if 0
    308306        DBVT_PREFIX
    309307                void            collideTT(      const btDbvtNode* root0,
    … …  
    317315                const btTransform& xform1,
    318316                DBVT_IPOLICY);
     317#endif
     318
    319319        DBVT_PREFIX
    320320                void            collideTV(      const btDbvtNode* root,
    … …  
    531531}
    532532
    533 //
    534 DBVT_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 }
     533
    548534
    549535//
    … …  
    849835}
    850836
    851 
     837#if 0
    852838//
    853839DBVT_PREFIX
    … …  
    905891                }
    906892}
    907 
    908893//
    909894DBVT_PREFIX
    … …  
    917902        collideTT(root0,root1,xform,policy);
    918903}
     904#endif
    919905
    920906//

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

    r2662 r2882  
    101101                        btDbvtProxy*    pb=(btDbvtProxy*)nb->data;
    102102#if DBVT_BP_SORTPAIRS
    103                         if(pa>pb) btSwap(pa,pb);
     103                        if(pa->m_uniqueId>pb->m_uniqueId)
     104                                btSwap(pa,pb);
    104105#endif
    105106                        pbp->m_paircache->addOverlappingPair(pa,pb);
    … …  
    133134        m_updates_done          =       0;
    134135        m_updates_ratio         =       0;
    135         m_paircache                     =       paircache?
    136 paircache       :
    137         new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
     136        m_paircache                     =       paircache? paircache    : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
    138137        m_gid                           =       0;
    139138        m_pid                           =       0;
    … …  
    211210}
    212211
     212struct  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
    213226void    btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
    214227{
    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 
    230228        BroadphaseRayTester callback(rayCallback);
    231229
    … …  
    342340        }
    343341#endif
     342
     343        performDeferredRemoval(dispatcher);
     344
     345}
     346
     347void 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        }
    344417}
    345418
    … …  
    347420void                                                    btDbvtBroadphase::collide(btDispatcher* dispatcher)
    348421{
     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
    349439        SPC(m_profiling.m_total);
    350440        /* optimize                             */
    … …  
    402492                if(pairs.size()>0)
    403493                {
    404                         const int       ci=pairs.size();
    405                         int                     ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
     494
     495                        int                     ni=btMin(pairs.size(),btMax<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
    406496                        for(int i=0;i<ni;++i)
    407497                        {
    408                                 btBroadphasePair&       p=pairs[(m_cid+i)%ci];
     498                                btBroadphasePair&       p=pairs[(m_cid+i)%pairs.size()];
    409499                                btDbvtProxy*            pa=(btDbvtProxy*)p.m_pProxy0;
    410500                                btDbvtProxy*            pb=(btDbvtProxy*)p.m_pProxy1;
    … …  
    412502                                {
    413503#if DBVT_BP_SORTPAIRS
    414                                         if(pa>pb) btSwap(pa,pb);
     504                                        if(pa->m_uniqueId>pb->m_uniqueId)
     505                                                btSwap(pa,pb);
    415506#endif
    416507                                        m_paircache->removeOverlappingPair(pa,pb,dispatcher);
    … …  
    467558        aabbMin=bounds.Mins();
    468559        aabbMax=bounds.Maxs();
     560}
     561
     562void 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        }
    469593}
    470594
    … …  
    601725#undef  SPC
    602726#endif
     727

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

    r2662 r2882  
    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
    117124};
    118125

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp

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

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h

    r2662 r2882  
    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
    146149};
    147150

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

    r2662 r2882  
    2020#include "btDispatcher.h"
    2121#include "btCollisionAlgorithm.h"
     22#include "LinearMath/btAabbUtil2.h"
    2223
    2324#include <stdio.h>
    … …  
    136137{
    137138        gFindPairs++;
    138         if(proxy0>proxy1) btSwap(proxy0,proxy1);
     139        if(proxy0->m_uniqueId>proxy1->m_uniqueId)
     140                btSwap(proxy0,proxy1);
    139141        int proxyId1 = proxy0->getUid();
    140142        int proxyId2 = proxy1->getUid();
    … …  
    212214btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
    213215{
    214         if(proxy0>proxy1) btSwap(proxy0,proxy1);
     216        if(proxy0->m_uniqueId>proxy1->m_uniqueId)
     217                btSwap(proxy0,proxy1);
    215218        int proxyId1 = proxy0->getUid();
    216219        int proxyId2 = proxy1->getUid();
    … …  
    271274{
    272275        gRemovePairs++;
    273         if(proxy0>proxy1) btSwap(proxy0,proxy1);
     276        if(proxy0->m_uniqueId>proxy1->m_uniqueId)
     277                btSwap(proxy0,proxy1);
    274278        int proxyId1 = proxy0->getUid();
    275279        int proxyId2 = proxy1->getUid();
    … …  
    393397}
    394398
     399void    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}
    395428
    396429
    … …  
    430463{
    431464        //don't add overlap with own
    432         assert(proxy0 != proxy1);
     465        btAssert(proxy0 != proxy1);
    433466
    434467        if (!needsBroadphaseCollision(proxy0,proxy1))
    … …  
    461494        if (findIndex < m_overlappingPairArray.size())
    462495        {
    463                 //assert(it != m_overlappingPairSet.end());
     496                //btAssert(it != m_overlappingPairSet.end());
    464497                 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
    465498                return pair;
    … …  
    491524                {
    492525                        cleanOverlappingPair(*pair,dispatcher);
    493 
    494                         m_overlappingPairArray.swap(i,m_overlappingPairArray.capacity()-1);
     526                        pair->m_pProxy0 = 0;
     527                        pair->m_pProxy1 = 0;
     528                        m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
    495529                        m_overlappingPairArray.pop_back();
    496530                        gOverlappingPairs--;
    … …  
    592626        processAllOverlappingPairs(&removeCallback,dispatcher);
    593627}
     628
     629void    btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
     630{
     631        //should already be sorted
     632}
     633

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

    r2662 r2882  
    8484
    8585        virtual void    setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0;
     86
     87        virtual void    sortOverlappingPairs(btDispatcher* dispatcher) = 0;
     88
    8689
    8790};
    … …  
    260263        }
    261264
    262 public:
     265        virtual void    sortOverlappingPairs(btDispatcher* dispatcher);
     266       
     267
     268protected:
    263269       
    264270        btAlignedObjectArray<int>       m_hashTable;
    … …  
    370376                }
    371377
     378                virtual void    sortOverlappingPairs(btDispatcher* dispatcher);
     379               
    372380
    373381};
    … …  
    448456        }
    449457       
     458        virtual void    sortOverlappingPairs(btDispatcher* dispatcher)
     459        {
     460        }
    450461
    451462

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp

    r2662 r2882  
    470470
    471471#ifdef RAYAABB2
    472         btVector3 rayFrom = raySource;
    473472        btVector3 rayDir = (rayTarget-raySource);
    474473        rayDir.normalize ();
    … …  
    559558
    560559#ifdef RAYAABB2
    561         btVector3 rayFrom = raySource;
    562560        btVector3 rayDirection = (rayTarget-raySource);
    563561        rayDirection.normalize ();
    … …  
    818816#include <new>
    819817
     818#if 0
    820819//PCK: consts
    821820static const unsigned BVH_ALIGNMENT = 16;
    … …  
    823822
    824823static const unsigned BVH_ALIGNMENT_BLOCKS = 2;
    825 
     824#endif
    826825
    827826
    … …  
    11471146
    11481147
     1148

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

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

  • code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h

    r2662 r2882  
    109109        }
    110110
     111        ///reset broadphase internal structures, to ensure determinism/reproducability
     112        virtual void resetPool(btDispatcher* dispatcher);
     113
    111114
    112115        void    validate();

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

    r2662 r2882  
    5353                {
    5454                        m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
    55                         assert(m_doubleDispatch[i][j]);
     55                        btAssert(m_doubleDispatch[i][j]);
    5656                }
    5757        }
    … …  
    8080        btCollisionObject* body1 = (btCollisionObject*)b1;
    8181
    82         btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()));
    83        
     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               
    8488        void* mem = 0;
    8589       
    … …  
    9296
    9397        }
    94         btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold);
     98        btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold);
    9599        manifold->m_index1a = m_manifoldsPtr.size();
    96100        m_manifoldsPtr.push_back(manifold);
    … …  
    143147        return algo;
    144148}
    145 
    146149
    147150
    … …  
    161164bool    btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1)
    162165{
    163         assert(body0);
    164         assert(body1);
     166        btAssert(body0);
     167        btAssert(body1);
    165168
    166169        bool needsCollision = true;

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp

    r2662 r2882  
    1818
    1919btCollisionObject::btCollisionObject()
    20         :       m_broadphaseHandle(0),
     20        :       m_anisotropicFriction(1.f,1.f,1.f),
     21        m_hasAnisotropicFriction(false),
     22        m_contactProcessingThreshold(1e30f),
     23                m_broadphaseHandle(0),
    2124                m_collisionShape(0),
    2225                m_rootCollisionShape(0),

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h

    r2662 r2882  
    5353        btVector3       m_interpolationLinearVelocity;
    5454        btVector3       m_interpolationAngularVelocity;
     55       
     56        btVector3               m_anisotropicFriction;
     57        bool                            m_hasAnisotropicFriction;
     58        btScalar                m_contactProcessingThreshold;   
     59
    5560        btBroadphaseProxy*              m_broadphaseHandle;
    5661        btCollisionShape*               m_collisionShape;
    … …  
    115120                CO_COLLISION_OBJECT =1,
    116121                CO_RIGID_BODY,
    117                 CO_SOFT_BODY,
    118122                ///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter
    119123                ///It is useful for collision sensors, explosion objects, character controller etc.
    120                 CO_GHOST_OBJECT
     124                CO_GHOST_OBJECT,
     125                CO_SOFT_BODY,
     126                CO_HF_FLUID
    121127        };
    122128
    … …  
    127133        }
    128134
     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        }
    129159
    130160        SIMD_FORCE_INLINE bool          isStaticObject() const {

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

    r2662 r2882  
    7070                        getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
    7171                        getBroadphase()->destroyProxy(bp,m_dispatcher1);
     72                        collisionObject->setBroadphaseHandle(0);
    7273                }
    7374        }
    … …  
    119120
    120121
     122void    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
    121155void    btCollisionWorld::updateAabbs()
    122156{
    … …  
    131165                if (colObj->isActive())
    132166                {
    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 
     167                        updateSingleAabb(colObj);
     168                }
     169        }
    165170}
    166171
    … …  
    294299                                        BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
    295300                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh):
    296                                                 btTriangleRaycastCallback(from,to),
     301                  //@BP Mod
     302                                                btTriangleRaycastCallback(from,to, resultCallback->m_flags),
    297303                                                        m_resultCallback(resultCallback),
    298304                                                        m_collisionObject(collisionObject),
    … …  
    343349                                        BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
    344350                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh):
    345                                                 btTriangleRaycastCallback(from,to),
     351                  //@BP Mod
     352                  btTriangleRaycastCallback(from,to, resultCallback->m_flags),
    346353                                                        m_resultCallback(resultCallback),
    347354                                                        m_collisionObject(collisionObject),
    … …  
    664671                        //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
    665672                        //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
    666                        
     673#if 0
    667674#ifdef RECALCULATE_AABB
    668675                        btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
    … …  
    672679                        const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
    673680                        const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
     681#endif
    674682#endif
    675683                        //btScalar hitLambda = m_resultCallback.m_closestHitFraction;

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h

    r2877 r2882  
    139139        }
    140140
     141        void    updateSingleAabb(btCollisionObject* colObj);
     142
    141143        virtual void    updateAabbs();
    142 
    143144
    144145       
    … …  
    193194                short int       m_collisionFilterGroup;
    194195                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;
    195198
    196199                virtual ~RayResultCallback()
    … …  
    206209                        m_collisionObject(0),
    207210                        m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
    208                         m_collisionFilterMask(btBroadphaseProxy::AllFilter)
     211                        m_collisionFilterMask(btBroadphaseProxy::AllFilter),
     212         //@BP Mod
     213         m_flags(0)
    209214                {
    210215                }

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

    r2662 r2882  
    2020#include "LinearMath/btIDebugDraw.h"
    2121#include "LinearMath/btAabbUtil2.h"
     22#include "btManifoldResult.h"
    2223
    2324btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
    … …  
    2930
    3031        btCollisionObject* colObj = m_isSwapped? body1 : body0;
     32        btAssert (colObj->getCollisionShape()->isCompound());
     33       
     34        btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
     35        m_compoundShapeRevision = compoundShape->getUpdateRevision();
     36       
     37        preallocateChildAlgorithms(body0,body1);
     38}
     39
     40void    btCompoundCollisionAlgorithm::preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1)
     41{
     42        btCollisionObject* colObj = m_isSwapped? body1 : body0;
    3143        btCollisionObject* otherObj = m_isSwapped? body0 : body1;
    32         assert (colObj->getCollisionShape()->isCompound());
     44        btAssert (colObj->getCollisionShape()->isCompound());
    3345       
    3446        btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
     47
    3548        int numChildren = compoundShape->getNumChildShapes();
    3649        int i;
    … …  
    4760                        btCollisionShape* childShape = compoundShape->getChildShape(i);
    4861                        colObj->internalSetTemporaryCollisionShape( childShape );
    49                         m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj,m_sharedManifold);
     62                        m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(colObj,otherObj,m_sharedManifold);
    5063                        colObj->internalSetTemporaryCollisionShape( tmpShape );
    5164                }
    … …  
    5366}
    5467
    55 
    56 btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
     68void    btCompoundCollisionAlgorithm::removeChildAlgorithms()
    5769{
    5870        int numChildren = m_childCollisionAlgorithms.size();
    … …  
    6678                }
    6779        }
     80}
     81
     82btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
     83{
     84        removeChildAlgorithms();
    6885}
    6986
    … …  
    168185        btCollisionObject* otherObj = m_isSwapped? body0 : body1;
    169186
    170         assert (colObj->getCollisionShape()->isCompound());
     187       
     188
     189        btAssert (colObj->getCollisionShape()->isCompound());
    171190        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
    172202
    173203        btDbvt* tree = compoundShape->getDynamicAabbTree();
    … …  
    175205        btCompoundLeafCallback  callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
    176206
     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        }
    177231
    178232        if (tree)
    … …  
    243297        btCollisionObject* otherObj = m_isSwapped? body0 : body1;
    244298
    245         assert (colObj->getCollisionShape()->isCompound());
     299        btAssert (colObj->getCollisionShape()->isCompound());
    246300       
    247301        btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
    … …  
    286340
    287341
     342

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h

    r2662 r2882  
    2727#include "LinearMath/btAlignedObjectArray.h"
    2828class btDispatcher;
     29class btCollisionObject;
    2930
    3031/// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
    … …  
    3637        class btPersistentManifold*     m_sharedManifold;
    3738        bool                                    m_ownsManifold;
     39
     40        int     m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated
    3841       
     42        void    removeChildAlgorithms();
     43       
     44        void    preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1);
     45
    3946public:
    4047

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

    r2662 r2882  
    5252btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*                       simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
    5353{
     54        m_numPerturbationIterations = 0;
     55        m_minimumPointsPerturbationThreshold = 3;
    5456        m_simplexSolver = simplexSolver;
    5557        m_pdSolver = pdSolver;
    … …  
    6062}
    6163
    62 btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
     64btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
    6365: btActivatingCollisionAlgorithm(ci,body0,body1),
    6466m_simplexSolver(simplexSolver),
    … …  
    6668m_ownManifold (false),
    6769m_manifoldPtr(mf),
    68 m_lowLevelOfDetail(false)
     70m_lowLevelOfDetail(false),
    6971#ifdef USE_SEPDISTANCE_UTIL2
    7072,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
    71                           (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc())
     73                          (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
    7274#endif
     75m_numPerturbationIterations(numPerturbationIterations),
     76m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
    7377{
    7478        (void)body0;
    … …  
    9498
    9599
    96 
    97 
     100struct 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
     156extern btScalar gContactBreakingThreshold;
    98157
    99158//
    … …  
    111170        resultOut->setPersistentManifold(m_manifoldPtr);
    112171
     172        //comment-out next line to test multi-contact generation
     173        //resultOut->getPersistentManifold()->clearManifold();
    113174       
    114175
    … …  
    147208
    148209        gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
    149 
    150210        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       
    151281
    152282#ifdef USE_SEPDISTANCE_UTIL2

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

    r2662 r2882  
    3434///#define USE_SEPDISTANCE_UTIL2 1
    3535
    36 ///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
     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
    3739class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
    3840{
    … …  
    4850        bool                    m_lowLevelOfDetail;
    4951       
     52        int m_numPerturbationIterations;
     53        int m_minimumPointsPerturbationThreshold;
     54
     55
    5056        ///cache separating vector to speedup collision detection
    5157       
    … …  
    5359public:
    5460
    55         btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
     61        btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
     62
    5663
    5764        virtual ~btConvexConvexAlgorithm();
    … …  
    7986        struct CreateFunc :public       btCollisionAlgorithmCreateFunc
    8087        {
     88
    8189                btConvexPenetrationDepthSolver*         m_pdSolver;
    8290                btSimplexSolverInterface*                       m_simplexSolver;
    83                
     91                int m_numPerturbationIterations;
     92                int m_minimumPointsPerturbationThreshold;
     93
    8494                CreateFunc(btSimplexSolverInterface*                    simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
    8595               
    … …  
    8999                {
    90100                        void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
    91                         return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver);
     101                        return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
    92102                }
    93103        };

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp

    r2662 r2882  
    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)
     25btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold)
    2626: btCollisionAlgorithm(ci),
    2727m_ownManifold(false),
    2828m_manifoldPtr(mf),
    29 m_isSwapped(isSwapped)
     29m_isSwapped(isSwapped),
     30m_numPerturbationIterations(numPerturbationIterations),
     31m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
    3032{
    3133        btCollisionObject* convexObj = m_isSwapped? col1 : col0;
    3234        btCollisionObject* planeObj = m_isSwapped? col0 : col1;
    33        
     35
    3436        if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj))
    3537        {
    … …  
    4951}
    5052
    51 
    52 
    53 void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
     53void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
    5454{
    55         (void)dispatchInfo;
    56         (void)resultOut;
    57         if (!m_manifoldPtr)
    58                 return;
    59 
    60         btCollisionObject* convexObj = m_isSwapped? body1 : body0;
     55    btCollisionObject* convexObj = m_isSwapped? body1 : body0;
    6156        btCollisionObject* planeObj = m_isSwapped? body0: body1;
    6257
    … …  
    6459        btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
    6560
    66         bool hasCollision = false;
     61    bool hasCollision = false;
    6762        const btVector3& planeNormal = planeShape->getPlaneNormal();
    6863        const btScalar& planeConstant = planeShape->getPlaneConstant();
     64       
     65        btTransform convexWorldTransform = convexObj->getWorldTransform();
     66        btTransform convexInPlaneTrans;
     67        convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexWorldTransform;
     68        //now perturbe the convex-world transform
     69        convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot);
    6970        btTransform planeInConvex;
    70         planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform();
    71         btTransform convexInPlaneTrans;
    72         convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform();
     71        planeInConvex= convexWorldTransform.inverse() * planeObj->getWorldTransform();
     72       
     73        btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
    7374
    74         btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
    7575        btVector3 vtxInPlane = convexInPlaneTrans(vtx);
    7676        btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
    … …  
    8888                resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
    8989        }
     90}
     91
     92
     93void 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
    90137        if (m_ownManifold)
    91138        {

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h

    r2662 r2882  
    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        
     33        bool            m_isSwapped;
     34        int                     m_numPerturbationIterations;
     35        int                     m_minimumPointsPerturbationThreshold;
     36
    3537public:
    3638
    37         btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
     39        btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold);
    3840
    3941        virtual ~btConvexPlaneCollisionAlgorithm();
    4042
    4143        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);
    4246
    4347        virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
    … …  
    5357        struct CreateFunc :public       btCollisionAlgorithmCreateFunc
    5458        {
     59                int     m_numPerturbationIterations;
     60                int m_minimumPointsPerturbationThreshold;
     61                       
     62                CreateFunc()
     63                        : m_numPerturbationIterations(3),
     64                        m_minimumPointsPerturbationThreshold(3)
     65                {
     66                }
     67               
    5568                virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
    5669                {
    … …  
    5871                        if (!m_swapped)
    5972                        {
    60                                 return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false);
     73                                return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
    6174                        } else
    6275                        {
    63                                 return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true);
     76                                return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
    6477                        }
    6578                }

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp

    r2662 r2882  
    289289        return m_emptyCreateFunc;
    290290}
     291
     292void 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/trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h

    r2662 r2882  
    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);
    114122
    115123};

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp

    r2662 r2882  
    6464btPairCachingGhostObject::~btPairCachingGhostObject()
    6565{
     66        m_hashPairCache->~btHashedOverlappingPairCache();
    6667        btAlignedFree( m_hashPairCache );
    6768}

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp

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

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h

    r2662 r2882  
    4646        int m_index0;
    4747        int m_index1;
     48       
     49
    4850public:
    4951
    … …  
    7880        }
    7981
     82
    8083        virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
    8184

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

    r2662 r2882  
    2525#include "LinearMath/btQuickprof.h"
    2626
    27 btSimulationIslandManager::btSimulationIslandManager()
     27btSimulationIslandManager::btSimulationIslandManager():
     28m_splitIslands(true)
    2829{
    2930}
    … …  
    4445       
    4546        {
    46                 btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
    47 
     47               
    4848                for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++)
    4949                {
     50                        btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
    5051                        const btBroadphasePair& collisionPair = pairPtr[i];
    5152                        btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
    … …  
    186187                        }
    187188
    188                         assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
     189                        btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
    189190                        if (colObj0->getIslandTag() == islandId)
    190191                        {
    … …  
    213214                                }
    214215
    215                                 assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
     216                                btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
    216217
    217218                                if (colObj0->getIslandTag() == islandId)
    … …  
    234235                                }
    235236
    236                                 assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
     237                                btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
    237238
    238239                                if (colObj0->getIslandTag() == islandId)
    … …  
    252253        int maxNumManifolds = dispatcher->getNumManifolds();
    253254
    254 #define SPLIT_ISLANDS 1
    255 #ifdef SPLIT_ISLANDS
    256 
    257        
    258 #endif //SPLIT_ISLANDS
     255//#define SPLIT_ISLANDS 1
     256//#ifdef SPLIT_ISLANDS
     257
     258       
     259//#endif //SPLIT_ISLANDS
    259260
    260261       
    … …  
    280281                                colObj0->activate();
    281282                        }
    282 #ifdef SPLIT_ISLANDS
    283         //              //filtering for response
    284                         if (dispatcher->needsResponse(colObj0,colObj1))
    285                                 m_islandmanifold.push_back(manifold);
    286 #endif //SPLIT_ISLANDS
     283                        if(m_splitIslands)
     284                        {
     285                                //filtering for response
     286                                if (dispatcher->needsResponse(colObj0,colObj1))
     287                                        m_islandmanifold.push_back(manifold);
     288                        }
    287289                }
    288290        }
    … …  
    304306        BT_PROFILE("processIslands");
    305307
    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];
     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                        }
    359380                       
    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 }
     381                        if (numIslandManifolds)
     382                        {
     383                                startManifoldIndex = endManifoldIndex;
     384                        }
     385
     386                        m_islandBodies.resize(0);
     387                }
     388        } // else if(!splitIslands)
     389
     390}

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h

    r2662 r2882  
    3636        btAlignedObjectArray<btCollisionObject* >  m_islandBodies;
    3737       
     38        bool m_splitIslands;
    3839       
    3940public:
    … …  
    6667        void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
    6768
     69        bool getSplitIslands()
     70        {
     71                return m_splitIslands;
     72        }
     73        void setSplitIslands(bool doSplitIslands)
     74        {
     75                m_splitIslands = doSplitIslands;
     76        }
     77
    6878};
    6979

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp

    r2662 r2882  
    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/trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp

    r2662 r2882  
    1515
    1616#include "btUnionFind.h"
    17 #include <assert.h>
    18 
    1917
    2018

  • code/trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.h

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

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btBoxShape.h

    r2662 r2882  
    162162                {
    163163                case 0:
    164                         plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
    165                         plane[3] = -halfExtents.x();
     164                        plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
    166165                        break;
    167166                case 1:
    168                         plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
    169                         plane[3] = -halfExtents.x();
     167                        plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
    170168                        break;
    171169                case 2:
    172                         plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
    173                         plane[3] = -halfExtents.y();
     170                        plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
    174171                        break;
    175172                case 3:
    176                         plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
    177                         plane[3] = -halfExtents.y();
     173                        plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
    178174                        break;
    179175                case 4:
    180                         plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
    181                         plane[3] = -halfExtents.z();
     176                        plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
    182177                        break;
    183178                case 5:
    184                         plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
    185                         plane[3] = -halfExtents.z();
     179                        plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
    186180                        break;
    187181                default:
    188                         assert(0);
     182                        btAssert(0);
    189183                }
    190184        }
    … …  
    313307                        break;
    314308                default:
    315                         assert(0);
     309                        btAssert(0);
    316310                }
    317311        }

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp

    r2662 r2882  
    1515
    1616#include "BulletCollision/CollisionShapes/btCollisionShape.h"
     17
     18
     19btScalar gContactThresholdFactor=btScalar(0.02);
    1720
    1821
    … …  
    4548btScalar        btCollisionShape::getContactBreakingThreshold() const
    4649{
    47         ///@todo make this 0.1 configurable
    48         return getAngularMotionDisc() * btScalar(0.1);
     50        return getAngularMotionDisc() * gContactThresholdFactor;
    4951}
    5052btScalar        btCollisionShape::getAngularMotionDisc() const

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp

    r2662 r2882  
    2323m_collisionMargin(btScalar(0.)),
    2424m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
    25 m_dynamicAabbTree(0)
     25m_dynamicAabbTree(0),
     26m_updateRevision(1)
    2627{
    2728        m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
    … …  
    4748void    btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
    4849{
     50        m_updateRevision++;
    4951        //m_childTransforms.push_back(localTransform);
    5052        //m_childShapes.push_back(shape);
    … …  
    5557        child.m_childMargin = shape->getMargin();
    5658
    57         m_children.push_back(child);
    58 
     59       
    5960        //extend the local aabbMin/aabbMax
    6061        btVector3 localAabbMin,localAabbMax;
    … …  
    7576        {
    7677                const btDbvtVolume      bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
    77                 int index = m_children.size()-1;
     78                int index = m_children.size();
    7879                child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
    7980        }
     81
     82        m_children.push_back(child);
    8083
    8184}
    … …  
    100103void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
    101104{
     105        m_updateRevision++;
    102106        btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
    103107        if (m_dynamicAabbTree)
    … …  
    114118void btCompoundShape::removeChildShape(btCollisionShape* shape)
    115119{
     120        m_updateRevision++;
    116121        // Find the children containing the shape specified, and remove those children.
    117122        //note: there might be multiple children using the same shape!
    … …  
    140145        // Recalculate the local aabb
    141146        // Brute force, it iterates over all the shapes left.
     147
    142148        m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
    143149        m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
    … …  
    162168{
    163169        btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
     170        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        }
    164178        localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
    165         btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
     179               
    166180
    167181        btMatrix3x3 abs_b = trans.getBasis().absolute(); 
    … …  
    174188        aabbMin = center-extent;
    175189        aabbMax = center+extent;
    176 
     190       
    177191}
    178192

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.h

    r2662 r2882  
    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;
    6164
    6265public:
    … …  
    153156        void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const;
    154157
     158        int     getUpdateRevision() const
     159        {
     160                return m_updateRevision;
     161        }
    155162
    156163private:

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btConeShape.cpp

    r2662 r2882  
    6161                break;
    6262        default:
    63                 assert(0);
     63                btAssert(0);
    6464        };
    6565}

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp

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

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp

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

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp

    r2662 r2882  
    155155
    156156                btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
    157                 btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
    158157                btScalar halfHeight = capsuleShape->getHalfHeight();
    159158                int capsuleUpAxis = capsuleShape->getUpAxis();
    … …  
    302301        {
    303302                btSphereShape* sphereShape = (btSphereShape*)this;
    304                 float radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
    305                 float margin = radius + sphereShape->getMarginNonVirtual();
     303                btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
     304                btScalar margin = radius + sphereShape->getMarginNonVirtual();
    306305                const btVector3& center = t.getOrigin();
    307306                btVector3 extent(margin,margin,margin);
    … …  
    315314        {
    316315                btBoxShape* convexShape = (btBoxShape*)this;
    317                 float margin=convexShape->getMarginNonVirtual();
     316                btScalar margin=convexShape->getMarginNonVirtual();
    318317                btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
    319318                halfExtents += btVector3(margin,margin,margin);

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

    r2662 r2882  
    145145}
    146146
    147 btScalar        btHeightfieldTerrainShape::getHeightFieldValue(int x,int y) const
     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.
     151btScalar
     152btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const
    148153{
    149154        btScalar val = 0.f;
    … …  
    182187
    183188
    184 
     189/// this returns the vertex in bullet-local coordinates
    185190void    btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const
    186191{
    187 
    188192        btAssert(x>=0);
    189193        btAssert(y>=0);
    … …  
    191195        btAssert(y<m_heightStickLength);
    192196
    193 
    194         btScalar        height = getHeightFieldValue(x,y);
     197        btScalar        height = getRawHeightFieldValue(x,y);
    195198
    196199        switch (m_upAxis)
    … …  
    199202                {
    200203                vertex.setValue(
    201                         height,
     204                        height - m_localOrigin.getX(),
    202205                        (-m_width/btScalar(2.0)) + x,
    203206                        (-m_length/btScalar(2.0) ) + y
    … …  
    209212                        vertex.setValue(
    210213                        (-m_width/btScalar(2.0)) + x,
    211                         height,
     214                        height - m_localOrigin.getY(),
    212215                        (-m_length/btScalar(2.0)) + y
    213216                        );
    … …  
    219222                        (-m_width/btScalar(2.0)) + x,
    220223                        (-m_length/btScalar(2.0)) + y,
    221                         height
     224                        height - m_localOrigin.getZ()
    222225                        );
    223226                        break;
    … …  
    231234
    232235        vertex*=m_localScaling;
    233        
    234236}
    235237
    … …  
    239241getQuantized
    240242(
    241 float x
     243btScalar x
    242244)
    243245{

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h

    r2662 r2882  
    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.
    3243
    3344  Most (but not all) rendering and heightfield libraries assume upAxis = 1
    … …  
    89100        btVector3       m_localScaling;
    90101
    91         virtual btScalar        getHeightFieldValue(int x,int y) const;
     102        virtual btScalar        getRawHeightFieldValue(int x,int y) const;
    92103        void            quantizeWithClamp(int* out, const btVector3& point,int isMax) const;
    93104        void            getVertex(int x,int y,btVector3& vertex) const;

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp

    r2662 r2882  
    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/trunk/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h

    r2662 r2882  
    3232        BT_DECLARE_ALIGNED_ALLOCATOR();
    3333
    34     btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {}
     34    btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;}
    3535    btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true):
    3636        btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh)
    3737        {
     38            m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
     39
    3840            btVector3 m_triangle[3];
    3941            const unsigned char *vertexbase;
    … …  
    6870        btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh)
    6971        {
     72            m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
     73
    7074            btVector3 m_triangle[3];
    7175            const unsigned char *vertexbase;
    … …  
    108112*/
    109113    }
    110         virtual int     getShapeType() const
    111         {
    112                 return MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
    113         }
    114        
    115114        //debugging
    116115        virtual const char*     getName()const {return "MULTIMATERIALTRIANGLEMESH";}

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h

    r2662 r2882  
    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
    5056        //debugging
    5157        virtual const char*     getName()const {return "SPHERE";}

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp

    r2662 r2882  
    8383}
    8484
    85 void    btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax )
     85
     86void    btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const
    8687{
    8788        m_aabbMin = aabbMin;
    … …  
    9697}
    9798
     99

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h

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

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp

    r2662 r2882  
     1
    12/*
    23Bullet Continuous Collision Detection and Physics Library

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp

    r2662 r2882  
    3636        {
    3737                m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size()/3;
    38                 m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0];
     38                m_indexedMeshes[0].m_triangleIndexBase = 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 = (unsigned char*) &m_16bitIndices[0];
     44                m_indexedMeshes[0].m_triangleIndexBase = 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 = (unsigned char*)&m_4componentVertices[0];
     52                m_indexedMeshes[0].m_vertexBase = 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 = (unsigned char*)&m_3componentVertices[0];
     57                m_indexedMeshes[0].m_vertexBase = 0;
    5858                m_indexedMeshes[0].m_vertexStride = 3*sizeof(btScalar);
    5959        }
    … …  
    112112                        }
    113113        }
    114                 m_3componentVertices.push_back(vertex.getX());
    115                 m_3componentVertices.push_back(vertex.getY());
    116                 m_3componentVertices.push_back(vertex.getZ());
     114                m_3componentVertices.push_back((float)vertex.getX());
     115                m_3componentVertices.push_back((float)vertex.getY());
     116                m_3componentVertices.push_back((float)vertex.getZ());
    117117                m_indexedMeshes[0].m_numVertices++;
    118118                m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];

  • code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h

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

  • code/trunk/src/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp

    r2662 r2882  
    151151        }
    152152
    153         bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
    154         btAssert(!unbal);
     153        btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
    155154
    156155        return splitIndex;

  • code/trunk/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp

    r2662 r2882  
    172172        }
    173173
    174         bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
    175         btAssert(!unbal);
     174        btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
    176175
    177176        return splitIndex;

  • code/trunk/src/bullet/BulletCollision/Gimpact/btGImpactShape.h

    r2662 r2882  
    882882class btGImpactMeshShape : public btGImpactShapeInterface
    883883{
     884        btStridingMeshInterface* m_meshInterface;
     885
    884886protected:
    885887        btAlignedObjectArray<btGImpactMeshShapePart*> m_mesh_parts;
    … …  
    908910        btGImpactMeshShape(btStridingMeshInterface * meshInterface)
    909911        {
     912                m_meshInterface = meshInterface;
    910913                buildMeshParts(meshInterface);
    911914        }
    … …  
    923926
    924927
     928        btStridingMeshInterface* getMeshInterface()
     929        {
     930                return m_meshInterface;
     931        }
     932
     933        const btStridingMeshInterface* getMeshInterface() const
     934        {
     935                return m_meshInterface;
     936        }
    925937
    926938        int getMeshPartCount() const
    … …  
    10341046
    10351047        //! call when reading child shapes
    1036         virtual void lockChildShapes()
    1037         {
    1038                 btAssert(0);
    1039         }
    1040 
    1041         virtual void unlockChildShapes()
     1048        virtual void lockChildShapes() const
     1049        {
     1050                btAssert(0);
     1051        }
     1052
     1053        virtual void unlockChildShapes() const
    10421054        {
    10431055                btAssert(0);

  • code/trunk/src/bullet/BulletCollision/Gimpact/gim_box_set.cpp

    r2662 r2882  
    111111        }
    112112
    113         bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
    114         btAssert(!unbal);
     113        btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
    115114
    116115        return splitIndex;
    … …  
    181180}
    182181
     182

  • code/trunk/src/bullet/BulletCollision/Gimpact/gim_math.h

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

  • code/trunk/src/bullet/BulletCollision/Gimpact/gim_tri_collision.h

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

  • code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp

    r2662 r2882  
    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;
    138141                       
    139142                        dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
    … …  
    197200
    198201                }
    199 
    200                 //don't report time of impact for motion away from the contact normal (or causes minor penetration)
     202       
    201203                if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON)
    202204                        return false;
    203 
     205                       
    204206                result.m_fraction = lambda;
    205207                result.m_normal = n;

  • code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

    r2662 r2882  
    151151                        if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
    152152                        {
    153                                 checkPenetration = false;
     153                                checkSimplex=true;
     154                                //checkPenetration = false;
    154155                                break;
    155156                        }

  • code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

    r2662 r2882  
    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
    115121                       
    116122

  • code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

    r2662 r2882  
    184184               
    185185        }
     186        if (insertIndex<0)
     187                insertIndex=0;
     188
    186189        btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
    187190        m_pointCache[insertIndex] = newPoint;

  • code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

    r2662 r2882  
    5656
    5757        btScalar        m_contactBreakingThreshold;
     58        btScalar        m_contactProcessingThreshold;
    5859
    5960       
    … …  
    7172        btPersistentManifold();
    7273
    73         btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold)
     74        btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold)
    7475                : m_body0(body0),m_body1(body1),m_cachedPoints(0),
    75                 m_contactBreakingThreshold(contactBreakingThreshold)
     76                m_contactBreakingThreshold(contactBreakingThreshold),
     77                m_contactProcessingThreshold(contactProcessingThreshold)
    7678        {
    7779               
    … …  
    112114        ///@todo: get this margin from the current physics / collision environment
    113115        btScalar        getContactBreakingThreshold() const;
     116
     117        btScalar        getContactProcessingThreshold() const
     118        {
     119                return m_contactProcessingThreshold;
     120        }
    114121       
    115122        int getCacheEntry(const btManifoldPoint& newPoint) const;

  • code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp

    r2662 r2882  
    2424#include "btRaycastCallback.h"
    2525
    26 btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to)
     26btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags)
    2727        :
    2828        m_from(from),
    2929        m_to(to),
     30   //@BP Mod
     31   m_flags(flags),
    3032        m_hitFraction(btScalar(1.))
    3133{
    … …  
    5658                return ; // same sign
    5759        }
     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   }
    5866       
    5967        const btScalar proj_length=dist_a-dist_b;
    … …  
    9098                                        if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance)
    9199                                        {
     100                  //@BP Mod
     101                  // Triangle normal isn't normalized
     102                                      triangleNormal.normalize();
    92103
    93                                                 if ( dist_a > 0 )
     104                  //@BP Mod - Allow for unflipped normal when raycasting against backfaces
     105                  if (((m_flags & kF_KeepUnflippedNormal) != 0) || (dist_a <= btScalar(0.0)))
    94106                                                {
    95                                                         m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
     107                                                        m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex);
    96108                                                }
    97109                                                else
    98110                                                {
    99                                                         m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex);
     111                     m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
    100112                                                }
    101113                                        }

  • code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h

    r2662 r2882  
    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
    3243        btScalar        m_hitFraction;
    3344
    34         btTriangleRaycastCallback(const btVector3& from,const btVector3& to);
     45        btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0);
    3546       
    3647        virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);

  • code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp

    r2662 r2882  
    2626
    2727#include "btVoronoiSimplexSolver.h"
    28 #include <assert.h>
    29 //#include <stdio.h>
    3028
    3129#define VERTA  0
    … …  
    3836{
    3937       
    40         assert(m_numVertices>0);
     38        btAssert(m_numVertices>0);
    4139        m_numVertices--;
    4240        m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];

  • code/trunk/src/bullet/BulletDynamics/CMakeLists.txt

    r2710 r2882  
    3636        ConstraintSolver/btTypedConstraint.h
    3737
     38        Dynamics/btActionInterface.h
    3839        Dynamics/btContinuousDynamicsWorld.h
    3940        Dynamics/btDiscreteDynamicsWorld.h

  • code/trunk/src/bullet/BulletDynamics/Character/btCharacterControllerInterface.h

    r2662 r2882  
    1818
    1919#include "LinearMath/btVector3.h"
     20#include "BulletDynamics/Dynamics/btActionInterface.h"
    2021
    2122class btCollisionShape;
    … …  
    2324class btCollisionWorld;
    2425
    25 class btCharacterControllerInterface
     26class btCharacterControllerInterface : public btActionInterface
    2627{
    2728public:

  • code/trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp

    r2662 r2882  
    2323#include "btKinematicCharacterController.h"
    2424
     25static 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
    2527///@todo Interact with dynamic objects,
    2628///Ride kinematicly animated platforms properly
    … …  
    9496}
    9597
    96 btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight)
    97 {
     98btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
     99{
     100        m_upAxis = upAxis;
    98101        m_addedMargin = 0.02f;
    99102        m_walkDirection.setValue(0,0,0);
    … …  
    102105        m_stepHeight = stepHeight;
    103106        m_turnAngle = btScalar(0.0);
    104         m_convexShape=convexShape;
    105        
     107        m_convexShape=convexShape;     
    106108}
    107109
    … …  
    110112}
    111113
    112 
    113114btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
    114115{
    … …  
    116117}
    117118
    118 bool btKinematicCharacterController::recoverFromPenetration (btCollisionWorld* collisionWorld)
     119bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
    119120{
    120121
    … …  
    173174        // phase 1: up
    174175        btTransform start, end;
    175         m_targetPosition = m_currentPosition + btVector3 (btScalar(0.0), m_stepHeight, btScalar(0.0));
     176        m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
    176177
    177178        start.setIdentity ();
    … …  
    179180
    180181        /* FIXME: Handle penetration properly */
    181         start.setOrigin (m_currentPosition + btVector3(btScalar(0.0), btScalar(0.1), btScalar(0.0)));
     182        start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f));
    182183        end.setOrigin (m_targetPosition);
    183184
    … …  
    344345
    345346        // phase 3: down
    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));
     347        btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
     348        btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight;
    348349        m_targetPosition -= (step_drop + gravity_drop);
    349350
    … …  
    390391
    391392
    392 void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
     393void btKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
    393394{
    394395       
    … …  
    413414}
    414415
    415 void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
     416void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
    416417{
    417418        btTransform xform;
    … …  
    469470        return true;
    470471}
     472
     473
     474void    btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
     475{
     476}

  • code/trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.h

    r2662 r2882  
    6363
    6464        bool    m_useGhostObjectSweepTest;
     65
     66        int m_upAxis;
    6567       
    6668        btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
    … …  
    6870        btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
    6971
    70         bool recoverFromPenetration (btCollisionWorld* collisionWorld);
     72        bool recoverFromPenetration ( btCollisionWorld* collisionWorld);
    7173        void stepUp (btCollisionWorld* collisionWorld);
    7274        void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
    … …  
    7476        void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
    7577public:
    76         btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight);
     78        btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
    7779        ~btKinematicCharacterController ();
    7880       
     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
    79101        virtual void    setWalkDirection(const btVector3& walkDirection)
    80102        {
    … …  
    85107        void warp (const btVector3& origin);
    86108
    87         void preStep ( btCollisionWorld* collisionWorld);
    88         void playerStep (btCollisionWorld* collisionWorld, btScalar dt);
     109        void preStep (  btCollisionWorld* collisionWorld);
     110        void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
    89111
    90112        void setFallSpeed (btScalar fallSpeed);

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

    r2662 r2882  
    2323#include <new>
    2424
     25//-----------------------------------------------------------------------------
     26
     27#define CONETWIST_USE_OBSOLETE_SOLVER false
     28#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
     29
     30//-----------------------------------------------------------------------------
     31
    2532btConeTwistConstraint::btConeTwistConstraint()
    26 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
     33:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
     34m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
    2735{
    2836}
    … …  
    3240                                                                                         const btTransform& rbAFrame,const btTransform& rbBFrame)
    3341                                                                                         :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
    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_angularOnly(false),
     43                                                                                         m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
     44{
     45        init();
     46}
     47
     48btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
     49                                                                                        :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
     50                                                                                         m_angularOnly(false),
     51                                                                                         m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
     52{
     53        m_rbBFrame = m_rbAFrame;
     54        init();
     55}
     56
     57
     58void btConeTwistConstraint::init()
     59{
     60        m_angularOnly = false;
    4261        m_solveTwistLimit = false;
    4362        m_solveSwingLimit = false;
    44 
    45 }
    46 
    47 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
    48                                                                                         :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
    49                                                                                          m_angularOnly(false)
    50 {
    51         m_rbBFrame = m_rbAFrame;
     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
     74void 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()
    52103       
    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 
    59         m_solveTwistLimit = false;
    60         m_solveSwingLimit = false;
     104//-----------------------------------------------------------------------------
     105
     106void 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;
     176                }
     177                else
     178                {
     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}
    61232       
    62 }                       
     233//-----------------------------------------------------------------------------
    63234
    64235void    btConeTwistConstraint::buildJacobian()
    65236{
    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();
    86                 }
    87                 else
    88                 {
    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(
     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(
    97264                                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    98265                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    104271                                m_rbB.getInvInertiaDiagLocal(),
    105272                                m_rbB.getInvMass());
    106                 }
    107         }
     273                        }
     274                }
     275
     276                calcAngleInfo2();
     277        }
     278}
     279
     280//-----------------------------------------------------------------------------
     281
     282void    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
     495void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
     496{
     497        (void)timeStep;
     498
     499}
     500
     501//-----------------------------------------------------------------------------
     502
     503void btConeTwistConstraint::calcAngleInfo()
     504{
     505        m_swingCorrection = btScalar(0.);
     506        m_twistLimitSign = btScalar(0.);
     507        m_solveTwistLimit = false;
     508        m_solveSwingLimit = false;
    108509
    109510        btVector3 b1Axis1,b1Axis2,b1Axis3;
    … …  
    123524        {
    124525                b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
    125 //              swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
    126526                swx = b2Axis1.dot(b1Axis1);
    127527                swy = b2Axis1.dot(b1Axis2);
    … …  
    130530                fact = fact / (fact + btScalar(1.0));
    131531                swing1 *= fact;
    132 
    133532        }
    134533
    … …  
    136535        {
    137536                b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);                     
    138 //              swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
    139537                swx = b2Axis1.dot(b1Axis1);
    140538                swy = b2Axis1.dot(b1Axis3);
    … …  
    153551                m_swingCorrection = EllipseAngle-1.0f;
    154552                m_solveSwingLimit = true;
    155                
    156553                // Calculate necessary axis & factors
    157554                m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
    158555                m_swingAxis.normalize();
    159 
    160556                btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
    161557                m_swingAxis *= swingAxisSign;
    162 
    163                 m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
    164                         getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
    165 
    166558        }
    167559
    … …  
    173565                btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
    174566                btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
    175 
    176                 btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
     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.);
    177571                if (twist <= -m_twistSpan*lockedFreeFactor)
    178572                {
    179573                        m_twistCorrection = -(twist + m_twistSpan);
    180574                        m_solveTwistLimit = true;
    181 
    182575                        m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
    183576                        m_twistAxis.normalize();
    184577                        m_twistAxis *= -1.0f;
    185 
    186                         m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
    187                                 getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
    188 
    189                 }       else
    190                         if (twist >  m_twistSpan*lockedFreeFactor)
    191                         {
    192                                 m_twistCorrection = (twist - m_twistSpan);
     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
     590static btVector3 vTwist(1,0,0); // twist axis in constraint's space
     591
     592//-----------------------------------------------------------------------------
     593
     594void 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)
     619                        {
     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));
     646                        }
     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                        {
    193732                                m_solveTwistLimit = true;
    194733
    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 
    201                         }
    202         }
    203 }
    204 
    205 void    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 
    285 void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
    286 {
    287         (void)timeStep;
    288 
    289 }
     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".
     766void 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
     829btVector3 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.
     862void 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
     886void 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
     920void 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
     934void 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

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

    r2662 r2882  
    4343        btScalar        m_relaxationFactor;
    4444
     45        btScalar        m_damping;
     46
    4547        btScalar        m_swingSpan1;
    4648        btScalar        m_swingSpan2;
    4749        btScalar        m_twistSpan;
    4850
     51        btScalar        m_fixThresh;
     52
    4953        btVector3   m_swingAxis;
    5054        btVector3       m_twistAxis;
    … …  
    5761        btScalar        m_twistCorrection;
    5862
     63        btScalar        m_twistAngle;
     64
    5965        btScalar        m_accSwingLimitImpulse;
    6066        btScalar        m_accTwistLimitImpulse;
    … …  
    6470        bool            m_solveSwingLimit;
    6571
     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;
    6685       
    6786public:
    … …  
    7594        virtual void    buildJacobian();
    7695
    77         virtual void    solveConstraint(btScalar        timeStep);
     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);
    78102
    79103        void    updateRHS(btScalar      timeStep);
    … …  
    93117        }
    94118
    95         void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
     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)
    96145        {
    97146                m_swingSpan1 = _swingSpan1;
    … …  
    122171        }
    123172
     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
     217protected:
     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;
    124227};
    125228

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

    r2662 r2882  
    2323#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
    2424
    25 #define ASSERT2 assert
     25#define ASSERT2 btAssert
    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         assert(cpd);
     117        btAssert(cpd);
    118118        btScalar distance = cpd->m_penetration;
    119119        btScalar positionalError = Kcor *-distance;
    … …  
    167167       
    168168        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    169         assert(cpd);
     169        btAssert(cpd);
    170170
    171171        btScalar combinedFriction = cpd->m_friction;
    … …  
    256256       
    257257        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    258         assert(cpd);
     258        btAssert(cpd);
    259259
    260260        btScalar combinedFriction = cpd->m_friction;
    … …  
    338338
    339339        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
    340         assert(cpd);
     340        btAssert(cpd);
    341341        btScalar distance = cpd->m_penetration;
    342342        btScalar positionalError = Kcor *-distance;

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

    r2662 r2882  
    2323        SOLVER_USE_WARMSTARTING = 4,
    2424        SOLVER_USE_FRICTION_WARMSTARTING = 8,
    25         SOLVER_CACHE_FRIENDLY = 16
     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.
    2631};
    2732
    … …  
    4045        btScalar        m_erp;//used as Baumgarte factor
    4146        btScalar        m_erp2;//used in Split Impulse
     47        btScalar        m_globalCfm;//constraint force mixing
    4248        int                     m_splitImpulse;
    4349        btScalar        m_splitImpulsePenetrationThreshold;
    … …  
    6672                m_erp = btScalar(0.2);
    6773                m_erp2 = btScalar(0.1);
    68                 m_sor = btScalar(1.3);
     74                m_globalCfm = btScalar(0.);
     75                m_sor = btScalar(1.);
    6976                m_splitImpulse = false;
    7077                m_splitImpulsePenetrationThreshold = -0.02f;
    7178                m_linearSlop = btScalar(0.0);
    7279                m_warmstartingFactor=btScalar(0.85);
    73                 m_solverMode = SOLVER_CACHE_FRIENDLY |  SOLVER_RANDMIZE_ORDER |  SOLVER_USE_WARMSTARTING;
     80                m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER
    7481                m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
    7582        }

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

    r2662 r2882  
    2020*/
    2121
    22 
    2322#include "btGeneric6DofConstraint.h"
    2423#include "BulletDynamics/Dynamics/btRigidBody.h"
    … …  
    2726
    2827
     28#define D6_USE_OBSOLETE_METHOD false
     29//-----------------------------------------------------------------------------
     30
     31btGeneric6DofConstraint::btGeneric6DofConstraint()
     32:btTypedConstraint(D6_CONSTRAINT_TYPE),
     33m_useLinearReferenceFrameA(true),
     34m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
     35{
     36}
     37
     38//-----------------------------------------------------------------------------
     39
     40btGeneric6DofConstraint::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),
     44m_useLinearReferenceFrameA(useLinearReferenceFrameA),
     45m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
     46{
     47
     48}
     49//-----------------------------------------------------------------------------
     50
     51
    2952#define GENERIC_D6_DISABLE_WARMSTARTING 1
     53
     54//-----------------------------------------------------------------------------
    3055
    3156btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
    … …  
    3762}
    3863
     64//-----------------------------------------------------------------------------
     65
    3966///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
    4067bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
    4168bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
    4269{
    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))
    49                 {
    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                         }
     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))
     79                {
     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;
    6584                }
    6685                else
    6786                {
    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 
    73                 }
    74 
    75 
     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;
     92                }
     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        }
    76101        return false;
    77102}
    78103
    79 
    80 
    81104//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
    82 
    83105
    84106int btRotationalLimitMotor::testLimitValue(btScalar test_value)
    … …  
    105127        m_currentLimit = 0;//Free from violation
    106128        return 0;
    107        
    108 }
    109 
     129
     130}
     131
     132//-----------------------------------------------------------------------------
    110133
    111134btScalar btRotationalLimitMotor::solveAngularLimits(
    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;
     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;
    119142
    120143        //current error correction
    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);
     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);
    139165
    140166        // correction velocity
    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     }
     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        }
    148174
    149175
    150176        // correction impulse
    151     btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
     177        btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
    152178
    153179        // clip correction impulse
    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     }
     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        }
    165191
    166192
    167193        // sort with accumulated impulses
    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;
     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;
    186213
    187214
    … …  
    190217//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
    191218
     219
     220
     221
    192222//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
     223
     224
     225int 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
    193256btScalar btTranslationalLimitMotor::solveLinearAxis(
    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;
     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;
    266340}
    267341
    268342//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
    269343
    270 
    271 btGeneric6DofConstraint::btGeneric6DofConstraint()
    272         :btTypedConstraint(D6_CONSTRAINT_TYPE),
    273                 m_useLinearReferenceFrameA(true)
    274 {
    275 }
    276 
    277 btGeneric6DofConstraint::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 
    290344void btGeneric6DofConstraint::calculateAngleInfo()
    291345{
    292346        btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
    293 
    294347        matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
    295 
    296 
    297 
    298348        // in euler angle mode we do not actually constrain the angular velocity
    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 
     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.
    313362        btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
    314363        btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
    … …  
    318367        m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
    319368
    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 }
     369        m_calculatedAxis[0].normalize();
     370        m_calculatedAxis[1].normalize();
     371        m_calculatedAxis[2].normalize();
     372
     373}
     374
     375//-----------------------------------------------------------------------------
    333376
    334377void btGeneric6DofConstraint::calculateTransforms()
    335378{
    336     m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
    337     m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
    338 
    339     calculateAngleInfo();
    340 }
    341 
     379        m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
     380        m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
     381        calculateLinearInfo();
     382        calculateAngleInfo();
     383}
     384
     385//-----------------------------------------------------------------------------
    342386
    343387void btGeneric6DofConstraint::buildLinearJacobian(
    344     btJacobianEntry & jacLinear,const btVector3 & normalWorld,
    345     const btVector3 & pivotAInW,const btVector3 & pivotBInW)
    346 {
    347     new (&jacLinear) btJacobianEntry(
     388        btJacobianEntry & jacLinear,const btVector3 & normalWorld,
     389        const btVector3 & pivotAInW,const btVector3 & pivotBInW)
     390{
     391        new (&jacLinear) btJacobianEntry(
    348392        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    349393        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    355399        m_rbB.getInvInertiaDiagLocal(),
    356400        m_rbB.getInvMass());
    357 
    358 }
     401}
     402
     403//-----------------------------------------------------------------------------
    359404
    360405void btGeneric6DofConstraint::buildAngularJacobian(
    361     btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
    362 {
    363     new (&jacAngular)   btJacobianEntry(jointAxisW,
     406        btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
     407{
     408        new (&jacAngular)      btJacobianEntry(jointAxisW,
    364409                                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    365410                                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    369414}
    370415
     416//-----------------------------------------------------------------------------
     417
    371418bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
    372419{
    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 }
     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//-----------------------------------------------------------------------------
    379427
    380428void btGeneric6DofConstraint::buildJacobian()
    381429{
    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 
    437 void 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 }
     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
     488void 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
     524void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
     525{
     526        btAssert(!m_useSolveConstraintObsolete);
     527        int row = setLinearLimits(info);
     528        setAngularLimits(info, row);
     529}
     530
     531//-----------------------------------------------------------------------------
     532
     533int 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
     564int 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
     587void 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//-----------------------------------------------------------------------------
    490647
    491648void    btGeneric6DofConstraint::updateRHS(btScalar     timeStep)
    492649{
    493     (void)timeStep;
    494 
    495 }
     650        (void)timeStep;
     651
     652}
     653
     654//-----------------------------------------------------------------------------
    496655
    497656btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
    498657{
    499     return m_calculatedAxis[axis_index];
    500 }
     658        return m_calculatedAxis[axis_index];
     659}
     660
     661//-----------------------------------------------------------------------------
    501662
    502663btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
    503664{
    504     return m_calculatedAxisAngleDiff[axis_index];
    505 }
     665        return m_calculatedAxisAngleDiff[axis_index];
     666}
     667
     668//-----------------------------------------------------------------------------
    506669
    507670void btGeneric6DofConstraint::calcAnchorPos(void)
    … …  
    524687} // btGeneric6DofConstraint::calcAnchorPos()
    525688
     689//-----------------------------------------------------------------------------
     690
     691void 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
     703int 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/trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

    r2662 r2882  
    2929
    3030class btRigidBody;
     31
     32
    3133
    3234
    … …  
    9395    bool isLimited()
    9496    {
    95         if(m_loLimit>=m_hiLimit) return false;
     97        if(m_loLimit > m_hiLimit) return false;
    9698        return true;
    9799    }
    … …  
    111113
    112114        //! apply the correction impulses for two bodies
    113     btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
    114 
     115    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
    115116
    116117};
    … …  
    130131    btScalar    m_restitution;//! Bounce parameter for linear limit
    131132    //!@}
     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
    132138
    133139    btTranslationalLimitMotor()
    … …  
    140146        m_damping = btScalar(1.0f);
    141147        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                }
    142154    }
    143155
    … …  
    151163        m_damping = other.m_damping;
    152164        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                }
    153171    }
    154172
    … …  
    164182       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
    165183    }
     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);
    166190
    167191
    … …  
    169193        btScalar timeStep,
    170194        btScalar jacDiagABInv,
    171         btRigidBody& body1,const btVector3 &pointInA,
    172         btRigidBody& body2,const btVector3 &pointInB,
     195        btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
     196        btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
    173197        int limit_index,
    174198        const btVector3 & axis_normal_on_a,
    … …  
    248272    btVector3 m_calculatedAxisAngleDiff;
    249273    btVector3 m_calculatedAxis[3];
     274    btVector3 m_calculatedLinearDiff;
    250275   
    251276        btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
    … …  
    263288
    264289
     290        int setAngularLimits(btConstraintInfo2 *info, int row_offset);
     291
     292        int setLinearLimits(btConstraintInfo2 *info);
    265293
    266294    void buildLinearJacobian(
    … …  
    270298    void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
    271299
     300        // tests linear limits
     301        void calculateLinearInfo();
    272302
    273303        //! calcs the euler angles between the two bodies.
    … …  
    277307
    278308public:
     309
     310        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
     311        bool            m_useSolveConstraintObsolete;
     312
    279313    btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
    280314
    … …  
    331365    virtual void        buildJacobian();
    332366
    333     virtual     void    solveConstraint(btScalar        timeStep);
     367        virtual void getInfo1 (btConstraintInfo1* info);
     368
     369        virtual void getInfo2 (btConstraintInfo2* info);
     370
     371    virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
    334372
    335373    void        updateRHS(btScalar      timeStep);
    … …  
    433471        virtual void calcAnchorPos(void); // overridable
    434472
     473        int get_limit_motor_info2(      btRotationalLimitMotor * limot,
     474                                                                btRigidBody * body0, btRigidBody * body1,
     475                                                                btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
     476
     477
    435478};
    436479

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

    r2662 r2882  
    2020#include "LinearMath/btMinMax.h"
    2121#include <new>
     22#include "btSolverBody.h"
     23
     24//-----------------------------------------------------------------------------
     25
     26#define HINGE_USE_OBSOLETE_SOLVER false
     27
     28//-----------------------------------------------------------------------------
    2229
    2330
    2431btHingeConstraint::btHingeConstraint()
    2532: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
    26 m_enableAngularMotor(false)
    27 {
    28 }
     33m_enableAngularMotor(false),
     34m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
     35m_useReferenceFrameA(false)
     36{
     37        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
     38}
     39
     40//-----------------------------------------------------------------------------
    2941
    3042btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
    31                                                                          btVector3& axisInA,btVector3& axisInB)
     43                                                                         btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
    3244                                                                         :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
    3345                                                                         m_angularOnly(false),
    34                                                                          m_enableAngularMotor(false)
     46                                                                         m_enableAngularMotor(false),
     47                                                                         m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
     48                                                                         m_useReferenceFrameA(useReferenceFrameA)
    3549{
    3650        m_rbAFrame.getOrigin() = pivotInA;
    … …  
    6175       
    6276        m_rbBFrame.getOrigin() = pivotInB;
    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() );
     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() );
    6680       
    6781        //start with free
    … …  
    7286        m_limitSoftness = 0.9f;
    7387        m_solveLimit = false;
    74 
    75 }
    76 
    77 
    78 
    79 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
    80 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
     88        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
     89}
     90
     91//-----------------------------------------------------------------------------
     92
     93btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
     94:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
     95m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
     96m_useReferenceFrameA(useReferenceFrameA)
    8197{
    8298
    … …  
    91107                                                                        rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
    92108
    93         btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
     109        btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
    94110
    95111        btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
    … …  
    110126        m_limitSoftness = 0.9f;
    111127        m_solveLimit = false;
    112 }
     128        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
     129}
     130
     131//-----------------------------------------------------------------------------
    113132
    114133btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
    115                                                                      const btTransform& rbAFrame, const btTransform& rbBFrame)
     134                                                                     const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
    116135:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
    117136m_angularOnly(false),
    118 m_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 
     137m_enableAngularMotor(false),
     138m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
     139m_useReferenceFrameA(useReferenceFrameA)
     140{
    125141        //start with free
    126142        m_lowerLimit = btScalar(1e30);
    … …  
    130146        m_limitSoftness = 0.9f;
    131147        m_solveLimit = false;
     148        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
    132149}                       
    133150
    134 
    135 
    136 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
     151//-----------------------------------------------------------------------------
     152
     153btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
    137154:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
    138155m_angularOnly(false),
    139 m_enableAngularMotor(false)
     156m_enableAngularMotor(false),
     157m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
     158m_useReferenceFrameA(useReferenceFrameA)
    140159{
    141160        ///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.);
    147161
    148162        m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
    … …  
    155169        m_limitSoftness = 0.9f;
    156170        m_solveLimit = false;
    157 }
     171        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
     172}
     173
     174//-----------------------------------------------------------------------------
    158175
    159176void    btHingeConstraint::buildJacobian()
    160177{
    161         m_appliedImpulse = btScalar(0.);
    162 
    163         if (!m_angularOnly)
    164         {
    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(
     178        if (m_useSolveConstraintObsolete)
     179        {
     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(
    184203                                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    185204                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    191210                                m_rbB.getInvInertiaDiagLocal(),
    192211                                m_rbB.getInvMass());
     212                        }
    193213                }
    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);
     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;
    209221               
    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 
     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);
     228                       
     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
     258        }
     259}
     260
     261//-----------------------------------------------------------------------------
     262
     263void 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
     286void 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
     472void    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//-----------------------------------------------------------------------------
     619
     620void    btHingeConstraint::updateRHS(btScalar   timeStep)
     621{
     622        (void)timeStep;
     623
     624}
     625
     626//-----------------------------------------------------------------------------
     627
     628btScalar btHingeConstraint::getHingeAngle()
     629{
     630        const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
     631        const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
     632        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
     639void btHingeConstraint::testLimit()
     640{
    229641        // Compute limit information
    230         btScalar hingeAngle = getHingeAngle(); 
    231 
    232         //set bias, sign, clear accumulator
     642        m_hingeAngle = getHingeAngle(); 
    233643        m_correction = btScalar(0.);
    234644        m_limitSign = btScalar(0.);
    235645        m_solveLimit = false;
    236         m_accLimitImpulse = btScalar(0.);
    237 
    238 //      if (m_lowerLimit < m_upperLimit)
    239646        if (m_lowerLimit <= m_upperLimit)
    240647        {
    241 //              if (hingeAngle <= m_lowerLimit*m_limitSoftness)
    242                 if (hingeAngle <= m_lowerLimit)
    243                 {
    244                         m_correction = (m_lowerLimit - hingeAngle);
     648                if (m_hingeAngle <= m_lowerLimit)
     649                {
     650                        m_correction = (m_lowerLimit - m_hingeAngle);
    245651                        m_limitSign = 1.0f;
    246652                        m_solveLimit = true;
    247653                }
    248 //              else if (hingeAngle >= m_upperLimit*m_limitSoftness)
    249                 else if (hingeAngle >= m_upperLimit)
    250                 {
    251                         m_correction = m_upperLimit - hingeAngle;
     654                else if (m_hingeAngle >= m_upperLimit)
     655                {
     656                        m_correction = m_upperLimit - m_hingeAngle;
    252657                        m_limitSign = -1.0f;
    253658                        m_solveLimit = true;
    254659                }
    255660        }
    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 
    264 void    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;
    327                         }
    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);
    382                        
    383                 }
    384         }
    385 
    386 }
    387 
    388 void    btHingeConstraint::updateRHS(btScalar   timeStep)
    389 {
    390         (void)timeStep;
    391 
    392 }
    393 
    394 btScalar btHingeConstraint::getHingeAngle()
    395 {
    396         const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
    397         const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
    398         const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
    399 
    400         return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
    401 }
    402 
     661        return;
     662} // btHingeConstraint::testLimit()
     663
     664//-----------------------------------------------------------------------------
     665//-----------------------------------------------------------------------------
     666//-----------------------------------------------------------------------------

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h

    r2662 r2882  
    5454
    5555        btScalar        m_accLimitImpulse;
     56        btScalar        m_hingeAngle;
     57        btScalar    m_referenceSign;
    5658
    5759        bool            m_angularOnly;
    5860        bool            m_enableAngularMotor;
    5961        bool            m_solveLimit;
     62        bool            m_useSolveConstraintObsolete;
     63        bool            m_useReferenceFrameA;
    6064
    6165       
    6266public:
    6367
    64         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
     68        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false);
    6569
    66         btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
     70        btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
    6771       
    68         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
     72        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
    6973
    70         btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
     74        btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
    7175
    7276        btHingeConstraint();
    … …  
    7478        virtual void    buildJacobian();
    7579
    76         virtual void    solveConstraint(btScalar        timeStep);
     80        virtual void getInfo1 (btConstraintInfo1* info);
     81
     82        virtual void getInfo2 (btConstraintInfo2* info);
     83       
     84        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
    7785
    7886        void    updateRHS(btScalar      timeStep);
    … …  
    8795        }
    8896
     97        btRigidBody& getRigidBodyA()   
     98        {               
     99                return m_rbA;   
     100        }       
     101
     102        btRigidBody& getRigidBodyB()   
     103        {               
     104                return m_rbB;   
     105        }       
     106       
    89107        void    setAngularOnly(bool angularOnly)
    90108        {
    … …  
    123141        btScalar getHingeAngle();
    124142
     143        void testLimit();
     144
    125145
    126146        const btTransform& getAFrame() { return m_rbAFrame; }; 

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

    r2662 r2882  
    2222
    2323btPoint2PointConstraint::btPoint2PointConstraint()
    24 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
     24:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
     25m_useSolveConstraintObsolete(false)
    2526{
    2627}
    2728
    2829btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
    29 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
     30:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
     31m_useSolveConstraintObsolete(false)
    3032{
    3133
    … …  
    3436
    3537btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
    36 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
     38:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
     39m_useSolveConstraintObsolete(false)
    3740{
    3841       
    … …  
    4144void    btPoint2PointConstraint::buildJacobian()
    4245{
    43         m_appliedImpulse = btScalar(0.);
    44 
    45         btVector3       normal(0,0,0);
    46 
    47         for (int i=0;i<3;i++)
    48         {
    49                 normal[i] = 1;
    50                 new (&m_jac[i]) btJacobianEntry(
     46        ///we need it for both methods
     47        {
     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(
    5156                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
    5257                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
    … …  
    5964                        m_rbB.getInvMass());
    6065                normal[i] = 0;
    61         }
    62 
    63 }
    64 
    65 void    btPoint2PointConstraint::solveConstraint(btScalar       timeStep)
    66 {
    67         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
    68         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
    69 
    70 
    71         btVector3 normal(0,0,0);
    72        
    73 
    74 //      btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
    75 //      btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
    76 
    77         for (int i=0;i<3;i++)
    78         {               
    79                 normal[i] = 1;
    80                 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
    81 
    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);
     66                }
     67        }
     68
     69}
     70
     71
     72void 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}
     84
     85void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
     86{
     87        btAssert(!m_useSolveConstraintObsolete);
     88
     89         //retrieve matrices
     90        btTransform body0_trans;
     91        body0_trans = m_rbA.getCenterOfMassTransform();
     92    btTransform body1_trans;
     93        body1_trans = m_rbB.getCenterOfMassTransform();
     94
     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;
     101
     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;
    97114        */
    98115       
    99                 //positional error (zeroth order error)
    100                 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
     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   
     126
     127
     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
     151void    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);
    101160               
    102                 btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
    103 
    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;
     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;
    111221                }
    112 
    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());
    117                
    118                 normal[i] = 0;
    119222        }
    120223}

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h

    r2662 r2882  
    5151public:
    5252
     53        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
     54        bool            m_useSolveConstraintObsolete;
     55
    5356        btConstraintSetting     m_setting;
    5457
    … …  
    6164        virtual void    buildJacobian();
    6265
     66        virtual void getInfo1 (btConstraintInfo1* info);
    6367
    64         virtual void    solveConstraint(btScalar        timeStep);
     68        virtual void getInfo2 (btConstraintInfo2* info);
     69
     70
     71        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
    6572
    6673        void    updateRHS(btScalar      timeStep);

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

    r2662 r2882  
    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
    1918
    2019#include "btSequentialImpulseConstraintSolver.h"
    … …  
    3332#include "btSolverBody.h"
    3433#include "btSolverConstraint.h"
    35 
    36 
    3734#include "LinearMath/btAlignedObjectArray.h"
    38 
    39 
    40 int totalCpd = 0;
    41 
    42 int     gTotalContactPoints = 0;
    43 
    44 struct  btOrderIndex
    45 {
    46         int     m_manifoldIndex;
    47         int     m_pointIndex;
    48 };
    49 
    50 
    51 
    52 #define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
    53 static btOrderIndex     gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
     35#include <string.h> //for memset
     36
     37btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
     38:m_btSeed2(0)
     39{
     40
     41}
     42
     43btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
     44{
     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))
     50static 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
     58void 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
    54178
    55179
    56180unsigned long btSequentialImpulseConstraintSolver::btRand2()
    57181{
    58   m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
    59   return m_btSeed2;
     182        m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
     183        return m_btSeed2;
    60184}
    61185
    … …  
    65189int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
    66190{
    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 
    95 bool  MyContactDestroyedCallback(void* userPersistentData);
    96 bool  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 
    107 
    108 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
    109 :m_btSeed2(0)
    110 {
    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                 }
    122 }
    123 
    124 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
    125 {
    126 
    127 }
    128 
    129 void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
    130 void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
    131 {
    132         btRigidBody* rb = btRigidBody::upcast(collisionObject);
     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
     218void    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
    133225        if (rb)
    134226        {
    135                 solverBody->m_angularVelocity = rb->getAngularVelocity() ;
    136                 solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
    137                 solverBody->m_friction = collisionObject->getFriction();
    138227                solverBody->m_invMass = rb->getInvMass();
    139                 solverBody->m_linearVelocity = rb->getLinearVelocity();
    140228                solverBody->m_originalBody = rb;
    141229                solverBody->m_angularFactor = rb->getAngularFactor();
    142230        } else
    143231        {
    144                 solverBody->m_angularVelocity.setValue(0,0,0);
    145                 solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
    146                 solverBody->m_friction = collisionObject->getFriction();
    147232                solverBody->m_invMass = 0.f;
    148                 solverBody->m_linearVelocity.setValue(0,0,0);
    149233                solverBody->m_originalBody = 0;
    150234                solverBody->m_angularFactor = 1.f;
    151235        }
    152         solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
    153         solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
    154236}
    155237
    … …  
    157239int             gNumSplitImpulseRecoveries = 0;
    158240
    159 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
    160 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
     241btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
    161242{
    162243        btScalar rest = restitution * -rel_vel;
    … …  
    165246
    166247
    167 void    resolveSplitPenetrationImpulseCacheFriendly(
    168         btSolverBody& body1,
    169         btSolverBody& body2,
    170         const btSolverConstraint& contactConstraint,
    171         const btContactSolverInfo& solverInfo);
    172 
    173 //SIMD_FORCE_INLINE
    174 void    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 
    231 btScalar resolveSingleCollisionCombinedCacheFriendly(
    232         btSolverBody& body1,
    233         btSolverBody& body2,
    234         const btSolverConstraint& contactConstraint,
    235         const btContactSolverInfo& solverInfo);
    236 
    237 //SIMD_FORCE_INLINE
    238 btScalar 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 
    298 btScalar resolveSingleFrictionCacheFriendly(
    299         btSolverBody& body1,
    300         btSolverBody& body2,
    301         const btSolverConstraint& contactConstraint,
    302         const btContactSolverInfo& solverInfo,
    303         btScalar appliedNormalImpulse);
    304 
    305 //SIMD_FORCE_INLINE
    306 btScalar 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
    382 btScalar 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 
     248
     249void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
     250void    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}
    437263
    438264
    … …  
    440266btSolverConstraint&     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)
    441267{
     268
    442269
    443270        btRigidBody* body0=btRigidBody::upcast(colObj0);
    444271        btRigidBody* body1=btRigidBody::upcast(colObj1);
    445272
    446         btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
     273        btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
     274        memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
    447275        solverConstraint.m_contactNormal = normalAxis;
    448276
    449277        solverConstraint.m_solverBodyIdA = solverBodyIdA;
    450278        solverConstraint.m_solverBodyIdB = solverBodyIdB;
    451         solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
    452279        solverConstraint.m_frictionIndex = frictionIndex;
    453280
    … …  
    455282        solverConstraint.m_originalContactPoint = 0;
    456283
    457         solverConstraint.m_appliedImpulse = btScalar(0.);
    458         solverConstraint.m_appliedPushImpulse = 0.f;
    459         solverConstraint.m_penetration = 0.f;
     284        solverConstraint.m_appliedImpulse = 0.f;
     285        //      solverConstraint.m_appliedPushImpulse = 0.f;
     286
    460287        {
    461288                btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
    462289                solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
    463                 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
    464         }
    465         {
    466                 btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
     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);
    467294                solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
    468                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
     295                solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
    469296        }
    470297
    … …  
    483310        if (body1)
    484311        {
    485                 vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
     312                vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
    486313                denom1 = body1->getInvMass() + normalAxis.dot(vec);
    487314        }
    … …  
    492319        solverConstraint.m_jacDiagABInv = denom;
    493320
     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
    494350        return solverConstraint;
    495351}
    496352
     353int     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
     381void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
     382{
     383        btCollisionObject* colObj0=0,*colObj1=0;
     384
     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)
     449                                        {
     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)
     544                                                {
     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;
     558                                                } else
     559                                                {
     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                                                        }
     613                                                }
     614                                        } else
     615                                        {
     616                                                btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
     617                                                frictionConstraint1.m_appliedImpulse = 0.f;
     618                                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
     619                                                {
     620                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
     621                                                        frictionConstraint2.m_appliedImpulse = 0.f;
     622                                                }
     623                                        }
     624                                }
     625                        }
     626
     627
     628                }
     629        }
     630}
    497631
    498632
    … …  
    506640        if (!(numConstraints + numManifolds))
    507641        {
    508 //              printf("empty\n");
     642                //              printf("empty\n");
    509643                return 0.f;
    510644        }
    511         btPersistentManifold* manifold = 0;
    512         btCollisionObject* colObj0=0,*colObj1=0;
    513 
    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)
    600                                         {
    601                                                 if (colObj0->getCompanionId() >= 0)
    602                                                 {
    603                                                         //body has already been converted
    604                                                         solverBodyIdA = colObj0->getCompanionId();
    605                                                 } else
    606                                                 {
    607                                                         solverBodyIdA = m_tmpSolverBodyPool.size();
    608                                                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
    609                                                         initSolverBody(&solverBody,colObj0);
    610                                                         colObj0->setCompanionId(solverBodyIdA);
    611                                                 }
    612                                         } else
    613                                         {
    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)
    623                                                 {
    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);
    631                                                 }
    632                                         } else
    633                                         {
    634                                                 //create a static body
    635                                                 solverBodyIdB = m_tmpSolverBodyPool.size();
    636                                                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
    637                                                 initSolverBody(&solverBody,colObj1);
    638                                         }
    639                                 }
    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 
     645
     646        if (1)
    839647        {
    840648                int j;
    … …  
    845653                }
    846654        }
    847        
    848        
    849 
    850         int numConstraintPool = m_tmpSolverConstraintPool.size();
    851         int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
     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();
    852812
    853813        ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
    … …  
    866826        }
    867827
    868 
    869 
    870828        return 0.f;
    871829
    … …  
    875833{
    876834        BT_PROFILE("solveGroupCacheFriendlyIterations");
    877         int numConstraintPool = m_tmpSolverConstraintPool.size();
    878         int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
     835
     836        int numConstraintPool = m_tmpSolverContactConstraintPool.size();
     837        int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
    879838
    880839        //should traverse the contacts random order...
    … …  
    904863                        }
    905864
    906                         for (j=0;j<numConstraints;j++)
     865                        if (infoGlobal.m_solverMode & SOLVER_SIMD)
    907866                        {
    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 
     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();
     885                                for (j=0;j<numPoolConstraints;j++)
     886                                {
     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))
     899                                        {
     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);
     904                                        }
     905                                }
     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                                }
    931948                        }
    932949
    933                         {
    934                                 int numPoolConstraints = m_tmpSolverConstraintPool.size();
    935                                 for (j=0;j<numPoolConstraints;j++)
    936                                 {
    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                 }
     950
     951
     952                }
     953        }
     954        return 0.f;
     955}
     956
     957
     958
     959/// btSequentialImpulseConstraintSolver Sequentially applies impulses
     960btScalar 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
    962963       
    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++)
    972                                         {
    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);
    977                                         }
    978                                 }
    979                         }
    980 
    981                 }
    982 
    983         }
    984 
    985         return 0.f;
    986 }
    987 
    988 
    989 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
    990 {
     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
    991971        int i;
    992972
    … …  
    994974        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
    995975
    996         int numPoolConstraints = m_tmpSolverConstraintPool.size();
     976        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
    997977        int j;
     978
    998979        for (j=0;j<numPoolConstraints;j++)
    999980        {
    1000                
    1001                 const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
     981
     982                const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
    1002983                btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
    1003984                btAssert(pt);
    … …  
    1005986                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    1006987                {
    1007                         pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    1008                         pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
     988                        pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
     989                        pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
    1009990                }
    1010991
    1011992                //do a callback here?
    1012 
    1013993        }
    1014994
    … …  
    10221002        {
    10231003                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
    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 */
     1004                {
     1005                        m_tmpSolverBodyPool[i].writebackVelocity();
     1006                }
     1007        }
     1008
    10411009
    10421010        m_tmpSolverBodyPool.resize(0);
    1043         m_tmpSolverConstraintPool.resize(0);
    1044         m_tmpSolverFrictionConstraintPool.resize(0);
    1045 
     1011        m_tmpSolverContactConstraintPool.resize(0);
     1012        m_tmpSolverNonContactConstraintPool.resize(0);
     1013        m_tmpSolverContactFrictionConstraintPool.resize(0);
    10461014
    10471015        return 0.f;
    10481016}
    10491017
    1050 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
    1051 btScalar 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 
    1153 void    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 
    1347 btScalar 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 
    1380 btScalar 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 
    1412 btScalar 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 }
     1018
     1019
     1020
     1021
     1022
     1023
    14411024
    14421025

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

    r2662 r2882  
    2424
    2525
    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
     26
     27///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
    3028class btSequentialImpulseConstraintSolver : public btConstraintSolver
    3129{
     30protected:
    3231
    3332        btAlignedObjectArray<btSolverBody>      m_tmpSolverBodyPool;
    34         btAlignedObjectArray<btSolverConstraint>        m_tmpSolverConstraintPool;
    35         btAlignedObjectArray<btSolverConstraint>        m_tmpSolverFrictionConstraintPool;
     33        btConstraintArray                       m_tmpSolverContactConstraintPool;
     34        btConstraintArray                       m_tmpSolverNonContactConstraintPool;
     35        btConstraintArray                       m_tmpSolverContactFrictionConstraintPool;
    3636        btAlignedObjectArray<int>       m_orderTmpConstraintPool;
    3737        btAlignedObjectArray<int>       m_orderFrictionConstraintPool;
    3838
    39 
    40 protected:
    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);
    4439        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 
    4940       
    5041        ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
    5142        unsigned long   m_btSeed2;
    5243
     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               
    5366public:
    5467
    5568       
    5669        btSequentialImpulseConstraintSolver();
     70        virtual ~btSequentialImpulseConstraintSolver();
    5771
    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         }
     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);
    6473       
    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 
    72         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);
    75 
    76         virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
     74        btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
    7775        btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
    78         btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
    79 
    8076
    8177        ///clear internal cached data and reset random seed
    8278        virtual void    reset();
    83 
    84         btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
    85 
    86 
    8779       
    8880        unsigned long btRand2();

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

    r2662 r2882  
    6969btSliderConstraint::btSliderConstraint()
    7070        :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
    71                 m_useLinearReferenceFrameA(true)
     71                m_useLinearReferenceFrameA(true),
     72                m_useSolveConstraintObsolete(false)
     73//              m_useSolveConstraintObsolete(true)
    7274{
    7375        initParams();
    … …  
    8082        , m_frameInA(frameInA)
    8183        , m_frameInB(frameInB),
    82                 m_useLinearReferenceFrameA(useLinearReferenceFrameA)
     84                m_useLinearReferenceFrameA(useLinearReferenceFrameA),
     85                m_useSolveConstraintObsolete(false)
     86//              m_useSolveConstraintObsolete(true)
    8387{
    8488        initParams();
    … …  
    8993void btSliderConstraint::buildJacobian()
    9094{
     95        if (!m_useSolveConstraintObsolete)
     96        {
     97                return;
     98        }
    9199        if(m_useLinearReferenceFrameA)
    92100        {
    … …  
    156164//-----------------------------------------------------------------------------
    157165
    158 void btSliderConstraint::solveConstraint(btScalar timeStep)
    159 {
    160     m_timeStep = timeStep;
    161         if(m_useLinearReferenceFrameA)
    162         {
    163                 solveConstraintInt(m_rbA, m_rbB);
     166void btSliderConstraint::getInfo1(btConstraintInfo1* info)
     167{
     168        if (m_useSolveConstraintObsolete)
     169        {
     170                info->m_numConstraintRows = 0;
     171                info->nub = 0;
    164172        }
    165173        else
    166174        {
    167                 solveConstraintInt(m_rbB, m_rbA);
     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
     196void 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
     506void 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                }
    168519        }
    169520} // btSliderConstraint::solveConstraint()
    … …  
    171522//-----------------------------------------------------------------------------
    172523
    173 void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
     524void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
    174525{
    175526    int i;
    176527    // linear
    177     btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
    178     btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
     528    btVector3 velA;
     529        bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
     530    btVector3 velB;
     531        bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
    179532    btVector3 vel = velA - velB;
    180533        for(i = 0; i < 3; i++)
    … …  
    191544                btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
    192545                btVector3 impulse_vector = normal * normalImpulse;
    193                 rbA.applyImpulse( impulse_vector, m_relPosA);
    194                 rbB.applyImpulse(-impulse_vector, m_relPosB);
     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
    195558                if(m_poweredLinMotor && (!i))
    196559                { // apply linear motor
    … …  
    218581                                // apply clamped impulse
    219582                                impulse_vector = normal * normalImpulse;
    220                                 rbA.applyImpulse( impulse_vector, m_relPosA);
    221                                 rbB.applyImpulse(-impulse_vector, m_relPosB);
     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
    222595                        }
    223596                }
    … …  
    228601        btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
    229602
    230         const btVector3& angVelA = rbA.getAngularVelocity();
    231         const btVector3& angVelB = rbB.getAngularVelocity();
     603        btVector3 angVelA;
     604        bodyA.getAngularVelocity(angVelA);
     605        btVector3 angVelB;
     606        bodyB.getAngularVelocity(angVelB);
    232607
    233608        btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
    … …  
    239614        //solve orthogonal angular velocity correction
    240615        btScalar len = velrelOrthog.length();
     616        btScalar orthorImpulseMag = 0.f;
     617
    241618        if (len > btScalar(0.00001))
    242619        {
    243620                btVector3 normal = velrelOrthog.normalized();
    244621                btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
    245                 velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
     622                //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
     623                orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
    246624        }
    247625        //solve angular positional correction
    248626        btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
     627        btVector3 angularAxis = angularError;
     628        btScalar angularImpulseMag = 0;
     629
    249630        btScalar len2 = angularError.length();
    250631        if (len2>btScalar(0.00001))
    … …  
    252633                btVector3 normal2 = angularError.normalized();
    253634                btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
    254                 angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
     635                angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
     636                angularError *= angularImpulseMag;
    255637        }
    256638        // apply impulse
    257         rbA.applyTorqueImpulse(-velrelOrthog+angularError);
    258         rbB.applyTorqueImpulse(velrelOrthog-angularError);
     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
    259648        btScalar impulseMag;
    260649        //solve angular limits
    … …  
    270659        }
    271660        btVector3 impulse = axisA * impulseMag;
    272         rbA.applyTorqueImpulse(impulse);
    273         rbB.applyTorqueImpulse(-impulse);
     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
    274669        //apply angular motor
    275670        if(m_poweredAngMotor)
    … …  
    302697                        // apply clamped impulse
    303698                        btVector3 motorImp = angImpulse * axisA;
    304                         m_rbA.applyTorqueImpulse(motorImp);
    305                         m_rbB.applyTorqueImpulse(-motorImp);
     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);
    306704                }
    307705        }
    … …  
    313711
    314712void btSliderConstraint::calculateTransforms(void){
    315         if(m_useLinearReferenceFrameA)
     713        if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
    316714        {
    317715                m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
    … …  
    326724        m_realPivotBInW = m_calculatedTransformB.getOrigin();
    327725        m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
    328         m_delta = m_realPivotBInW - m_realPivotAInW;
     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        }
    329734        m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
    330735    btVector3 normalWorld;
    … …  
    368773
    369774//-----------------------------------------------------------------------------
    370  
    371775
    372776void btSliderConstraint::testAngLimits(void)
    … …  
    380784                const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
    381785                btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); 
     786                m_angPos = rot;
    382787                if(rot < m_lowerAngLimit)
    383788                {
    … …  
    392797        }
    393798} // btSliderConstraint::testAngLimits()
    394 
    395799       
    396800//-----------------------------------------------------------------------------
    397 
    398 
    399801
    400802btVector3 btSliderConstraint::getAncorInA(void)

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h

    r2662 r2882  
    4747{
    4848protected:
     49        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
     50        bool            m_useSolveConstraintObsolete;
    4951        btTransform     m_frameInA;
    5052    btTransform m_frameInB;
    … …  
    105107
    106108        btScalar m_linPos;
     109        btScalar m_angPos;
    107110
    108111        btScalar m_angDepth;
    … …  
    127130        // overrides
    128131    virtual void        buildJacobian();
    129     virtual     void    solveConstraint(btScalar        timeStep);
     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
    130139        // access
    131140    const btRigidBody& getRigidBodyA() const { return m_rbA; }
    … …  
    195204        btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
    196205        btScalar getLinearPos() { return m_linPos; }
     206       
    197207
    198208        // access for ODE solver
    … …  
    203213        // internal
    204214    void        buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
    205     void        solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB);
     215    void        solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
    206216        // shared code used by ODE solver
    207217        void    calculateTransforms(void);
    208218        void    testLinLimits(void);
     219        void    testLinLimits2(btConstraintInfo2* info);
    209220        void    testAngLimits(void);
    210221        // access for PE Solver

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h

    r2662 r2882  
    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
     34struct  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
     89SIMD_FORCE_INLINE btSimdScalar
     90operator*(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
     96SIMD_FORCE_INLINE btSimdScalar
     97operator+(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
    26106
    27107///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
    … …  
    29109{
    30110        BT_DECLARE_ALIGNED_ALLOCATOR();
     111        btVector3               m_deltaLinearVelocity;
     112        btVector3               m_deltaAngularVelocity;
     113        btScalar                m_angularFactor;
     114        btScalar                m_invMass;
     115        btScalar                m_friction;
     116        btRigidBody*    m_originalBody;
     117        btVector3               m_pushVelocity;
     118        //btVector3             m_turnVelocity;
     119
    31120       
    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;
    43         btRigidBody*    m_originalBody;
    44        
    45        
    46         SIMD_FORCE_INLINE void  getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
     121        SIMD_FORCE_INLINE void  getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
    47122        {
    48                 velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
     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);
    49127        }
    50128
     129        SIMD_FORCE_INLINE void  getAngularVelocity(btVector3& angVel) const
     130        {
     131                if (m_originalBody)
     132                        angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
     133                else
     134                        angVel.setValue(0,0,0);
     135        }
     136
     137
    51138        //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)
     139        SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
    53140        {
    54                 if (m_invMass)
     141                //if (m_invMass)
    55142                {
    56                         m_linearVelocity += linearComponent*impulseMagnitude;
    57                         m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
    58                 }
    59         }
    60        
    61         SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
    62         {
    63                 if (m_invMass)
    64                 {
    65                         m_pushVelocity += linearComponent*impulseMagnitude;
    66                         m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
     143                        m_deltaLinearVelocity += linearComponent*impulseMagnitude;
     144                        m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
    67145                }
    68146        }
    69147
     148       
     149/*
    70150       
    71151        void    writebackVelocity()
    … …  
    73153                if (m_invMass)
    74154                {
    75                         m_originalBody->setLinearVelocity(m_linearVelocity);
    76                         m_originalBody->setAngularVelocity(m_angularVelocity);
     155                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
     156                        m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
    77157                       
    78158                        //m_originalBody->setCompanionId(-1);
    79159                }
    80160        }
    81        
     161        */
    82162
    83         void    writebackVelocity(btScalar timeStep)
     163        void    writebackVelocity(btScalar timeStep=0)
    84164        {
    85165                if (m_invMass)
    86166                {
    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                        
     167                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
     168                        m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
    95169                        //m_originalBody->setCompanionId(-1);
    96170                }
    97171        }
    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 
    108172       
    109173

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h

    r2662 r2882  
    2020#include "LinearMath/btVector3.h"
    2121#include "LinearMath/btMatrix3x3.h"
     22#include "btJacobianEntry.h"
    2223
    2324//#define NO_FRICTION_TANGENTIALS 1
     25#include "btSolverBody.h"
     26
    2427
    2528///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
    … …  
    2831        BT_DECLARE_ALIGNED_ALLOCATOR();
    2932
    30         btVector3       m_relpos1CrossNormal;
    31         btVector3       m_contactNormal;
     33        btVector3               m_relpos1CrossNormal;
     34        btVector3               m_contactNormal;
    3235
    33         btVector3       m_relpos2CrossNormal;
    34         btVector3       m_angularComponentA;
     36        btVector3               m_relpos2CrossNormal;
     37        //btVector3             m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
    3538
    36         btVector3       m_angularComponentB;
    37 
    38         mutable btScalar        m_appliedPushImpulse;
     39        btVector3               m_angularComponentA;
     40        btVector3               m_angularComponentB;
    3941       
    40         mutable btScalar        m_appliedImpulse;
    41         int                     m_solverBodyIdA;
    42         int                     m_solverBodyIdB;
     42        mutable btSimdScalar    m_appliedPushImpulse;
     43        mutable btSimdScalar    m_appliedImpulse;
     44       
    4345       
    4446        btScalar        m_friction;
    45         btScalar        m_restitution;
    4647        btScalar        m_jacDiagABInv;
    47         btScalar        m_penetration;
     48        union
     49        {
     50                int     m_numConsecutiveRowsPerKernel;
     51                btScalar        m_unusedPadding0;
     52        };
     53
     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        };
    4869       
     70        union
     71        {
     72                void*           m_originalContactPoint;
     73                btScalar        m_unusedPadding4;
     74        };
    4975
    50        
    51         int                     m_constraintType;
    52         int                     m_frictionIndex;
    53         void*           m_originalContactPoint;
    54         int                     m_unusedPadding[1];
    55 
     76        btScalar                m_rhs;
     77        btScalar                m_cfm;
     78        btScalar                m_lowerLimit;
     79        btScalar                m_upperLimit;
    5680
    5781        enum            btSolverConstraintType
    … …  
    6286};
    6387
    64 
    65 
    66 
     88typedef btAlignedObjectArray<btSolverConstraint>        btConstraintArray;
    6789
    6890

  • code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

    r2662 r2882  
    2020static btRigidBody s_fixed(0, 0,0);
    2121
     22#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
     23
    2224btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
    2325:m_userConstraintType(-1),
    … …  
    2628m_rbA(s_fixed),
    2729m_rbB(s_fixed),
    28 m_appliedImpulse(btScalar(0.))
     30m_appliedImpulse(btScalar(0.)),
     31m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
    2932{
    3033        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    … …  
    3639m_rbA(rbA),
    3740m_rbB(s_fixed),
    38 m_appliedImpulse(btScalar(0.))
     41m_appliedImpulse(btScalar(0.)),
     42m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
    3943{
    4044                s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    … …  
    4953m_rbA(rbA),
    5054m_rbB(rbB),
    51 m_appliedImpulse(btScalar(0.))
     55m_appliedImpulse(btScalar(0.)),
     56m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
    5257{
    5358                s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
    … …  
    5560}
    5661
     62
     63//-----------------------------------------------------------------------------
     64
     65btScalar 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/trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h

    r2662 r2882  
    1919class btRigidBody;
    2020#include "LinearMath/btScalar.h"
     21#include "btSolverConstraint.h"
     22struct  btSolverBody;
     23
     24
     25
    2126
    2227enum btTypedConstraintType
    … …  
    2631        CONETWIST_CONSTRAINT_TYPE,
    2732        D6_CONSTRAINT_TYPE,
    28         VEHICLE_CONSTRAINT_TYPE,
    2933        SLIDER_CONSTRAINT_TYPE
    3034};
    … …  
    4953        btRigidBody&    m_rbB;
    5054        btScalar        m_appliedImpulse;
     55        btScalar        m_dbgDrawSize;
    5156
    5257
    … …  
    5661        virtual ~btTypedConstraint() {};
    5762        btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
     63        btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
    5864
    59         btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
     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
    6098
    6199        virtual void    buildJacobian() = 0;
    62100
    63         virtual void    solveConstraint(btScalar        timeStep) = 0;
     101        virtual void    setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
     102        {
     103        }
     104        virtual void getInfo1 (btConstraintInfo1* info)=0;
    64105
     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       
    65112        const btRigidBody& getRigidBodyA() const
    66113        {
    … …  
    95142                m_userConstraintId = uid;
    96143        }
    97        
     144
    98145        int getUserConstraintId() const
    99146        {
    … …  
    115162                return m_constraintType;
    116163        }
    117 
     164       
     165        void setDbgDrawSize(btScalar dbgDrawSize)
     166        {
     167                m_dbgDrawSize = dbgDrawSize;
     168        }
     169        btScalar getDbgDrawSize()
     170        {
     171                return m_dbgDrawSize;
     172        }
     173       
    118174};
    119175

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

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

  • code/trunk/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp

    r2662 r2882  
    9191
    9292        ///update vehicle simulation
    93         updateVehicles(timeStep);
    94 
     93        updateActions(timeStep);
    9594
    9695        updateActivationState( timeStep );

  • code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

    r2662 r2882  
    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"
    3237
    3338//for debug rendering
    … …  
    4752
    4853
    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"
     54#include "BulletDynamics/Dynamics/btActionInterface.h"
    5855#include "LinearMath/btQuickprof.h"
    5956#include "LinearMath/btMotionState.h"
    … …  
    114111                if (body)
    115112                {
    116                                 btTransform predictedTrans;
    117113                                if (body->getActivationState() != ISLAND_SLEEPING)
    118114                                {
    … …  
    149145                }
    150146        }
     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
    151165
    152166
    … …  
    191205                }
    192206       
    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 
     207                if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
     208                {
     209                        for (i=0;i<m_actions.size();i++)
     210                        {
     211                                m_actions[i]->debugDraw(m_debugDrawer);
    219212                        }
    220213                }
    … …  
    288281                }
    289282        }
    290 
     283/*
    291284        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
    292285        {
    … …  
    300293                }
    301294        }
     295        */
     296
    302297
    303298}
    … …  
    340335        if (getDebugDrawer())
    341336        {
    342                 gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
     337                btIDebugDraw* debugDrawer = getDebugDrawer ();
     338                gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
    343339        }
    344340        if (numSimulationSubSteps)
    … …  
    404400
    405401        ///update vehicle simulation
    406         updateVehicles(timeStep);
    407        
    408         updateCharacters(timeStep);
    409 
     402        updateActions(timeStep);
     403       
    410404        updateActivationState( timeStep );
    411405
    … …  
    471465
    472466
    473 void    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 
    484 void    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 
     467void    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}
    496476       
    497477       
    … …  
    550530}
    551531
    552 void    btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
    553 {
    554         m_vehicles.push_back(vehicle);
    555 }
    556 
    557 void    btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
    558 {
    559         m_vehicles.remove(vehicle);
    560 }
    561 
    562 void    btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character)
    563 {
    564         m_characters.push_back(character);
    565 }
    566 
    567 void    btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character)
    568 {
    569         m_characters.remove(character);
     532void    btDiscreteDynamicsWorld::addAction(btActionInterface* action)
     533{
     534        m_actions.push_back(action);
     535}
     536
     537void    btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
     538{
     539        m_actions.remove(action);
     540}
     541
     542
     543void    btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
     544{
     545        addAction(vehicle);
     546}
     547
     548void    btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
     549{
     550        removeAction(vehicle);
     551}
     552
     553void    btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
     554{
     555        addAction(character);
     556}
     557
     558void    btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
     559{
     560        removeAction(character);
    570561}
    571562
    … …  
    643634                        if (islandId<0)
    644635                        {
    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);
     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                                }
    647641                        } else
    648642                        {
    … …  
    690684        }
    691685
    692 //      assert(0);
     686//      btAssert(0);
    693687               
    694688       
    … …  
    754748        btScalar m_allowedPenetration;
    755749        btOverlappingPairCache* m_pairCache;
     750        btDispatcher* m_dispatcher;
    756751
    757752
    758753public:
    759         btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) :
     754        btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
    760755          btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
    761756                m_allowedPenetration(0.0f),
    762757                m_me(me),
    763                 m_pairCache(pairCache)
     758                m_pairCache(pairCache),
     759                m_dispatcher(dispatcher)
    764760        {
    765761        }
    … …  
    796792                        return false;
    797793
    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;
     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                                        }
    812814                                }
    813815                        }
    … …  
    847849                                                gNumClampedCcdMotions++;
    848850                                               
    849                                                 btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache());
     851                                                btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
    850852                                                btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
    851853                                                btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
    … …  
    11881190
    11891191
     1192void 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
    11901375void    btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
    11911376{
    … …  
    12161401        return m_constraints[index];
    12171402}
     1403
     1404

  • code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

    r2662 r2882  
    2424class btSimulationIslandManager;
    2525class btTypedConstraint;
     26class btActionInterface;
    2627
    27 
    28 class btRaycastVehicle;
    29 class btCharacterControllerInterface;
    3028class btIDebugDraw;
    3129#include "LinearMath/btAlignedObjectArray.h"
    … …  
    5351        bool    m_ownsConstraintSolver;
    5452
     53        btAlignedObjectArray<btActionInterface*>        m_actions;
    5554       
    56         btAlignedObjectArray<btRaycastVehicle*> m_vehicles;
    57        
    58         btAlignedObjectArray<btCharacterControllerInterface*> m_characters;
    59        
    60 
    6155        int     m_profileTimings;
    6256
    … …  
    7165        void    updateActivationState(btScalar timeStep);
    7266
    73         void    updateVehicles(btScalar timeStep);
    74 
    75         void    updateCharacters(btScalar timeStep);
     67        void    updateActions(btScalar timeStep);
    7668
    7769        void    startProfiling(btScalar timeStep);
    … …  
    10698        virtual void    removeConstraint(btTypedConstraint* constraint);
    10799
    108         virtual void    addVehicle(btRaycastVehicle* vehicle);
     100        virtual void    addAction(btActionInterface*);
    109101
    110         virtual void    removeVehicle(btRaycastVehicle* vehicle);
     102        virtual void    removeAction(btActionInterface*);
    111103       
    112         virtual void    addCharacter(btCharacterControllerInterface* character);
    113 
    114         virtual void    removeCharacter(btCharacterControllerInterface* character);
    115                
    116 
    117104        btSimulationIslandManager*      getSimulationIslandManager()
    118105        {
    … …  
    131118
    132119        virtual void    setGravity(const btVector3& gravity);
     120
    133121        virtual btVector3 getGravity () const;
    134122
    … …  
    140128
    141129        void    debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
     130
     131        void    debugDrawConstraint(btTypedConstraint* constraint);
    142132
    143133        virtual void    debugDrawWorld();
    … …  
    170160        }
    171161
     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
    172177};
    173178

  • code/trunk/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h

    r2662 r2882  
    2121
    2222class btTypedConstraint;
    23 class btRaycastVehicle;
     23class btActionInterface;
    2424class btConstraintSolver;
    2525class btDynamicsWorld;
    26 class btCharacterControllerInterface;
     26
    2727
    2828/// Type for the callback for each tick
    … …  
    7373                virtual void    removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
    7474
    75                 virtual void    addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
     75                virtual void    addAction(btActionInterface* action) = 0;
    7676
    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 
     77                virtual void    removeAction(btActionInterface* action) = 0;
    8378
    8479                //once a rigidbody is added to the dynamics world, it will get this gravity assigned
    … …  
    130125
    131126
     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
    132137};
    133138

  • code/trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp

    r2662 r2882  
    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));
    4950        m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
    5051        m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
    … …  
    125126                m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
    126127        }
     128        m_gravity_acceleration = acceleration;
    127129}
    128130

  • code/trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.h

    r2662 r2882  
    4949
    5050        btVector3               m_gravity;     
     51        btVector3               m_gravity_acceleration;
    5152        btVector3               m_invInertiaLocal;
    5253        btVector3               m_totalForce;
    … …  
    182183        const btVector3&        getGravity() const
    183184        {
    184                 return m_gravity;
     185                return m_gravity_acceleration;
    185186        }
    186187
    … …  
    232233                m_totalForce += force;
    233234        }
     235
     236        const btVector3& getTotalForce()
     237        {
     238                return m_totalForce;
     239        };
     240
     241        const btVector3& getTotalTorque()
     242        {
     243                return m_totalTorque;
     244        };
    234245   
    235         const btVector3& getInvInertiaDiagLocal()
     246        const btVector3& getInvInertiaDiagLocal() const
    236247        {
    237248                return m_invInertiaLocal;

  • code/trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp

    r2662 r2882  
    2020#include "btWheelInfo.h"
    2121#include "LinearMath/btMinMax.h"
    22 
    23 
     22#include "LinearMath/btIDebugDraw.h"
    2423#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
    2524
    … …  
    2726
    2827btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,  btVehicleRaycaster* raycaster )
    29 : btTypedConstraint(VEHICLE_CONSTRAINT_TYPE),
    30 m_vehicleRaycaster(raycaster),
     28:m_vehicleRaycaster(raycaster),
    3129m_pitchControl(btScalar(0.))
    3230{
    … …  
    8886const btTransform&      btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
    8987{
    90         assert(wheelIndex < getNumWheels());
     88        btAssert(wheelIndex < getNumWheels());
    9189        const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
    9290        return wheel.m_worldTransform;
    … …  
    176174        btVehicleRaycaster::btVehicleRaycasterResult    rayResults;
    177175
    178         assert(m_vehicleRaycaster);
     176        btAssert(m_vehicleRaycaster);
    179177
    180178        void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
    … …  
    360358void    btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
    361359{
    362         assert(wheel>=0 && wheel < getNumWheels());
     360        btAssert(wheel>=0 && wheel < getNumWheels());
    363361
    364362        btWheelInfo& wheelInfo = getWheelInfo(wheel);
    … …  
    376374void    btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
    377375{
    378         assert(wheel>=0 && wheel < getNumWheels());
     376        btAssert(wheel>=0 && wheel < getNumWheels());
    379377        btWheelInfo& wheelInfo = getWheelInfo(wheel);
    380378        wheelInfo.m_engineForce = force;
    … …  
    705703
    706704
     705
     706void    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
    707735void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
    708736{
    … …  
    728756        return 0;
    729757}
     758

  • code/trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h

    r2662 r2882  
    1818#include "LinearMath/btAlignedObjectArray.h"
    1919#include "btWheelInfo.h"
     20#include "BulletDynamics/Dynamics/btActionInterface.h"
    2021
    2122class btVehicleTuning;
    2223
    2324///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
    24 class btRaycastVehicle : public btTypedConstraint
     25class btRaycastVehicle : public btActionInterface
    2526{
    2627
    … …  
    7475        virtual ~btRaycastVehicle() ;
    7576
    76                
     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                       
    7788        const btTransform& getChassisWorldTransform() const;
    7889       
    … …  
    8091
    8192        virtual void updateVehicle(btScalar step);
    82 
     93       
     94       
    8395        void resetSuspension();
    8496
    … …  
    176188        }
    177189
    178         virtual void    buildJacobian()
    179         {
    180                 //not yet
    181         }
    182 
    183         virtual void    solveConstraint(btScalar        timeStep)
    184         {
    185                 (void)timeStep;
    186                 //not yet
    187         }
    188190
    189191

  • code/trunk/src/bullet/LinearMath/btAlignedAllocator.cpp

    r2662 r2882  
    2020int gTotalBytesAlignedAllocs = 0;//detect memory leaks
    2121
     22static void *btAllocDefault(size_t size)
     23{
     24        return malloc(size);
     25}
     26
     27static void btFreeDefault(void *ptr)
     28{
     29        free(ptr);
     30}
     31
     32static btAllocFunc *sAllocFunc = btAllocDefault;
     33static btFreeFunc *sFreeFunc = btFreeDefault;
     34
     35
     36
    2237#if defined (BT_HAS_ALIGNED_ALLOCATOR)
    2338#include <malloc.h>
    … …  
    5065  unsigned long offset;
    5166
    52   real = (char *)malloc(size + sizeof(void *) + (alignment-1));
     67  real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1));
    5368  if (real) {
    5469    offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1);
    … …  
    6782  if (ptr) {
    6883    real = *((void **)(ptr)-1);
    69     free(real);
     84    sFreeFunc(real);
    7085  }
    7186}
    7287#endif
    7388
    74 static void *btAllocDefault(size_t size)
    75 {
    76         return malloc(size);
    77 }
    78 
    79 static void btFreeDefault(void *ptr)
    80 {
    81         free(ptr);
    82 }
    8389
    8490static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault;
    8591static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault;
    86 static btAllocFunc *sAllocFunc = btAllocDefault;
    87 static btFreeFunc *sFreeFunc = btFreeDefault;
    8892
    8993void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc)

  • code/trunk/src/bullet/LinearMath/btAlignedAllocator.h

    r2662 r2882  
    3939        void    btAlignedFreeInternal   (void* ptr);
    4040
    41         #define btAlignedAlloc(a,b) btAlignedAllocInternal(a,b)
     41        #define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment)
    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
     53void 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.
    5255void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);
    53 void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
     56
    5457
    5558///The btAlignedAllocator is a portable class for aligned memory allocations.

  • code/trunk/src/bullet/LinearMath/btAlignedObjectArray.h

    r2662 r2882  
    5959                        return (size ? size*2 : 1);
    6060                }
    61                 SIMD_FORCE_INLINE       void    copy(int start,int end, T* dest)
     61                SIMD_FORCE_INLINE       void    copy(int start,int end, T* dest) const
    6262                {
    6363                        int i;
    … …  
    121121                }
    122122
    123                 SIMD_FORCE_INLINE       int capacity() const
    124                 {       // return current length of allocated storage
    125                         return m_capacity;
    126                 }
    127                
     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
     133               
     134               
     135                /// return the number of elements in the array
    128136                SIMD_FORCE_INLINE       int size() const
    129                 {       // return length of sequence
     137                {       
    130138                        return m_size;
    131139                }
    … …  
    142150               
    143151
     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.
    144153                SIMD_FORCE_INLINE       void    clear()
    145154                {
    … …  
    157166                }
    158167
     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.
    159170                SIMD_FORCE_INLINE       void    resize(int newsize, const T& fillData=T())
    160171                {
    … …  
    220231
    221232       
     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                }
    222238               
    223239                SIMD_FORCE_INLINE       void reserve(int _Count)

  • code/trunk/src/bullet/LinearMath/btConvexHull.cpp

    r2662 r2882  
    263263                for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
    264264                {
    265                         btScalar s = sinf(SIMD_RADS_PER_DEG*(x));
    266                         btScalar c = cosf(SIMD_RADS_PER_DEG*(x));
     265                        btScalar s = btSin(SIMD_RADS_PER_DEG*(x));
     266                        btScalar c = btCos(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 = sinf(SIMD_RADS_PER_DEG*(xx));
    279                                         btScalar c = cosf(SIMD_RADS_PER_DEG*(xx));
     278                                        btScalar s = btSin(SIMD_RADS_PER_DEG*(xx));
     279                                        btScalar c = btCos(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/trunk/src/bullet/LinearMath/btIDebugDraw.h

    r2662 r2882  
    3030
    3131#include "btVector3.h"
     32#include "btTransform.h"
    3233
    3334
    … …  
    5354                DBG_DisableBulletLCP = 512,
    5455                DBG_EnableCCD = 1024,
     56                DBG_DrawConstraints = (1 << 11),
     57                DBG_DrawConstraintLimits = (1 << 12),
    5558                DBG_MAX_DEBUG_DRAW_MODE
    5659        };
    5760
    5861        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        }
    5975
    6076        virtual void    drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
    … …  
    110126                }
    111127        }
     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        }
    112293};
    113294

  • code/trunk/src/bullet/LinearMath/btMatrix3x3.h

    r2662 r2882  
    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
    195204  /**@brief Fill the values of the matrix into a 9 element array
    196205   * @param m The array to be filled */

  • code/trunk/src/bullet/LinearMath/btQuadWord.h

    r2662 r2882  
    2525#endif
    2626
    27 /**@brief The btQuadWordStorage class is base class for btVector3 and btQuaternion.
     27/**@brief The btQuadWord 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) btQuadWordStorage
     31ATTRIBUTE_ALIGNED16(class) btQuadWord
    3232#else
    33 class btQuadWordStorage
     33class btQuadWord
    3434#endif
    3535{
    … …  
    4646                return mVec128;
    4747        }
     48protected:
    4849#else //__CELLOS_LV2__ __SPU__
    4950        btScalar        m_floats[4];
    5051#endif //__CELLOS_LV2__ __SPU__
    5152
    52 };
    53 
    54 /** @brief The btQuadWord is base-class for vectors, points */
    55 class   btQuadWord : public btQuadWordStorage
    56 {
    5753        public:
    5854 
    … …  
    135131                {
    136132                }
    137   /**@brief Copy constructor */
    138                 SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q)
    139                 {
    140                         *((btQuadWordStorage*)this) = q;
    141                 }
     133 
    142134  /**@brief Three argument constructor (zeros w)
    143135   * @param x Value of x

  • code/trunk/src/bullet/LinearMath/btQuaternion.h

    r2662 r2882  
    2020
    2121#include "btVector3.h"
     22#include "btQuadWord.h"
    2223
    2324/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
    … …  
    5859        {
    5960                btScalar d = axis.length();
    60                 assert(d != btScalar(0.0));
     61                btAssert(d != btScalar(0.0));
    6162                btScalar s = btSin(angle * btScalar(0.5)) / d;
    6263                setValue(axis.x() * s, axis.y() * s, axis.z() * s,
    … …  
    177178        btQuaternion operator/(const btScalar& s) const
    178179        {
    179                 assert(s != btScalar(0.0));
     180                btAssert(s != btScalar(0.0));
    180181                return *this * (btScalar(1.0) / s);
    181182        }
    … …  
    185186        btQuaternion& operator/=(const btScalar& s)
    186187        {
    187                 assert(s != btScalar(0.0));
     188                btAssert(s != btScalar(0.0));
    188189                return *this *= btScalar(1.0) / s;
    189190        }
    … …  
    199200        {
    200201                btScalar s = btSqrt(length2() * q.length2());
    201                 assert(s != btScalar(0.0));
     202                btAssert(s != btScalar(0.0));
    202203                return btAcos(dot(q) / s);
    203204        }
    … …  
    275276        }
    276277
     278        static const btQuaternion&      getIdentity()
     279        {
     280                static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
     281                return identityQuat;
     282        }
     283
    277284        SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
    278285

  • code/trunk/src/bullet/LinearMath/btScalar.h

    r2662 r2882  
    2626#include <float.h>
    2727
    28 #define BT_BULLET_VERSION 273
     28#define BT_BULLET_VERSION 274
    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))
    6466                        #define BT_USE_SSE
    65                 #endif
     67                        #include <emmintrin.h>
     68#endif
     69
     70                #endif//_XBOX
     71
    6672                #endif //__MINGW32__
    6773
    … …  
    125131
    126132                #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)))
    127136                #define ATTRIBUTE_ALIGNED16(a) a
    128137                #define ATTRIBUTE_ALIGNED128(a) a
    … …  
    228237SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
    229238SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
    230   #if defined( __MINGW32__ )
    231   SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
    232   #else
    233   SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
    234   #endif
    235 
     239SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
     240       
    236241#endif
    237242

  • code/trunk/src/bullet/LinearMath/btTransform.h

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

  • code/trunk/src/bullet/LinearMath/btVector3.h

    r2662 r2882  
    1818#define SIMD__VECTOR3_H
    1919
    20 #include "btQuadWord.h"
    21 
     20
     21#include "btScalar.h"
     22#include "btScalar.h"
     23#include "btMinMax.h"
    2224/**@brief btVector3 can be used to represent 3D points and vectors.
    2325 * 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
    2426 * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
    2527 */
    26 class   btVector3 : public btQuadWord {
    27 
     28
     29ATTRIBUTE_ALIGNED16(class) btVector3
     30{
    2831public:
     32
     33#if defined (__SPU__) && defined (__CELLOS_LV2__)
     34        union {
     35                vec_float4 mVec128;
     36                btScalar        m_floats[4];
     37        };
     38public:
     39        vec_float4      get128() const
     40        {
     41                return mVec128;
     42        }
     43public:
     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
    2965  /**@brief No initialization constructor */
    3066        SIMD_FORCE_INLINE btVector3() {}
    3167
    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         }
     68 
    3869       
    3970  /**@brief Constructor from scalars
    … …  
    4273   * @param z Z value
    4374   */
    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 //      }
     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        }
    5382
    5483       
    … …  
    5887        {
    5988
    60                 m_floats[0] += v.x(); m_floats[1] += v.y(); m_floats[2] += v.z();
     89                m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
    6190                return *this;
    6291        }
    … …  
    6796        SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
    6897        {
    69                 m_floats[0] -= v.x(); m_floats[1] -= v.y(); m_floats[2] -= v.z();
     98                m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
    7099                return *this;
    71100        }
    … …  
    74103        SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
    75104        {
    76                 m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s;
     105                m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
    77106                return *this;
    78107        }
    … …  
    90119        SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
    91120        {
    92                 return m_floats[0] * v.x() + m_floats[1] * v.y() + m_floats[2] * v.z();
     121                return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
    93122        }
    94123
    … …  
    149178        {
    150179                return btVector3(
    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());
     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]);
    154183        }
    155184
    156185        SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
    157186        {
    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());
     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]);
    161190        }
    162191
    … …  
    165194        SIMD_FORCE_INLINE int minAxis() const
    166195        {
    167                 return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
     196                return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
    168197        }
    169198
    … …  
    172201        SIMD_FORCE_INLINE int maxAxis() const
    173202        {
    174                 return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
     203                return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
    175204        }
    176205
    … …  
    188217        {
    189218                btScalar s = btScalar(1.0) - rt;
    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();
     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];
    193222                //don't do the unused w component
    194223                //              m_co[3] = s * v0[3] + rt * v1[3];
    … …  
    200229        SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
    201230        {
    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);
     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);
    205234        }
    206235
    … …  
    209238        SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
    210239        {
    211                 m_floats[0] *= v.x(); m_floats[1] *= v.y(); m_floats[2] *= v.z();
     240                m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
    212241                return *this;
    213242        }
    214243
    215        
     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                }
    216318
    217319};
    … …  
    221323operator+(const btVector3& v1, const btVector3& v2)
    222324{
    223         return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
     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]);
    224326}
    225327
    … …  
    228330operator*(const btVector3& v1, const btVector3& v2)
    229331{
    230         return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
     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]);
    231333}
    232334
    … …  
    235337operator-(const btVector3& v1, const btVector3& v2)
    236338{
    237         return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
     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]);
    238340}
    239341/**@brief Return the negative of the vector */
    … …  
    241343operator-(const btVector3& v)
    242344{
    243         return btVector3(-v.x(), -v.y(), -v.z());
     345        return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
    244346}
    245347
    … …  
    248350operator*(const btVector3& v, const btScalar& s)
    249351{
    250         return btVector3(v.x() * s, v.y() * s, v.z() * s);
     352        return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
    251353}
    252354
    … …  
    270372operator/(const btVector3& v1, const btVector3& v2)
    271373{
    272         return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
     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]);
    273375}
    274376
    … …  
    326428}
    327429
    328 /**@brief Test if each element of the vector is equivalent */
    329 SIMD_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 }
     430
    333431
    334432SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
    … …  
    405503                {
    406504                        maxIndex = 2;
    407                         maxVal = m_floats[2];
     505                        maxVal =m_floats[2];
    408506                }
    409507                if (m_floats[3] > maxVal)
    … …  
    438536                {
    439537                        minIndex = 2;
    440                         minVal = m_floats[2];
     538                        minVal =m_floats[2];
    441539                }
    442540                if (m_floats[3] < minVal)
    … …  
    455553                return absolute4().maxAxis4();
    456554        }
     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 
    457589
    458590};
Note: See TracChangeset for help on using the changeset viewer.