Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Feb 27, 2011, 7:43:24 AM (14 years ago)
Author:
rgrieder
Message:

Updated Bullet Physics Engine from v2.74 to v2.77

Location:
code/branches/kicklib/src/external/bullet/LinearMath
Files:
2 added
18 edited

Legend:

Unmodified
Added
Removed

  • code/branches/kicklib/src/external/bullet/LinearMath/CMakeLists.txt

    r5929 r7983  
    22
    33COMPILATION_BEGIN BulletLinearMathCompilation.cpp
    4                 btConvexHull.cpp
    5                 btQuickprof.cpp
    6                 btGeometryUtil.cpp
    7                 btAlignedAllocator.cpp
     4        btAlignedAllocator.cpp
     5        btConvexHull.cpp
     6        btGeometryUtil.cpp
     7        btQuickprof.cpp
     8        btSerializer.cpp
    89COMPILATION_END
    910
    10                 # Headers
    11                 btAlignedObjectArray.h
    12                 btList.h
    13                 btPoolAllocator.h
    14                 btRandom.h
    15                 btVector3.h
    16                 btDefaultMotionState.h
    17                 btMatrix3x3.h
    18                 btQuadWord.h
    19                 btHashMap.h
    20                 btScalar.h
    21                 btAabbUtil2.h
    22                 btConvexHull.h
    23                 btMinMax.h
    24                 btQuaternion.h
    25                 btStackAlloc.h
    26                 btGeometryUtil.h
    27                 btMotionState.h
    28                 btTransform.h
    29                 btAlignedAllocator.h
    30                 btIDebugDraw.h
    31                 btQuickprof.h
    32                 btTransformUtil.h
     11        # Headers
     12        btAabbUtil2.h
     13        btAlignedAllocator.h
     14        btAlignedObjectArray.h
     15        btConvexHull.h
     16        btDefaultMotionState.h
     17        btGeometryUtil.h
     18        btHashMap.h
     19        btIDebugDraw.h
     20        btList.h
     21        btMatrix3x3.h
     22        btMinMax.h
     23        btMotionState.h
     24        btPoolAllocator.h
     25        btQuadWord.h
     26        btQuaternion.h
     27        btQuickprof.h
     28        btRandom.h
     29        btScalar.h
     30        btSerializer.h
     31        btStackAlloc.h
     32        btTransform.h
     33        btTransformUtil.h
     34        btVector3.h
    3335)

  • code/branches/kicklib/src/external/bullet/LinearMath/btAlignedAllocator.cpp

    r5781 r7983  
    161161{
    162162        gNumAlignedAllocs++;
    163   void* ptr;
    164 #if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
     163        void* ptr;
    165164        ptr = sAlignedAllocFunc(size, alignment);
    166 #else
    167   char *real;
    168   unsigned long offset;
    169 
    170   real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1));
    171   if (real) {
    172     offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1);
    173     ptr = (void *)((real + sizeof(void *)) + offset);
    174     *((void **)(ptr)-1) = (void *)(real);
    175   } else {
    176     ptr = (void *)(real);
    177   }
    178 #endif  // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
    179165//      printf("btAlignedAllocInternal %d, %x\n",size,ptr);
    180166        return ptr;
    … …  
    190176        gNumAlignedFree++;
    191177//      printf("btAlignedFreeInternal %x\n",ptr);
    192 #if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
    193178        sAlignedFreeFunc(ptr);
    194 #else
    195   void* real;
    196 
    197   if (ptr) {
    198     real = *((void **)(ptr)-1);
    199     sFreeFunc(real);
    200   }
    201 #endif  // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
    202179}
    203180

  • code/branches/kicklib/src/external/bullet/LinearMath/btAlignedObjectArray.h

    r5781 r7983  
    139139                }
    140140               
     141                SIMD_FORCE_INLINE const T& at(int n) const
     142                {
     143                        return m_data[n];
     144                }
     145
     146                SIMD_FORCE_INLINE T& at(int n)
     147                {
     148                        return m_data[n];
     149                }
     150
    141151                SIMD_FORCE_INLINE const T& operator[](int n) const
    142152                {
    … …  
    172182                        int curSize = size();
    173183
    174                         if (newsize < size())
    175                         {
    176                                 for(int i = curSize; i < newsize; i++)
     184                        if (newsize < curSize)
     185                        {
     186                                for(int i = newsize; i < curSize; i++)
    177187                                {
    178188                                        m_data[i].~T();
    … …  
    196206                }
    197207       
     208                SIMD_FORCE_INLINE       T&  expandNonInitializing( )
     209                {       
     210                        int sz = size();
     211                        if( sz == capacity() )
     212                        {
     213                                reserve( allocSize(size()) );
     214                        }
     215                        m_size++;
     216
     217                        return m_data[sz];             
     218                }
     219
    198220
    199221                SIMD_FORCE_INLINE       T&  expand( const T& fillValue=T())
    … …  
    438460        }
    439461
     462        void copyFromArray(const btAlignedObjectArray& otherArray)
     463        {
     464                int otherSize = otherArray.size();
     465                resize (otherSize);
     466                otherArray.copy(0, otherSize, m_data);
     467        }
     468
    440469};
    441470

  • code/branches/kicklib/src/external/bullet/LinearMath/btConvexHull.cpp

    r5781 r7983  
    1717
    1818#include "btConvexHull.h"
    19 #include "LinearMath/btAlignedObjectArray.h"
    20 #include "LinearMath/btMinMax.h"
    21 #include "LinearMath/btVector3.h"
     19#include "btAlignedObjectArray.h"
     20#include "btMinMax.h"
     21#include "btVector3.h"
    2222
    2323
    … …  
    9797                                static btVector3 dif;
    9898                dif = p1-p0;
    99                                 btScalar dn= dot(plane.normal,dif);
    100                                 btScalar t = -(plane.dist+dot(plane.normal,p0) )/dn;
     99                                btScalar dn= btDot(plane.normal,dif);
     100                                btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn;
    101101                                return p0 + (dif*t);
    102102}
    … …  
    104104btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
    105105{
    106         return point - plane.normal * (dot(point,plane.normal)+plane.dist);
     106        return point - plane.normal * (btDot(point,plane.normal)+plane.dist);
    107107}
    108108
    … …  
    111111        // return the normal of the triangle
    112112        // inscribed by v0, v1, and v2
    113         btVector3 cp=cross(v1-v0,v2-v1);
     113        btVector3 cp=btCross(v1-v0,v2-v1);
    114114        btScalar m=cp.length();
    115115        if(m==0) return btVector3(1,0,0);
    … …  
    121121{
    122122        static btVector3 cp;
    123         cp = cross(udir,vdir).normalized();
    124 
    125         btScalar distu = -dot(cp,ustart);
    126         btScalar distv = -dot(cp,vstart);
     123        cp = btCross(udir,vdir).normalized();
     124
     125        btScalar distu = -btDot(cp,ustart);
     126        btScalar distv = -btDot(cp,vstart);
    127127        btScalar dist = (btScalar)fabs(distu-distv);
    128128        if(upoint)
    129129                {
    130130                btPlane plane;
    131                 plane.normal = cross(vdir,cp).normalized();
    132                 plane.dist = -dot(plane.normal,vstart);
     131                plane.normal = btCross(vdir,cp).normalized();
     132                plane.dist = -btDot(plane.normal,vstart);
    133133                *upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
    134134        }
    … …  
    136136                {
    137137                btPlane plane;
    138                 plane.normal = cross(udir,cp).normalized();
    139                 plane.dist = -dot(plane.normal,ustart);
     138                plane.normal = btCross(udir,cp).normalized();
     139                plane.dist = -btDot(plane.normal,ustart);
    140140                *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
    141141        }
    … …  
    171171int PlaneTest(const btPlane &p, const btVector3 &v);
    172172int PlaneTest(const btPlane &p, const btVector3 &v) {
    173         btScalar a  = dot(v,p.normal)+p.dist;
     173        btScalar a  = btDot(v,p.normal)+p.dist;
    174174        int   flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
    175175        return flag;
    … …  
    229229                if(allow[i])
    230230                {
    231                         if(m==-1 || dot(p[i],dir)>dot(p[m],dir))
     231                        if(m==-1 || btDot(p[i],dir)>btDot(p[m],dir))
    232232                                m=i;
    233233                }
    … …  
    239239btVector3 orth(const btVector3 &v)
    240240{
    241         btVector3 a=cross(v,btVector3(0,0,1));
    242         btVector3 b=cross(v,btVector3(0,1,0));
     241        btVector3 a=btCross(v,btVector3(0,0,1));
     242        btVector3 b=btCross(v,btVector3(0,1,0));
    243243        if (a.length() > b.length())
    244244        {
    … …  
    259259                if(allow[m]==3) return m;
    260260                T u = orth(dir);
    261                 T v = cross(u,dir);
     261                T v = btCross(u,dir);
    262262                int ma=-1;
    263263                for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
    … …  
    314314{
    315315        btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
    316         return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
     316        return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
    317317}
    318318int hasedge(const int3 &t, int a,int b);
    … …  
    496496        if(p0==p1 || basis[0]==btVector3(0,0,0))
    497497                return int4(-1,-1,-1,-1);
    498         basis[1] = cross(btVector3(     btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
    499         basis[2] = cross(btVector3(btScalar(-0.02),     btScalar(1), btScalar(0)),basis[0]);
     498        basis[1] = btCross(btVector3(     btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
     499        basis[2] = btCross(btVector3(btScalar(-0.02),     btScalar(1), btScalar(0)),basis[0]);
    500500        if (basis[1].length() > basis[2].length())
    501501        {
    … …  
    513513                return int4(-1,-1,-1,-1);
    514514        basis[1] = verts[p2] - verts[p0];
    515         basis[2] = cross(basis[1],basis[0]).normalized();
     515        basis[2] = btCross(basis[1],basis[0]).normalized();
    516516        int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
    517517        if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
    … …  
    519519                return int4(-1,-1,-1,-1);
    520520        btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
    521         if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
     521        if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
    522522        return int4(p0,p1,p2,p3);
    523523}
    … …  
    565565                btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
    566566                t->vmax = maxdirsterid(verts,verts_count,n,allow);
    567                 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
     567                t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
    568568        }
    569569        btHullTriangle *te;
    … …  
    593593                        if(!hasvert(*m_tris[j],v)) break;
    594594                        int3 nt=*m_tris[j];
    595                         if(above(verts,nt,center,btScalar(0.01)*epsilon)  || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) )
     595                        if(above(verts,nt,center,btScalar(0.01)*epsilon)  || btCross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) )
    596596                        {
    597597                                btHullTriangle *nb = m_tris[m_tris[j]->n[0]];
    … …  
    615615                        else
    616616                        {
    617                                 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
     617                                t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
    618618                        }
    619619                }
    … …  
    877877        vcount = 0;
    878878
    879         btScalar recip[3];
     879        btScalar recip[3]={0.f,0.f,0.f};
    880880
    881881        if ( scale )
    … …  
    10121012                                btScalar z = v[2];
    10131013
    1014                                 btScalar dx = fabsf(x - px );
    1015                                 btScalar dy = fabsf(y - py );
    1016                                 btScalar dz = fabsf(z - pz );
     1014                                btScalar dx = btFabs(x - px );
     1015                                btScalar dy = btFabs(y - py );
     1016                                btScalar dz = btFabs(z - pz );
    10171017
    10181018                                if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
    … …  
    11361136        ocount = 0;
    11371137
    1138         for (i=0; i<indexcount; i++)
     1138        for (i=0; i<int (indexcount); i++)
    11391139        {
    11401140                unsigned int v = indices[i]; // original array index
    … …  
    11571157                        for (int k=0;k<m_vertexIndexMapping.size();k++)
    11581158                        {
    1159                                 if (tmpIndices[k]==v)
     1159                                if (tmpIndices[k]==int(v))
    11601160                                        m_vertexIndexMapping[k]=ocount;
    11611161                        }

  • code/branches/kicklib/src/external/bullet/LinearMath/btConvexHull.h

    r5781 r7983  
    2020#define CD_HULL_H
    2121
    22 #include "LinearMath/btVector3.h"
    23 #include "LinearMath/btAlignedObjectArray.h"
     22#include "btVector3.h"
     23#include "btAlignedObjectArray.h"
    2424
    2525typedef btAlignedObjectArray<unsigned int> TUIntArray;

  • code/branches/kicklib/src/external/bullet/LinearMath/btDefaultMotionState.h

    r5781 r7983  
    11#ifndef DEFAULT_MOTION_STATE_H
    22#define DEFAULT_MOTION_STATE_H
     3
     4#include "btMotionState.h"
    35
    46///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets.

  • code/branches/kicklib/src/external/bullet/LinearMath/btHashMap.h

    r5781 r7983  
    44#include "btAlignedObjectArray.h"
    55
     6///very basic hashable string implementation, compatible with btHashMap
     7struct btHashString
     8{
     9        const char* m_string;
     10        unsigned int    m_hash;
     11
     12        SIMD_FORCE_INLINE       unsigned int getHash()const
     13        {
     14                return m_hash;
     15        }
     16
     17        btHashString(const char* name)
     18                :m_string(name)
     19        {
     20                /* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
     21                static const unsigned int  InitialFNV = 2166136261u;
     22                static const unsigned int FNVMultiple = 16777619u;
     23
     24                /* Fowler / Noll / Vo (FNV) Hash */
     25                unsigned int hash = InitialFNV;
     26               
     27                for(int i = 0; m_string[i]; i++)
     28                {
     29                        hash = hash ^ (m_string[i]);       /* xor  the low 8 bits */
     30                        hash = hash * FNVMultiple;  /* multiply by the magic number */
     31                }
     32                m_hash = hash;
     33        }
     34
     35        int portableStringCompare(const char* src,      const char* dst) const
     36        {
     37                        int ret = 0 ;
     38
     39                        while( ! (ret = *(unsigned char *)src - *(unsigned char *)dst) && *dst)
     40                                        ++src, ++dst;
     41
     42                        if ( ret < 0 )
     43                                        ret = -1 ;
     44                        else if ( ret > 0 )
     45                                        ret = 1 ;
     46
     47                        return( ret );
     48        }
     49
     50        bool equals(const btHashString& other) const
     51        {
     52                return (m_string == other.m_string) ||
     53                        (0==portableStringCompare(m_string,other.m_string));
     54
     55        }
     56
     57};
     58
    659const int BT_HASH_NULL=0xffffffff;
     60
     61
     62class btHashInt
     63{
     64        int     m_uid;
     65public:
     66        btHashInt(int uid)      :m_uid(uid)
     67        {
     68        }
     69
     70        int     getUid1() const
     71        {
     72                return m_uid;
     73        }
     74
     75        void    setUid1(int uid)
     76        {
     77                m_uid = uid;
     78        }
     79
     80        bool equals(const btHashInt& other) const
     81        {
     82                return getUid1() == other.getUid1();
     83        }
     84        //to our success
     85        SIMD_FORCE_INLINE       unsigned int getHash()const
     86        {
     87                int key = m_uid;
     88                // Thomas Wang's hash
     89                key += ~(key << 15);    key ^=  (key >> 10);    key +=  (key << 3);     key ^=  (key >> 6);     key += ~(key << 11);    key ^=  (key >> 16);
     90                return key;
     91        }
     92};
     93
     94
     95
     96class btHashPtr
     97{
     98
     99        union
     100        {
     101                const void*     m_pointer;
     102                int     m_hashValues[2];
     103        };
     104
     105public:
     106
     107        btHashPtr(const void* ptr)
     108                :m_pointer(ptr)
     109        {
     110        }
     111
     112        const void*     getPointer() const
     113        {
     114                return m_pointer;
     115        }
     116
     117        bool equals(const btHashPtr& other) const
     118        {
     119                return getPointer() == other.getPointer();
     120        }
     121
     122        //to our success
     123        SIMD_FORCE_INLINE       unsigned int getHash()const
     124        {
     125                const bool VOID_IS_8 = ((sizeof(void*)==8));
     126               
     127                int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0];
     128       
     129                // Thomas Wang's hash
     130                key += ~(key << 15);    key ^=  (key >> 10);    key +=  (key << 3);     key ^=  (key >> 6);     key += ~(key << 11);    key ^=  (key >> 16);
     131                return key;
     132        }
     133
     134       
     135};
     136
     137
     138template <class Value>
     139class btHashKeyPtr
     140{
     141        int     m_uid;
     142public:
     143
     144        btHashKeyPtr(int uid)    :m_uid(uid)
     145        {
     146        }
     147
     148        int     getUid1() const
     149        {
     150                return m_uid;
     151        }
     152
     153        bool equals(const btHashKeyPtr<Value>& other) const
     154        {
     155                return getUid1() == other.getUid1();
     156        }
     157
     158        //to our success
     159        SIMD_FORCE_INLINE       unsigned int getHash()const
     160        {
     161                int key = m_uid;
     162                // Thomas Wang's hash
     163                key += ~(key << 15);    key ^=  (key >> 10);    key +=  (key << 3);     key ^=  (key >> 6);     key += ~(key << 11);    key ^=  (key >> 16);
     164                return key;
     165        }
     166
     167       
     168};
     169
    7170
    8171template <class Value>
    … …  
    12175public:
    13176
    14         btHashKey(int uid)
    15                 :m_uid(uid)
    16         {
    17         }
    18 
    19         int     getUid() const
     177        btHashKey(int uid)      :m_uid(uid)
     178        {
     179        }
     180
     181        int     getUid1() const
    20182        {
    21183                return m_uid;
    22184        }
    23185
     186        bool equals(const btHashKey<Value>& other) const
     187        {
     188                return getUid1() == other.getUid1();
     189        }
    24190        //to our success
    25191        SIMD_FORCE_INLINE       unsigned int getHash()const
    … …  
    27193                int key = m_uid;
    28194                // Thomas Wang's hash
    29                 key += ~(key << 15);
    30                 key ^=  (key >> 10);
    31                 key +=  (key << 3);
    32                 key ^=  (key >> 6);
    33                 key += ~(key << 11);
    34                 key ^=  (key >> 16);
     195                key += ~(key << 15);    key ^=  (key >> 10);    key +=  (key << 3);     key ^=  (key >> 6);     key += ~(key << 11);    key ^=  (key >> 16);
    35196                return key;
    36197        }
    37 
    38         btHashKey       getKey(const Value& value) const
    39         {
    40                 return btHashKey(value.getUid());
    41         }
    42 };
    43 
    44 
    45 template <class Value>
    46 class btHashKeyPtr
    47 {
    48         int     m_uid;
    49 public:
    50 
    51         btHashKeyPtr(int uid)
    52                 :m_uid(uid)
    53         {
    54         }
    55 
    56         int     getUid() const
    57         {
    58                 return m_uid;
    59         }
    60 
    61         //to our success
    62         SIMD_FORCE_INLINE       unsigned int getHash()const
    63         {
    64                 int key = m_uid;
    65                 // Thomas Wang's hash
    66                 key += ~(key << 15);
    67                 key ^=  (key >> 10);
    68                 key +=  (key << 3);
    69                 key ^=  (key >> 6);
    70                 key += ~(key << 11);
    71                 key ^=  (key >> 16);
    72                 return key;
    73         }
    74 
    75         btHashKeyPtr    getKey(const Value& value) const
    76         {
    77                 return btHashKeyPtr(value->getUid());
    78         }
    79 };
     198};
     199
    80200
    81201///The btHashMap template class implements a generic and lightweight hashmap.
    … …  
    85205{
    86206
     207protected:
    87208        btAlignedObjectArray<int>               m_hashTable;
    88209        btAlignedObjectArray<int>               m_next;
     210       
    89211        btAlignedObjectArray<Value>             m_valueArray;
    90 
    91 
    92 
    93         void    growTables(const Key& key)
     212        btAlignedObjectArray<Key>               m_keyArray;
     213
     214        void    growTables(const Key& /*key*/)
    94215        {
    95216                int newCapacity = m_valueArray.capacity();
    … …  
    116237                        for(i=0;i<curHashtableSize;i++)
    117238                        {
    118                                 const Value& value = m_valueArray[i];
    119 
    120                                 int     hashValue = key.getKey(value).getHash() & (m_valueArray.capacity()-1);  // New hash value with new mask
     239                                //const Value& value = m_valueArray[i];
     240                                //const Key& key = m_keyArray[i];
     241
     242                                int     hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity()-1);      // New hash value with new mask
    121243                                m_next[i] = m_hashTable[hashValue];
    122244                                m_hashTable[hashValue] = i;
    … …  
    131253        void insert(const Key& key, const Value& value) {
    132254                int hash = key.getHash() & (m_valueArray.capacity()-1);
    133                 //don't add it if it is already there
    134                 if (find(key))
    135                 {
     255
     256                //replace value if the key is already there
     257                int index = findIndex(key);
     258                if (index != BT_HASH_NULL)
     259                {
     260                        m_valueArray[index]=value;
    136261                        return;
    137262                }
     263
    138264                int count = m_valueArray.size();
    139265                int oldCapacity = m_valueArray.capacity();
    140266                m_valueArray.push_back(value);
     267                m_keyArray.push_back(key);
     268
    141269                int newCapacity = m_valueArray.capacity();
    142270                if (oldCapacity < newCapacity)
    … …  
    192320                {
    193321                        m_valueArray.pop_back();
     322                        m_keyArray.pop_back();
    194323                        return;
    195324                }
    196325
    197326                // Remove the last pair from the hash table.
    198                 const Value* lastValue = &m_valueArray[lastPairIndex];
    199                 int lastHash = key.getKey(*lastValue).getHash() & (m_valueArray.capacity()-1);
     327                int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity()-1);
    200328
    201329                index = m_hashTable[lastHash];
    … …  
    221349                // Copy the last pair into the remove pair's spot.
    222350                m_valueArray[pairIndex] = m_valueArray[lastPairIndex];
     351                m_keyArray[pairIndex] = m_keyArray[lastPairIndex];
    223352
    224353                // Insert the last pair into the hash table
    … …  
    227356
    228357                m_valueArray.pop_back();
     358                m_keyArray.pop_back();
    229359
    230360        }
    … …  
    277407        int     findIndex(const Key& key) const
    278408        {
    279                 int hash = key.getHash() & (m_valueArray.capacity()-1);
    280 
    281                 if (hash >= m_hashTable.size())
     409                unsigned int hash = key.getHash() & (m_valueArray.capacity()-1);
     410
     411                if (hash >= (unsigned int)m_hashTable.size())
    282412                {
    283413                        return BT_HASH_NULL;
    … …  
    285415
    286416                int index = m_hashTable[hash];
    287                 while ((index != BT_HASH_NULL) && (key.getUid() == key.getKey(m_valueArray[index]).getUid()) == false)
     417                while ((index != BT_HASH_NULL) && key.equals(m_keyArray[index]) == false)
    288418                {
    289419                        index = m_next[index];
    … …  
    297427                m_next.clear();
    298428                m_valueArray.clear();
     429                m_keyArray.clear();
    299430        }
    300431

  • code/branches/kicklib/src/external/bullet/LinearMath/btIDebugDraw.h

    r5781 r7983  
    11/*
    2 Copyright (c) 2005 Gino van den Bergen / Erwin Coumans http://continuousphysics.com
    3 
    4 Permission is hereby granted, free of charge, to any person or organization
    5 obtaining a copy of the software and accompanying documentation covered by
    6 this license (the "Software") to use, reproduce, display, distribute,
    7 execute, and transmit the Software, and to prepare derivative works of the
    8 Software, and to permit third-parties to whom the Software is furnished to
    9 do so, all subject to the following:
    10 
    11 The copyright notices in the Software and this entire statement, including
    12 the above license grant, this restriction and the following disclaimer,
    13 must be included in all copies of the Software, in whole or in part, and
    14 all derivative works of the Software, unless such copies or derivative
    15 works are solely in the form of machine-executable object code generated by
    16 a source language processor.
    17 
    18 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    19 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    20 FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
    21 SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
    22 FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
    23 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
    24 DEALINGS IN THE SOFTWARE.
     2Bullet Continuous Collision Detection and Physics Library
     3Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
     4
     5This software is provided 'as-is', without any express or implied warranty.
     6In no event will the authors be held liable for any damages arising from the use of this software.
     7Permission is granted to anyone to use this software for any purpose,
     8including commercial applications, and to alter it and redistribute it freely,
     9subject to the following restrictions:
     10
     111. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
     122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
     133. This notice may not be removed or altered from any source distribution.
    2514*/
    2615
    … …  
    3625///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld.
    3726///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum.
     27///For color arguments the X,Y,Z components refer to Red, Green and Blue each in the range [0..1]
    3828class   btIDebugDraw
    3929{
    … …  
    5646                DBG_DrawConstraints = (1 << 11),
    5747                DBG_DrawConstraintLimits = (1 << 12),
     48                DBG_FastWireframe = (1<<13),
    5849                DBG_MAX_DEBUG_DRAW_MODE
    5950        };
    … …  
    6152        virtual ~btIDebugDraw() {};
    6253
     54        virtual void    drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
     55               
    6356        virtual void    drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
    6457        {
     58        (void) toColor;
    6559                drawLine (from, to, fromColor);
    6660        }
    6761
    68         virtual void    drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha)
    69         {
    70         }
    71 
     62        virtual void    drawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
     63        {
     64                btVector3 start = transform.getOrigin();
     65
     66                const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
     67                const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
     68                const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
     69
     70                // XY
     71                drawLine(start-xoffs, start+yoffs, color);
     72                drawLine(start+yoffs, start+xoffs, color);
     73                drawLine(start+xoffs, start-yoffs, color);
     74                drawLine(start-yoffs, start-xoffs, color);
     75
     76                // XZ
     77                drawLine(start-xoffs, start+zoffs, color);
     78                drawLine(start+zoffs, start+xoffs, color);
     79                drawLine(start+xoffs, start-zoffs, color);
     80                drawLine(start-zoffs, start-xoffs, color);
     81
     82                // YZ
     83                drawLine(start-yoffs, start+zoffs, color);
     84                drawLine(start+zoffs, start+yoffs, color);
     85                drawLine(start+yoffs, start-zoffs, color);
     86                drawLine(start-zoffs, start-yoffs, color);
     87        }
     88       
    7289        virtual void    drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
    7390        {
    74         }
    75 
    76         virtual void    drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
     91                btTransform tr;
     92                tr.setIdentity();
     93                tr.setOrigin(p);
     94                drawSphere(radius,tr,color);
     95        }
    7796       
    7897        virtual void    drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& /*n0*/,const btVector3& /*n1*/,const btVector3& /*n2*/,const btVector3& color, btScalar alpha)
    … …  
    97116        virtual int             getDebugMode() const = 0;
    98117
    99         inline void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color)
     118        virtual void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color)
    100119        {
    101120
    … …  
    126145                }
    127146        }
    128         void drawTransform(const btTransform& transform, btScalar orthoLen)
     147        virtual void drawTransform(const btTransform& transform, btScalar orthoLen)
    129148        {
    130149                btVector3 start = transform.getOrigin();
    … …  
    134153        }
    135154
    136         void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
     155        virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
    137156                                const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
    138157        {
    … …  
    159178                }
    160179        }
    161         void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
     180        virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
    162181                btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f))
    163182        {
    … …  
    261280        }
    262281       
    263         void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color)
     282        virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color)
    264283        {
    265284                drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
    … …  
    276295                drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
    277296        }
    278         void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color)
     297        virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color)
    279298        {
    280299                drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color);

  • code/branches/kicklib/src/external/bullet/LinearMath/btMatrix3x3.h

    r5781 r7983  
    1414
    1515
    16 #ifndef btMatrix3x3_H
    17 #define btMatrix3x3_H
    18 
    19 #include "btScalar.h"
     16#ifndef BT_MATRIX3x3_H
     17#define BT_MATRIX3x3_H
    2018
    2119#include "btVector3.h"
    2220#include "btQuaternion.h"
    2321
     22#ifdef BT_USE_DOUBLE_PRECISION
     23#define btMatrix3x3Data btMatrix3x3DoubleData
     24#else
     25#define btMatrix3x3Data btMatrix3x3FloatData
     26#endif //BT_USE_DOUBLE_PRECISION
    2427
    2528
    2629/**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
    27  * Make sure to only include a pure orthogonal matrix without scaling. */
     30* Make sure to only include a pure orthogonal matrix without scaling. */
    2831class btMatrix3x3 {
    29         public:
    30   /** @brief No initializaion constructor */
    31                 btMatrix3x3 () {}
    32                
    33 //              explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
    34                
    35   /**@brief Constructor from Quaternion */
    36                 explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
    37                 /*
    38                 template <typename btScalar>
    39                 Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
    40                 {
    41                         setEulerYPR(yaw, pitch, roll);
    42                 }
    43                 */
    44   /** @brief Constructor with row major formatting */
    45                 btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
    46                                   const btScalar& yx, const btScalar& yy, const btScalar& yz,
    47                                   const btScalar& zx, const btScalar& zy, const btScalar& zz)
    48                 {
    49                         setValue(xx, xy, xz,
    50                                          yx, yy, yz,
    51                                          zx, zy, zz);
    52                 }
    53   /** @brief Copy constructor */
    54                 SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
    55                 {
    56                         m_el[0] = other.m_el[0];
    57                         m_el[1] = other.m_el[1];
    58                         m_el[2] = other.m_el[2];
    59                 }
    60   /** @brief Assignment Operator */
    61                 SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
    62                 {
    63                         m_el[0] = other.m_el[0];
    64                         m_el[1] = other.m_el[1];
    65                         m_el[2] = other.m_el[2];
    66                         return *this;
    67                 }
    68 
    69   /** @brief Get a column of the matrix as a vector
    70    *  @param i Column number 0 indexed */
    71                 SIMD_FORCE_INLINE btVector3 getColumn(int i) const
    72                 {
    73                         return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
    74                 }
    75                
    76 
    77   /** @brief Get a row of the matrix as a vector
    78    *  @param i Row number 0 indexed */
    79                 SIMD_FORCE_INLINE const btVector3& getRow(int i) const
    80                 {
    81                         btFullAssert(0 <= i && i < 3);
    82                         return m_el[i];
    83                 }
    84 
    85   /** @brief Get a mutable reference to a row of the matrix as a vector
    86    *  @param i Row number 0 indexed */
    87                 SIMD_FORCE_INLINE btVector3&  operator[](int i)
    88                 {
    89                         btFullAssert(0 <= i && i < 3);
    90                         return m_el[i];
    91                 }
    92                
    93   /** @brief Get a const reference to a row of the matrix as a vector
    94    *  @param i Row number 0 indexed */
    95                 SIMD_FORCE_INLINE const btVector3& operator[](int i) const
    96                 {
    97                         btFullAssert(0 <= i && i < 3);
    98                         return m_el[i];
    99                 }
    100                
    101   /** @brief Multiply by the target matrix on the right
    102    *  @param m Rotation matrix to be applied
    103    * Equivilant to this = this * m */
    104                 btMatrix3x3& operator*=(const btMatrix3x3& m);
    105                
    106   /** @brief Set from a carray of btScalars
    107    *  @param m A pointer to the beginning of an array of 9 btScalars */
     32
     33        ///Data storage for the matrix, each vector is a row of the matrix
     34        btVector3 m_el[3];
     35
     36public:
     37        /** @brief No initializaion constructor */
     38        btMatrix3x3 () {}
     39
     40        //              explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
     41
     42        /**@brief Constructor from Quaternion */
     43        explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
     44        /*
     45        template <typename btScalar>
     46        Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
     47        {
     48        setEulerYPR(yaw, pitch, roll);
     49        }
     50        */
     51        /** @brief Constructor with row major formatting */
     52        btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
     53                const btScalar& yx, const btScalar& yy, const btScalar& yz,
     54                const btScalar& zx, const btScalar& zy, const btScalar& zz)
     55        {
     56                setValue(xx, xy, xz,
     57                        yx, yy, yz,
     58                        zx, zy, zz);
     59        }
     60        /** @brief Copy constructor */
     61        SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
     62        {
     63                m_el[0] = other.m_el[0];
     64                m_el[1] = other.m_el[1];
     65                m_el[2] = other.m_el[2];
     66        }
     67        /** @brief Assignment Operator */
     68        SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
     69        {
     70                m_el[0] = other.m_el[0];
     71                m_el[1] = other.m_el[1];
     72                m_el[2] = other.m_el[2];
     73                return *this;
     74        }
     75
     76        /** @brief Get a column of the matrix as a vector
     77        *  @param i Column number 0 indexed */
     78        SIMD_FORCE_INLINE btVector3 getColumn(int i) const
     79        {
     80                return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
     81        }
     82
     83
     84        /** @brief Get a row of the matrix as a vector
     85        *  @param i Row number 0 indexed */
     86        SIMD_FORCE_INLINE const btVector3& getRow(int i) const
     87        {
     88                btFullAssert(0 <= i && i < 3);
     89                return m_el[i];
     90        }
     91
     92        /** @brief Get a mutable reference to a row of the matrix as a vector
     93        *  @param i Row number 0 indexed */
     94        SIMD_FORCE_INLINE btVector3&  operator[](int i)
     95        {
     96                btFullAssert(0 <= i && i < 3);
     97                return m_el[i];
     98        }
     99
     100        /** @brief Get a const reference to a row of the matrix as a vector
     101        *  @param i Row number 0 indexed */
     102        SIMD_FORCE_INLINE const btVector3& operator[](int i) const
     103        {
     104                btFullAssert(0 <= i && i < 3);
     105                return m_el[i];
     106        }
     107
     108        /** @brief Multiply by the target matrix on the right
     109        *  @param m Rotation matrix to be applied
     110        * Equivilant to this = this * m */
     111        btMatrix3x3& operator*=(const btMatrix3x3& m);
     112
     113        /** @brief Set from a carray of btScalars
     114        *  @param m A pointer to the beginning of an array of 9 btScalars */
    108115        void setFromOpenGLSubMatrix(const btScalar *m)
    109                 {
    110                         m_el[0].setValue(m[0],m[4],m[8]);
    111                         m_el[1].setValue(m[1],m[5],m[9]);
    112                         m_el[2].setValue(m[2],m[6],m[10]);
    113 
    114                 }
    115   /** @brief Set the values of the matrix explicitly (row major)
    116    *  @param xx Top left
    117    *  @param xy Top Middle
    118    *  @param xz Top Right
    119    *  @param yx Middle Left
    120    *  @param yy Middle Middle
    121    *  @param yz Middle Right
    122    *  @param zx Bottom Left
    123    *  @param zy Bottom Middle
    124    *  @param zz Bottom Right*/
    125                 void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
    126                                           const btScalar& yx, const btScalar& yy, const btScalar& yz,
    127                                           const btScalar& zx, const btScalar& zy, const btScalar& zz)
    128                 {
    129                         m_el[0].setValue(xx,xy,xz);
    130                         m_el[1].setValue(yx,yy,yz);
    131                         m_el[2].setValue(zx,zy,zz);
    132                 }
    133 
    134   /** @brief Set the matrix from a quaternion
    135    *  @param q The Quaternion to match */ 
    136                 void setRotation(const btQuaternion& q)
    137                 {
    138                         btScalar d = q.length2();
    139                         btFullAssert(d != btScalar(0.0));
    140                         btScalar s = btScalar(2.0) / d;
    141                         btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
    142                         btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
    143                         btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
    144                         btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
    145                         setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
    146                                          xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
    147                                          xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
    148                 }
    149                
    150 
    151   /** @brief Set the matrix from euler angles using YPR around YXZ respectively
    152    *  @param yaw Yaw about Y axis
    153    *  @param pitch Pitch about X axis
    154    *  @param roll Roll about Z axis
    155    */
    156                 void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
    157                 {
    158                         setEulerZYX(roll, pitch, yaw);
    159                 }
     116        {
     117                m_el[0].setValue(m[0],m[4],m[8]);
     118                m_el[1].setValue(m[1],m[5],m[9]);
     119                m_el[2].setValue(m[2],m[6],m[10]);
     120
     121        }
     122        /** @brief Set the values of the matrix explicitly (row major)
     123        *  @param xx Top left
     124        *  @param xy Top Middle
     125        *  @param xz Top Right
     126        *  @param yx Middle Left
     127        *  @param yy Middle Middle
     128        *  @param yz Middle Right
     129        *  @param zx Bottom Left
     130        *  @param zy Bottom Middle
     131        *  @param zz Bottom Right*/
     132        void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
     133                const btScalar& yx, const btScalar& yy, const btScalar& yz,
     134                const btScalar& zx, const btScalar& zy, const btScalar& zz)
     135        {
     136                m_el[0].setValue(xx,xy,xz);
     137                m_el[1].setValue(yx,yy,yz);
     138                m_el[2].setValue(zx,zy,zz);
     139        }
     140
     141        /** @brief Set the matrix from a quaternion
     142        *  @param q The Quaternion to match */ 
     143        void setRotation(const btQuaternion& q)
     144        {
     145                btScalar d = q.length2();
     146                btFullAssert(d != btScalar(0.0));
     147                btScalar s = btScalar(2.0) / d;
     148                btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
     149                btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
     150                btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
     151                btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
     152                setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
     153                        xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
     154                        xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
     155        }
     156
     157
     158        /** @brief Set the matrix from euler angles using YPR around YXZ respectively
     159        *  @param yaw Yaw about Y axis
     160        *  @param pitch Pitch about X axis
     161        *  @param roll Roll about Z axis
     162        */
     163        void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
     164        {
     165                setEulerZYX(roll, pitch, yaw);
     166        }
    160167
    161168        /** @brief Set the matrix from euler angles YPR around ZYX axes
    162          * @param eulerX Roll about X axis
    163          * @param eulerY Pitch around Y axis
    164          * @param eulerZ Yaw aboud Z axis
    165          *
    166          * These angles are used to produce a rotation matrix. The euler
    167          * angles are applied in ZYX order. I.e a vector is first rotated
    168          * about X then Y and then Z
    169          **/
     169        * @param eulerX Roll about X axis
     170        * @param eulerY Pitch around Y axis
     171        * @param eulerZ Yaw aboud Z axis
     172        *
     173        * These angles are used to produce a rotation matrix. The euler
     174        * angles are applied in ZYX order. I.e a vector is first rotated
     175        * about X then Y and then Z
     176        **/
    170177        void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
    171   ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
     178                ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
    172179                btScalar ci ( btCos(eulerX));
    173180                btScalar cj ( btCos(eulerY));
    … …  
    180187                btScalar sc = si * ch;
    181188                btScalar ss = si * sh;
    182                
     189
    183190                setValue(cj * ch, sj * sc - cs, sj * cc + ss,
    184                                  cj * sh, sj * ss + cc, sj * cs - sc,
    185                                  -sj,      cj * si,      cj * ci);
    186         }
    187 
    188   /**@brief Set the matrix to the identity */
    189                 void setIdentity()
     191                        cj * sh, sj * ss + cc, sj * cs - sc,
     192                        -sj,      cj * si,      cj * ci);
     193        }
     194
     195        /**@brief Set the matrix to the identity */
     196        void setIdentity()
     197        {
     198                setValue(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        }
     202
     203        static const btMatrix3x3&       getIdentity()
     204        {
     205                static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0),
     206                        btScalar(0.0), btScalar(1.0), btScalar(0.0),
     207                        btScalar(0.0), btScalar(0.0), btScalar(1.0));
     208                return identityMatrix;
     209        }
     210
     211        /**@brief Fill the values of the matrix into a 9 element array
     212        * @param m The array to be filled */
     213        void getOpenGLSubMatrix(btScalar *m) const
     214        {
     215                m[0]  = btScalar(m_el[0].x());
     216                m[1]  = btScalar(m_el[1].x());
     217                m[2]  = btScalar(m_el[2].x());
     218                m[3]  = btScalar(0.0);
     219                m[4]  = btScalar(m_el[0].y());
     220                m[5]  = btScalar(m_el[1].y());
     221                m[6]  = btScalar(m_el[2].y());
     222                m[7]  = btScalar(0.0);
     223                m[8]  = btScalar(m_el[0].z());
     224                m[9]  = btScalar(m_el[1].z());
     225                m[10] = btScalar(m_el[2].z());
     226                m[11] = btScalar(0.0);
     227        }
     228
     229        /**@brief Get the matrix represented as a quaternion
     230        * @param q The quaternion which will be set */
     231        void getRotation(btQuaternion& q) const
     232        {
     233                btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
     234                btScalar temp[4];
     235
     236                if (trace > btScalar(0.0))
     237                {
     238                        btScalar s = btSqrt(trace + btScalar(1.0));
     239                        temp[3]=(s * btScalar(0.5));
     240                        s = btScalar(0.5) / s;
     241
     242                        temp[0]=((m_el[2].y() - m_el[1].z()) * s);
     243                        temp[1]=((m_el[0].z() - m_el[2].x()) * s);
     244                        temp[2]=((m_el[1].x() - m_el[0].y()) * s);
     245                }
     246                else
     247                {
     248                        int i = m_el[0].x() < m_el[1].y() ?
     249                                (m_el[1].y() < m_el[2].z() ? 2 : 1) :
     250                                (m_el[0].x() < m_el[2].z() ? 2 : 0);
     251                        int j = (i + 1) % 3; 
     252                        int k = (i + 2) % 3;
     253
     254                        btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
     255                        temp[i] = s * btScalar(0.5);
     256                        s = btScalar(0.5) / s;
     257
     258                        temp[3] = (m_el[k][j] - m_el[j][k]) * s;
     259                        temp[j] = (m_el[j][i] + m_el[i][j]) * s;
     260                        temp[k] = (m_el[k][i] + m_el[i][k]) * s;
     261                }
     262                q.setValue(temp[0],temp[1],temp[2],temp[3]);
     263        }
     264
     265        /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
     266        * @param yaw Yaw around Y axis
     267        * @param pitch Pitch around X axis
     268        * @param roll around Z axis */ 
     269        void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
     270        {
     271
     272                // first use the normal calculus
     273                yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
     274                pitch = btScalar(btAsin(-m_el[2].x()));
     275                roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
     276
     277                // on pitch = +/-HalfPI
     278                if (btFabs(pitch)==SIMD_HALF_PI)
     279                {
     280                        if (yaw>0)
     281                                yaw-=SIMD_PI;
     282                        else
     283                                yaw+=SIMD_PI;
     284
     285                        if (roll>0)
     286                                roll-=SIMD_PI;
     287                        else
     288                                roll+=SIMD_PI;
     289                }
     290        };
     291
     292
     293        /**@brief Get the matrix represented as euler angles around ZYX
     294        * @param yaw Yaw around X axis
     295        * @param pitch Pitch around Y axis
     296        * @param roll around X axis
     297        * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/       
     298        void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
     299        {
     300                struct Euler
     301                {
     302                        btScalar yaw;
     303                        btScalar pitch;
     304                        btScalar roll;
     305                };
     306
     307                Euler euler_out;
     308                Euler euler_out2; //second solution
     309                //get the pointer to the raw data
     310
     311                // Check that pitch is not at a singularity
     312                if (btFabs(m_el[2].x()) >= 1)
     313                {
     314                        euler_out.yaw = 0;
     315                        euler_out2.yaw = 0;
     316
     317                        // From difference of angles formula
     318                        btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
     319                        if (m_el[2].x() > 0)  //gimbal locked up
     320                        {
     321                                euler_out.pitch = SIMD_PI / btScalar(2.0);
     322                                euler_out2.pitch = SIMD_PI / btScalar(2.0);
     323                                euler_out.roll = euler_out.pitch + delta;
     324                                euler_out2.roll = euler_out.pitch + delta;
     325                        }
     326                        else // gimbal locked down
     327                        {
     328                                euler_out.pitch = -SIMD_PI / btScalar(2.0);
     329                                euler_out2.pitch = -SIMD_PI / btScalar(2.0);
     330                                euler_out.roll = -euler_out.pitch + delta;
     331                                euler_out2.roll = -euler_out.pitch + delta;
     332                        }
     333                }
     334                else
     335                {
     336                        euler_out.pitch = - btAsin(m_el[2].x());
     337                        euler_out2.pitch = SIMD_PI - euler_out.pitch;
     338
     339                        euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch),
     340                                m_el[2].z()/btCos(euler_out.pitch));
     341                        euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch),
     342                                m_el[2].z()/btCos(euler_out2.pitch));
     343
     344                        euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch),
     345                                m_el[0].x()/btCos(euler_out.pitch));
     346                        euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch),
     347                                m_el[0].x()/btCos(euler_out2.pitch));
     348                }
     349
     350                if (solution_number == 1)
    190351                {
    191                         setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0),
    192                                          btScalar(0.0), btScalar(1.0), btScalar(0.0),
    193                                          btScalar(0.0), btScalar(0.0), btScalar(1.0));
     352                        yaw = euler_out.yaw;
     353                        pitch = euler_out.pitch;
     354                        roll = euler_out.roll;
    194355                }
    195 
    196                 static const btMatrix3x3&       getIdentity()
     356                else
     357                {
     358                        yaw = euler_out2.yaw;
     359                        pitch = euler_out2.pitch;
     360                        roll = euler_out2.roll;
     361                }
     362        }
     363
     364        /**@brief Create a scaled copy of the matrix
     365        * @param s Scaling vector The elements of the vector will scale each column */
     366
     367        btMatrix3x3 scaled(const btVector3& s) const
     368        {
     369                return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
     370                        m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
     371                        m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
     372        }
     373
     374        /**@brief Return the determinant of the matrix */
     375        btScalar            determinant() const;
     376        /**@brief Return the adjoint of the matrix */
     377        btMatrix3x3 adjoint() const;
     378        /**@brief Return the matrix with all values non negative */
     379        btMatrix3x3 absolute() const;
     380        /**@brief Return the transpose of the matrix */
     381        btMatrix3x3 transpose() const;
     382        /**@brief Return the inverse of the matrix */
     383        btMatrix3x3 inverse() const;
     384
     385        btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
     386        btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
     387
     388        SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
     389        {
     390                return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
     391        }
     392        SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
     393        {
     394                return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
     395        }
     396        SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
     397        {
     398                return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
     399        }
     400
     401
     402        /**@brief diagonalizes this matrix by the Jacobi method.
     403        * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
     404        * coordinate system, i.e., old_this = rot * new_this * rot^T.
     405        * @param threshold See iteration
     406        * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
     407        * by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
     408        *
     409        * Note that this matrix is assumed to be symmetric.
     410        */
     411        void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
     412        {
     413                rot.setIdentity();
     414                for (int step = maxSteps; step > 0; step--)
    197415                {
    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 
    204   /**@brief Fill the values of the matrix into a 9 element array
    205    * @param m The array to be filled */
    206                 void getOpenGLSubMatrix(btScalar *m) const
    207                 {
    208                         m[0]  = btScalar(m_el[0].x());
    209                         m[1]  = btScalar(m_el[1].x());
    210                         m[2]  = btScalar(m_el[2].x());
    211                         m[3]  = btScalar(0.0);
    212                         m[4]  = btScalar(m_el[0].y());
    213                         m[5]  = btScalar(m_el[1].y());
    214                         m[6]  = btScalar(m_el[2].y());
    215                         m[7]  = btScalar(0.0);
    216                         m[8]  = btScalar(m_el[0].z());
    217                         m[9]  = btScalar(m_el[1].z());
    218                         m[10] = btScalar(m_el[2].z());
    219                         m[11] = btScalar(0.0);
    220                 }
    221 
    222   /**@brief Get the matrix represented as a quaternion
    223    * @param q The quaternion which will be set */
    224                 void getRotation(btQuaternion& q) const
    225                 {
    226                         btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
    227                         btScalar temp[4];
    228                        
    229                         if (trace > btScalar(0.0))
    230                         {
    231                                 btScalar s = btSqrt(trace + btScalar(1.0));
    232                                 temp[3]=(s * btScalar(0.5));
    233                                 s = btScalar(0.5) / s;
    234                                
    235                                 temp[0]=((m_el[2].y() - m_el[1].z()) * s);
    236                                 temp[1]=((m_el[0].z() - m_el[2].x()) * s);
    237                                 temp[2]=((m_el[1].x() - m_el[0].y()) * s);
    238                         }
    239                         else
    240                         {
    241                                 int i = m_el[0].x() < m_el[1].y() ?
    242                                         (m_el[1].y() < m_el[2].z() ? 2 : 1) :
    243                                         (m_el[0].x() < m_el[2].z() ? 2 : 0);
    244                                 int j = (i + 1) % 3; 
    245                                 int k = (i + 2) % 3;
    246                                
    247                                 btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
    248                                 temp[i] = s * btScalar(0.5);
    249                                 s = btScalar(0.5) / s;
    250                                
    251                                 temp[3] = (m_el[k][j] - m_el[j][k]) * s;
    252                                 temp[j] = (m_el[j][i] + m_el[i][j]) * s;
    253                                 temp[k] = (m_el[k][i] + m_el[i][k]) * s;
    254                         }
    255                         q.setValue(temp[0],temp[1],temp[2],temp[3]);
    256                 }
    257 
    258   /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
    259    * @param yaw Yaw around Y axis
    260    * @param pitch Pitch around X axis
    261    * @param roll around Z axis */       
    262                 void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
    263                 {
    264                        
    265                         // first use the normal calculus
    266                         yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
    267                         pitch = btScalar(btAsin(-m_el[2].x()));
    268                         roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
    269 
    270                         // on pitch = +/-HalfPI
    271                         if (btFabs(pitch)==SIMD_HALF_PI)
    272                         {
    273                                 if (yaw>0)
    274                                         yaw-=SIMD_PI;
    275                                 else
    276                                         yaw+=SIMD_PI;
    277 
    278                                 if (roll>0)
    279                                         roll-=SIMD_PI;
    280                                 else
    281                                         roll+=SIMD_PI;
    282                         }
    283                 };
    284 
    285 
    286   /**@brief Get the matrix represented as euler angles around ZYX
    287    * @param yaw Yaw around X axis
    288    * @param pitch Pitch around Y axis
    289    * @param roll around X axis
    290    * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/   
    291   void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
    292   {
    293     struct Euler{btScalar yaw, pitch, roll;};
    294     Euler euler_out;
    295     Euler euler_out2; //second solution
    296     //get the pointer to the raw data
    297    
    298     // Check that pitch is not at a singularity
    299     if (btFabs(m_el[2].x()) >= 1)
    300     {
    301       euler_out.yaw = 0;
    302       euler_out2.yaw = 0;
    303        
    304       // From difference of angles formula
    305       btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
    306       if (m_el[2].x() > 0)  //gimbal locked up
    307       {
    308         euler_out.pitch = SIMD_PI / btScalar(2.0);
    309         euler_out2.pitch = SIMD_PI / btScalar(2.0);
    310         euler_out.roll = euler_out.pitch + delta;
    311         euler_out2.roll = euler_out.pitch + delta;
    312       }
    313       else // gimbal locked down
    314       {
    315         euler_out.pitch = -SIMD_PI / btScalar(2.0);
    316         euler_out2.pitch = -SIMD_PI / btScalar(2.0);
    317         euler_out.roll = -euler_out.pitch + delta;
    318         euler_out2.roll = -euler_out.pitch + delta;
    319       }
    320     }
    321     else
    322     {
    323       euler_out.pitch = - btAsin(m_el[2].x());
    324       euler_out2.pitch = SIMD_PI - euler_out.pitch;
    325        
    326       euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch),
    327                                m_el[2].z()/btCos(euler_out.pitch));
    328       euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch),
    329                                 m_el[2].z()/btCos(euler_out2.pitch));
    330        
    331       euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch),
    332                               m_el[0].x()/btCos(euler_out.pitch));
    333       euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch),
    334                                m_el[0].x()/btCos(euler_out2.pitch));
    335     }
    336    
    337     if (solution_number == 1)
    338     {
    339                 yaw = euler_out.yaw;
    340                 pitch = euler_out.pitch;
    341                 roll = euler_out.roll;
    342     }
    343     else
    344     {
    345                 yaw = euler_out2.yaw;
    346                 pitch = euler_out2.pitch;
    347                 roll = euler_out2.roll;
    348     }
    349   }
    350 
    351   /**@brief Create a scaled copy of the matrix
    352    * @param s Scaling vector The elements of the vector will scale each column */
    353                
    354                 btMatrix3x3 scaled(const btVector3& s) const
    355                 {
    356                         return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
    357                                                                          m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
    358                                                                          m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
    359                 }
    360 
    361   /**@brief Return the determinant of the matrix */
    362                 btScalar            determinant() const;
    363   /**@brief Return the adjoint of the matrix */
    364                 btMatrix3x3 adjoint() const;
    365   /**@brief Return the matrix with all values non negative */
    366                 btMatrix3x3 absolute() const;
    367   /**@brief Return the transpose of the matrix */
    368                 btMatrix3x3 transpose() const;
    369   /**@brief Return the inverse of the matrix */
    370                 btMatrix3x3 inverse() const;
    371                
    372                 btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
    373                 btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
    374                
    375                 SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
    376                 {
    377                         return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
    378                 }
    379                 SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
    380                 {
    381                         return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
    382                 }
    383                 SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
    384                 {
    385                         return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
    386                 }
    387                
    388 
    389   /**@brief diagonalizes this matrix by the Jacobi method.
    390    * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
    391    * coordinate system, i.e., old_this = rot * new_this * rot^T.
    392    * @param threshold See iteration
    393    * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
    394    * by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
    395    *
    396    * Note that this matrix is assumed to be symmetric.
    397    */
    398                 void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
    399                 {
    400                  rot.setIdentity();
    401                  for (int step = maxSteps; step > 0; step--)
    402                  {
    403416                        // find off-diagonal element [p][q] with largest magnitude
    404417                        int p = 0;
    … …  
    409422                        if (v > max)
    410423                        {
    411                            q = 2;
    412                            r = 1;
    413                            max = v;
     424                                q = 2;
     425                                r = 1;
     426                                max = v;
    414427                        }
    415428                        v = btFabs(m_el[1][2]);
    416429                        if (v > max)
    417430                        {
    418                            p = 1;
    419                            q = 2;
    420                            r = 0;
    421                            max = v;
     431                                p = 1;
     432                                q = 2;
     433                                r = 0;
     434                                max = v;
    422435                        }
    423436
    … …  
    425438                        if (max <= t)
    426439                        {
    427                            if (max <= SIMD_EPSILON * t)
    428                            {
    429                                   return;
    430                            }
    431                            step = 1;
     440                                if (max <= SIMD_EPSILON * t)
     441                                {
     442                                        return;
     443                                }
     444                                step = 1;
    432445                        }
    433446
    … …  
    440453                        if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
    441454                        {
    442                            t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
    443                                                                                 : 1 / (theta - btSqrt(1 + theta2));
    444                            cos = 1 / btSqrt(1 + t * t);
    445                            sin = cos * t;
     455                                t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
     456                                        : 1 / (theta - btSqrt(1 + theta2));
     457                                cos = 1 / btSqrt(1 + t * t);
     458                                sin = cos * t;
    446459                        }
    447460                        else
    448461                        {
    449                            // approximation for large theta-value, i.e., a nearly diagonal matrix
    450                            t = 1 / (theta * (2 + btScalar(0.5) / theta2));
    451                            cos = 1 - btScalar(0.5) * t * t;
    452                            sin = cos * t;
     462                                // approximation for large theta-value, i.e., a nearly diagonal matrix
     463                                t = 1 / (theta * (2 + btScalar(0.5) / theta2));
     464                                cos = 1 - btScalar(0.5) * t * t;
     465                                sin = cos * t;
    453466                        }
    454467
    … …  
    465478                        for (int i = 0; i < 3; i++)
    466479                        {
    467                            btVector3& row = rot[i];
    468                            mrp = row[p];
    469                            mrq = row[q];
    470                            row[p] = cos * mrp - sin * mrq;
    471                            row[q] = cos * mrq + sin * mrp;
    472                         }
    473                  }
     480                                btVector3& row = rot[i];
     481                                mrp = row[p];
     482                                mrq = row[q];
     483                                row[p] = cos * mrp - sin * mrq;
     484                                row[q] = cos * mrq + sin * mrp;
     485                        }
    474486                }
    475 
    476 
    477                
    478         protected:
    479   /**@brief Calculate the matrix cofactor
    480    * @param r1 The first row to use for calculating the cofactor
    481    * @param c1 The first column to use for calculating the cofactor
    482    * @param r1 The second row to use for calculating the cofactor
    483    * @param c1 The second column to use for calculating the cofactor
    484    * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
    485    */
    486                 btScalar cofac(int r1, int c1, int r2, int c2) const
    487                 {
    488                         return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
    489                 }
    490   ///Data storage for the matrix, each vector is a row of the matrix
    491                 btVector3 m_el[3];
    492         };
     487        }
     488
     489
     490
     491
     492        /**@brief Calculate the matrix cofactor
     493        * @param r1 The first row to use for calculating the cofactor
     494        * @param c1 The first column to use for calculating the cofactor
     495        * @param r1 The second row to use for calculating the cofactor
     496        * @param c1 The second column to use for calculating the cofactor
     497        * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
     498        */
     499        btScalar cofac(int r1, int c1, int r2, int c2) const
     500        {
     501                return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
     502        }
     503
     504        void    serialize(struct        btMatrix3x3Data& dataOut) const;
     505
     506        void    serializeFloat(struct   btMatrix3x3FloatData& dataOut) const;
     507
     508        void    deSerialize(const struct        btMatrix3x3Data& dataIn);
     509
     510        void    deSerializeFloat(const struct   btMatrix3x3FloatData& dataIn);
     511
     512        void    deSerializeDouble(const struct  btMatrix3x3DoubleData& dataIn);
     513
     514};
     515
     516
     517SIMD_FORCE_INLINE btMatrix3x3&
     518btMatrix3x3::operator*=(const btMatrix3x3& m)
     519{
     520        setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
     521                m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
     522                m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
     523        return *this;
     524}
     525
     526SIMD_FORCE_INLINE btScalar
     527btMatrix3x3::determinant() const
     528{
     529        return btTriple((*this)[0], (*this)[1], (*this)[2]);
     530}
     531
     532
     533SIMD_FORCE_INLINE btMatrix3x3
     534btMatrix3x3::absolute() const
     535{
     536        return btMatrix3x3(
     537                btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
     538                btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
     539                btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
     540}
     541
     542SIMD_FORCE_INLINE btMatrix3x3
     543btMatrix3x3::transpose() const
     544{
     545        return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
     546                m_el[0].y(), m_el[1].y(), m_el[2].y(),
     547                m_el[0].z(), m_el[1].z(), m_el[2].z());
     548}
     549
     550SIMD_FORCE_INLINE btMatrix3x3
     551btMatrix3x3::adjoint() const
     552{
     553        return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
     554                cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
     555                cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
     556}
     557
     558SIMD_FORCE_INLINE btMatrix3x3
     559btMatrix3x3::inverse() const
     560{
     561        btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
     562        btScalar det = (*this)[0].dot(co);
     563        btFullAssert(det != btScalar(0.0));
     564        btScalar s = btScalar(1.0) / det;
     565        return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
     566                co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
     567                co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
     568}
     569
     570SIMD_FORCE_INLINE btMatrix3x3
     571btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
     572{
     573        return btMatrix3x3(
     574                m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
     575                m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
     576                m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
     577                m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
     578                m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
     579                m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
     580                m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
     581                m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
     582                m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
     583}
     584
     585SIMD_FORCE_INLINE btMatrix3x3
     586btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
     587{
     588        return btMatrix3x3(
     589                m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
     590                m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
     591                m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
     592
     593}
     594
     595SIMD_FORCE_INLINE btVector3
     596operator*(const btMatrix3x3& m, const btVector3& v)
     597{
     598        return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
     599}
     600
     601
     602SIMD_FORCE_INLINE btVector3
     603operator*(const btVector3& v, const btMatrix3x3& m)
     604{
     605        return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
     606}
     607
     608SIMD_FORCE_INLINE btMatrix3x3
     609operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
     610{
     611        return btMatrix3x3(
     612                m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
     613                m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
     614                m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
     615}
     616
     617/*
     618SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
     619return btMatrix3x3(
     620m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
     621m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
     622m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
     623m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
     624m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
     625m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
     626m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
     627m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
     628m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
     629}
     630*/
     631
     632/**@brief Equality operator between two matrices
     633* It will test all elements are equal.  */
     634SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
     635{
     636        return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
     637                m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
     638                m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
     639}
     640
     641///for serialization
     642struct  btMatrix3x3FloatData
     643{
     644        btVector3FloatData m_el[3];
     645};
     646
     647///for serialization
     648struct  btMatrix3x3DoubleData
     649{
     650        btVector3DoubleData m_el[3];
     651};
     652
     653
    493654       
    494         SIMD_FORCE_INLINE btMatrix3x3&
    495         btMatrix3x3::operator*=(const btMatrix3x3& m)
    496         {
    497                 setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
    498                                  m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
    499                                  m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
    500                 return *this;
    501         }
    502        
    503         SIMD_FORCE_INLINE btScalar
    504         btMatrix3x3::determinant() const
    505         {
    506                 return triple((*this)[0], (*this)[1], (*this)[2]);
    507         }
    508        
    509 
    510         SIMD_FORCE_INLINE btMatrix3x3
    511         btMatrix3x3::absolute() const
    512         {
    513                 return btMatrix3x3(
    514                         btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
    515                         btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
    516                         btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
    517         }
    518 
    519         SIMD_FORCE_INLINE btMatrix3x3
    520         btMatrix3x3::transpose() const
    521         {
    522                 return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
    523                                                                  m_el[0].y(), m_el[1].y(), m_el[2].y(),
    524                                                                  m_el[0].z(), m_el[1].z(), m_el[2].z());
    525         }
    526        
    527         SIMD_FORCE_INLINE btMatrix3x3
    528         btMatrix3x3::adjoint() const
    529         {
    530                 return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
    531                                                                  cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
    532                                                                  cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
    533         }
    534        
    535         SIMD_FORCE_INLINE btMatrix3x3
    536         btMatrix3x3::inverse() const
    537         {
    538                 btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
    539                 btScalar det = (*this)[0].dot(co);
    540                 btFullAssert(det != btScalar(0.0));
    541                 btScalar s = btScalar(1.0) / det;
    542                 return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
    543                                                                  co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
    544                                                                  co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
    545         }
    546        
    547         SIMD_FORCE_INLINE btMatrix3x3
    548         btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
    549         {
    550                 return btMatrix3x3(
    551                         m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
    552                         m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
    553                         m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
    554                         m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
    555                         m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
    556                         m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
    557                         m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
    558                         m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
    559                         m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
    560         }
    561        
    562         SIMD_FORCE_INLINE btMatrix3x3
    563         btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
    564         {
    565                 return btMatrix3x3(
    566                         m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
    567                         m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
    568                         m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
    569                
    570         }
    571 
    572         SIMD_FORCE_INLINE btVector3
    573         operator*(const btMatrix3x3& m, const btVector3& v)
    574         {
    575                 return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
    576         }
    577        
    578 
    579         SIMD_FORCE_INLINE btVector3
    580         operator*(const btVector3& v, const btMatrix3x3& m)
    581         {
    582                 return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
    583         }
    584 
    585         SIMD_FORCE_INLINE btMatrix3x3
    586         operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
    587         {
    588                 return btMatrix3x3(
    589                         m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
    590                         m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
    591                         m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
    592         }
    593 
    594 /*
    595         SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
    596     return btMatrix3x3(
    597         m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
    598         m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
    599         m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
    600         m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
    601         m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
    602         m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
    603         m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
    604         m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
    605         m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
    606 }
    607 */
    608 
    609 /**@brief Equality operator between two matrices
    610  * It will test all elements are equal.  */
    611 SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
    612 {
    613    return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
    614             m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
    615             m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
    616 }
    617 
    618 #endif
     655
     656SIMD_FORCE_INLINE       void    btMatrix3x3::serialize(struct   btMatrix3x3Data& dataOut) const
     657{
     658        for (int i=0;i<3;i++)
     659                m_el[i].serialize(dataOut.m_el[i]);
     660}
     661
     662SIMD_FORCE_INLINE       void    btMatrix3x3::serializeFloat(struct      btMatrix3x3FloatData& dataOut) const
     663{
     664        for (int i=0;i<3;i++)
     665                m_el[i].serializeFloat(dataOut.m_el[i]);
     666}
     667
     668
     669SIMD_FORCE_INLINE       void    btMatrix3x3::deSerialize(const struct   btMatrix3x3Data& dataIn)
     670{
     671        for (int i=0;i<3;i++)
     672                m_el[i].deSerialize(dataIn.m_el[i]);
     673}
     674
     675SIMD_FORCE_INLINE       void    btMatrix3x3::deSerializeFloat(const struct      btMatrix3x3FloatData& dataIn)
     676{
     677        for (int i=0;i<3;i++)
     678                m_el[i].deSerializeFloat(dataIn.m_el[i]);
     679}
     680
     681SIMD_FORCE_INLINE       void    btMatrix3x3::deSerializeDouble(const struct     btMatrix3x3DoubleData& dataIn)
     682{
     683        for (int i=0;i<3;i++)
     684                m_el[i].deSerializeDouble(dataIn.m_el[i]);
     685}
     686
     687#endif //BT_MATRIX3x3_H
     688

  • code/branches/kicklib/src/external/bullet/LinearMath/btMinMax.h

    r5781 r7983  
    1818#define GEN_MINMAX_H
    1919
     20#include "LinearMath/btScalar.h"
     21
    2022template <class T>
    2123SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b)
    … …  
    3133
    3234template <class T>
    33 SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub)
     35SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub)
    3436{
    3537        return a < lb ? lb : (ub < a ? ub : a);
    … …  
    5557
    5658template <class T>
    57 SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub)
     59SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub)
    5860{
    5961        if (a < lb)

  • code/branches/kicklib/src/external/bullet/LinearMath/btPoolAllocator.h

    r5781 r7983  
    5858        }
    5959
     60        int getUsedCount() const
     61        {
     62                return m_maxElements - m_freeCount;
     63        }
     64
    6065        void*   allocate(int size)
    6166        {
    … …  
    97102        }
    98103
     104        unsigned char*  getPoolAddress()
     105        {
     106                return m_pool;
     107        }
     108
     109        const unsigned char*    getPoolAddress() const
     110        {
     111                return m_pool;
     112        }
    99113
    100114};

  • code/branches/kicklib/src/external/bullet/LinearMath/btQuaternion.h

    r5781 r7983  
    210210        }
    211211
    212 
    213   /**@brief Return the inverse of this quaternion */
     212        /**@brief Return the axis of the rotation represented by this quaternion */
     213        btVector3 getAxis() const
     214        {
     215                btScalar s_squared = btScalar(1.) - btPow(m_floats[3], btScalar(2.));
     216                if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero
     217                        return btVector3(1.0, 0.0, 0.0);  // Arbitrary
     218                btScalar s = btSqrt(s_squared);
     219                return btVector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
     220        }
     221
     222        /**@brief Return the inverse of this quaternion */
    214223        btQuaternion inverse() const
    215224        {
    … …  
    253262        }
    254263
     264        /**@todo document this and it's use */
     265        SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const
     266        {
     267                btQuaternion diff,sum;
     268                diff = *this - qd;
     269                sum = *this + qd;
     270                if( diff.dot(diff) < sum.dot(sum) )
     271                        return qd;
     272                return (-qd);
     273        }
     274
     275
    255276  /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
    256277   * @param q The other quaternion to interpolate with
    … …  
    265286                        btScalar s0 = btSin((btScalar(1.0) - t) * theta);
    266287                        btScalar s1 = btSin(t * theta);   
    267                         return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d,
    268                                 (m_floats[1] * s0 + q.y() * s1) * d,
    269                                 (m_floats[2] * s0 + q.z() * s1) * d,
    270                                 (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
     288                        if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
     289                          return btQuaternion((m_floats[0] * s0 + -q.x() * s1) * d,
     290                                              (m_floats[1] * s0 + -q.y() * s1) * d,
     291                                              (m_floats[2] * s0 + -q.z() * s1) * d,
     292                                              (m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
     293                        else
     294                          return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d,
     295                                              (m_floats[1] * s0 + q.y() * s1) * d,
     296                                              (m_floats[2] * s0 + q.z() * s1) * d,
     297                                              (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
     298                       
    271299                }
    272300                else
    … …  
    379407
    380408        if (d < -1.0 + SIMD_EPSILON)
    381                 return btQuaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector
     409        {
     410                btVector3 n,unused;
     411                btPlaneSpace1(v0,n,unused);
     412                return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
     413        }
    382414
    383415        btScalar  s = btSqrt((1.0f + d) * 2.0f);

  • code/branches/kicklib/src/external/bullet/LinearMath/btQuickprof.cpp

    r5781 r7983  
    11/*
    22
    3 /***************************************************************************************************
     3***************************************************************************************************
    44**
    55** profile.cpp
    … …  
    1414// Ogre (www.ogre3d.org).
    1515
    16 #include "LinearMath/btQuickprof.h"
    17 
    18 
    19 #ifdef USE_BT_CLOCK
     16#include "btQuickprof.h"
     17
     18#ifndef BT_NO_PROFILE
     19
    2020
    2121static btClock gProfileClock;
     22
     23
     24#ifdef __CELLOS_LV2__
     25#include <sys/sys_time.h>
     26#include <sys/time_util.h>
     27#include <stdio.h>
     28#endif
     29
     30#if defined (SUNOS) || defined (__SUNOS__)
     31#include <stdio.h>
     32#endif
     33
     34#if defined(WIN32) || defined(_WIN32)
     35
     36#define BT_USE_WINDOWS_TIMERS
     37#define WIN32_LEAN_AND_MEAN
     38#define NOWINRES
     39#define NOMCX
     40#define NOIME
     41
     42#ifdef _XBOX
     43        #include <Xtl.h>
     44#else //_XBOX
     45        #include <windows.h>
     46#endif //_XBOX
     47
     48#include <time.h>
     49
     50
     51#else //_WIN32
     52#include <sys/time.h>
     53#endif //_WIN32
     54
     55#define mymin(a,b) (a > b ? a : b)
     56
     57struct btClockData
     58{
     59
     60#ifdef BT_USE_WINDOWS_TIMERS
     61        LARGE_INTEGER mClockFrequency;
     62        DWORD mStartTick;
     63        LONGLONG mPrevElapsedTime;
     64        LARGE_INTEGER mStartTime;
     65#else
     66#ifdef __CELLOS_LV2__
     67        uint64_t        mStartTime;
     68#else
     69        struct timeval mStartTime;
     70#endif
     71#endif //__CELLOS_LV2__
     72
     73};
     74
     75///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
     76btClock::btClock()
     77{
     78        m_data = new btClockData;
     79#ifdef BT_USE_WINDOWS_TIMERS
     80        QueryPerformanceFrequency(&m_data->mClockFrequency);
     81#endif
     82        reset();
     83}
     84
     85btClock::~btClock()
     86{
     87        delete m_data;
     88}
     89
     90btClock::btClock(const btClock& other)
     91{
     92        m_data = new btClockData;
     93        *m_data = *other.m_data;
     94}
     95
     96btClock& btClock::operator=(const btClock& other)
     97{
     98        *m_data = *other.m_data;
     99        return *this;
     100}
     101
     102
     103        /// Resets the initial reference time.
     104void btClock::reset()
     105{
     106#ifdef BT_USE_WINDOWS_TIMERS
     107        QueryPerformanceCounter(&m_data->mStartTime);
     108        m_data->mStartTick = GetTickCount();
     109        m_data->mPrevElapsedTime = 0;
     110#else
     111#ifdef __CELLOS_LV2__
     112
     113        typedef uint64_t  ClockSize;
     114        ClockSize newTime;
     115        //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
     116        SYS_TIMEBASE_GET( newTime );
     117        m_data->mStartTime = newTime;
     118#else
     119        gettimeofday(&m_data->mStartTime, 0);
     120#endif
     121#endif
     122}
     123
     124/// Returns the time in ms since the last call to reset or since
     125/// the btClock was created.
     126unsigned long int btClock::getTimeMilliseconds()
     127{
     128#ifdef BT_USE_WINDOWS_TIMERS
     129        LARGE_INTEGER currentTime;
     130        QueryPerformanceCounter(&currentTime);
     131        LONGLONG elapsedTime = currentTime.QuadPart -
     132                m_data->mStartTime.QuadPart;
     133                // Compute the number of millisecond ticks elapsed.
     134        unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
     135                m_data->mClockFrequency.QuadPart);
     136                // Check for unexpected leaps in the Win32 performance counter. 
     137        // (This is caused by unexpected data across the PCI to ISA
     138                // bridge, aka south bridge.  See Microsoft KB274323.)
     139                unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick;
     140                signed long msecOff = (signed long)(msecTicks - elapsedTicks);
     141                if (msecOff < -100 || msecOff > 100)
     142                {
     143                        // Adjust the starting time forwards.
     144                        LONGLONG msecAdjustment = mymin(msecOff *
     145                                m_data->mClockFrequency.QuadPart / 1000, elapsedTime -
     146                                m_data->mPrevElapsedTime);
     147                        m_data->mStartTime.QuadPart += msecAdjustment;
     148                        elapsedTime -= msecAdjustment;
     149
     150                        // Recompute the number of millisecond ticks elapsed.
     151                        msecTicks = (unsigned long)(1000 * elapsedTime /
     152                                m_data->mClockFrequency.QuadPart);
     153                }
     154
     155                // Store the current elapsed time for adjustments next time.
     156                m_data->mPrevElapsedTime = elapsedTime;
     157
     158                return msecTicks;
     159#else
     160
     161#ifdef __CELLOS_LV2__
     162                uint64_t freq=sys_time_get_timebase_frequency();
     163                double dFreq=((double) freq) / 1000.0;
     164                typedef uint64_t  ClockSize;
     165                ClockSize newTime;
     166                SYS_TIMEBASE_GET( newTime );
     167                //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
     168
     169                return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq);
     170#else
     171
     172                struct timeval currentTime;
     173                gettimeofday(&currentTime, 0);
     174                return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 +
     175                        (currentTime.tv_usec - m_data->mStartTime.tv_usec) / 1000;
     176#endif //__CELLOS_LV2__
     177#endif
     178}
     179
     180        /// Returns the time in us since the last call to reset or since
     181        /// the Clock was created.
     182unsigned long int btClock::getTimeMicroseconds()
     183{
     184#ifdef BT_USE_WINDOWS_TIMERS
     185                LARGE_INTEGER currentTime;
     186                QueryPerformanceCounter(&currentTime);
     187                LONGLONG elapsedTime = currentTime.QuadPart -
     188                        m_data->mStartTime.QuadPart;
     189
     190                // Compute the number of millisecond ticks elapsed.
     191                unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
     192                        m_data->mClockFrequency.QuadPart);
     193
     194                // Check for unexpected leaps in the Win32 performance counter. 
     195                // (This is caused by unexpected data across the PCI to ISA
     196                // bridge, aka south bridge.  See Microsoft KB274323.)
     197                unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick;
     198                signed long msecOff = (signed long)(msecTicks - elapsedTicks);
     199                if (msecOff < -100 || msecOff > 100)
     200                {
     201                        // Adjust the starting time forwards.
     202                        LONGLONG msecAdjustment = mymin(msecOff *
     203                                m_data->mClockFrequency.QuadPart / 1000, elapsedTime -
     204                                m_data->mPrevElapsedTime);
     205                        m_data->mStartTime.QuadPart += msecAdjustment;
     206                        elapsedTime -= msecAdjustment;
     207                }
     208
     209                // Store the current elapsed time for adjustments next time.
     210                m_data->mPrevElapsedTime = elapsedTime;
     211
     212                // Convert to microseconds.
     213                unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime /
     214                        m_data->mClockFrequency.QuadPart);
     215
     216                return usecTicks;
     217#else
     218
     219#ifdef __CELLOS_LV2__
     220                uint64_t freq=sys_time_get_timebase_frequency();
     221                double dFreq=((double) freq)/ 1000000.0;
     222                typedef uint64_t  ClockSize;
     223                ClockSize newTime;
     224                //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
     225                SYS_TIMEBASE_GET( newTime );
     226
     227                return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq);
     228#else
     229
     230                struct timeval currentTime;
     231                gettimeofday(&currentTime, 0);
     232                return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 +
     233                        (currentTime.tv_usec - m_data->mStartTime.tv_usec);
     234#endif//__CELLOS_LV2__
     235#endif
     236}
     237
     238
     239
     240
    22241
    23242inline void Profile_Get_Ticks(unsigned long int * ticks)
    … …  
    343562
    344563
    345 #endif //USE_BT_CLOCK
    346 
     564
     565#endif //BT_NO_PROFILE

  • code/branches/kicklib/src/external/bullet/LinearMath/btQuickprof.h

    r5781 r7983  
    1919//#define BT_NO_PROFILE 1
    2020#ifndef BT_NO_PROFILE
    21 
     21#include <stdio.h>//@todo remove this, backwards compatibility
    2222#include "btScalar.h"
    23 #include "LinearMath/btAlignedAllocator.h"
     23#include "btAlignedAllocator.h"
    2424#include <new>
    2525
    … …  
    2727
    2828
    29 //if you don't need btClock, you can comment next line
     29
    3030#define USE_BT_CLOCK 1
    3131
    3232#ifdef USE_BT_CLOCK
    33 #ifdef __CELLOS_LV2__
    34 #include <sys/sys_time.h>
    35 #include <sys/time_util.h>
    36 #include <stdio.h>
    37 #endif
    38 
    39 #if defined (SUNOS) || defined (__SUNOS__)
    40 #include <stdio.h>
    41 #endif
    42 
    43 #if defined(WIN32) || defined(_WIN32)
    44 
    45 #define USE_WINDOWS_TIMERS
    46 #define WIN32_LEAN_AND_MEAN
    47 #define NOWINRES
    48 #define NOMCX
    49 #define NOIME
    50 #ifdef _XBOX
    51 #include <Xtl.h>
    52 #else
    53 #include <windows.h>
    54 #endif
    55 #include <time.h>
    56 
    57 #else
    58 #include <sys/time.h>
    59 #endif
    60 
    61 #define mymin(a,b) (a > b ? a : b)
    6233
    6334///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
    … …  
    6536{
    6637public:
    67         btClock()
    68         {
    69 #ifdef USE_WINDOWS_TIMERS
    70                 QueryPerformanceFrequency(&mClockFrequency);
    71 #endif
    72                 reset();
    73         }
     38        btClock();
    7439
    75         ~btClock()
    76         {
    77         }
     40        btClock(const btClock& other);
     41        btClock& operator=(const btClock& other);
     42
     43        ~btClock();
    7844
    7945        /// Resets the initial reference time.
    80         void reset()
    81         {
    82 #ifdef USE_WINDOWS_TIMERS
    83                 QueryPerformanceCounter(&mStartTime);
    84                 mStartTick = GetTickCount();
    85                 mPrevElapsedTime = 0;
    86 #else
    87 #ifdef __CELLOS_LV2__
    88 
    89                 typedef uint64_t  ClockSize;
    90                 ClockSize newTime;
    91                 //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
    92                 SYS_TIMEBASE_GET( newTime );
    93                 mStartTime = newTime;
    94 #else
    95                 gettimeofday(&mStartTime, 0);
    96 #endif
    97 
    98 #endif
    99         }
     46        void reset();
    10047
    10148        /// Returns the time in ms since the last call to reset or since
    10249        /// the btClock was created.
    103         unsigned long int getTimeMilliseconds()
    104         {
    105 #ifdef USE_WINDOWS_TIMERS
    106                 LARGE_INTEGER currentTime;
    107                 QueryPerformanceCounter(&currentTime);
    108                 LONGLONG elapsedTime = currentTime.QuadPart -
    109                         mStartTime.QuadPart;
    110 
    111                 // Compute the number of millisecond ticks elapsed.
    112                 unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
    113                         mClockFrequency.QuadPart);
    114 
    115                 // Check for unexpected leaps in the Win32 performance counter. 
    116                 // (This is caused by unexpected data across the PCI to ISA
    117                 // bridge, aka south bridge.  See Microsoft KB274323.)
    118                 unsigned long elapsedTicks = GetTickCount() - mStartTick;
    119                 signed long msecOff = (signed long)(msecTicks - elapsedTicks);
    120                 if (msecOff < -100 || msecOff > 100)
    121                 {
    122                         // Adjust the starting time forwards.
    123                         LONGLONG msecAdjustment = mymin(msecOff *
    124                                 mClockFrequency.QuadPart / 1000, elapsedTime -
    125                                 mPrevElapsedTime);
    126                         mStartTime.QuadPart += msecAdjustment;
    127                         elapsedTime -= msecAdjustment;
    128 
    129                         // Recompute the number of millisecond ticks elapsed.
    130                         msecTicks = (unsigned long)(1000 * elapsedTime /
    131                                 mClockFrequency.QuadPart);
    132                 }
    133 
    134                 // Store the current elapsed time for adjustments next time.
    135                 mPrevElapsedTime = elapsedTime;
    136 
    137                 return msecTicks;
    138 #else
    139 
    140 #ifdef __CELLOS_LV2__
    141                 uint64_t freq=sys_time_get_timebase_frequency();
    142                 double dFreq=((double) freq) / 1000.0;
    143                 typedef uint64_t  ClockSize;
    144                 ClockSize newTime;
    145                 SYS_TIMEBASE_GET( newTime );
    146                 //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
    147 
    148                 return (unsigned long int)((double(newTime-mStartTime)) / dFreq);
    149 #else
    150 
    151                 struct timeval currentTime;
    152                 gettimeofday(&currentTime, 0);
    153                 return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 +
    154                         (currentTime.tv_usec - mStartTime.tv_usec) / 1000;
    155 #endif //__CELLOS_LV2__
    156 #endif
    157         }
     50        unsigned long int getTimeMilliseconds();
    15851
    15952        /// Returns the time in us since the last call to reset or since
    16053        /// the Clock was created.
    161         unsigned long int getTimeMicroseconds()
    162         {
    163 #ifdef USE_WINDOWS_TIMERS
    164                 LARGE_INTEGER currentTime;
    165                 QueryPerformanceCounter(&currentTime);
    166                 LONGLONG elapsedTime = currentTime.QuadPart -
    167                         mStartTime.QuadPart;
    168 
    169                 // Compute the number of millisecond ticks elapsed.
    170                 unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
    171                         mClockFrequency.QuadPart);
    172 
    173                 // Check for unexpected leaps in the Win32 performance counter. 
    174                 // (This is caused by unexpected data across the PCI to ISA
    175                 // bridge, aka south bridge.  See Microsoft KB274323.)
    176                 unsigned long elapsedTicks = GetTickCount() - mStartTick;
    177                 signed long msecOff = (signed long)(msecTicks - elapsedTicks);
    178                 if (msecOff < -100 || msecOff > 100)
    179                 {
    180                         // Adjust the starting time forwards.
    181                         LONGLONG msecAdjustment = mymin(msecOff *
    182                                 mClockFrequency.QuadPart / 1000, elapsedTime -
    183                                 mPrevElapsedTime);
    184                         mStartTime.QuadPart += msecAdjustment;
    185                         elapsedTime -= msecAdjustment;
    186                 }
    187 
    188                 // Store the current elapsed time for adjustments next time.
    189                 mPrevElapsedTime = elapsedTime;
    190 
    191                 // Convert to microseconds.
    192                 unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime /
    193                         mClockFrequency.QuadPart);
    194 
    195                 return usecTicks;
    196 #else
    197 
    198 #ifdef __CELLOS_LV2__
    199                 uint64_t freq=sys_time_get_timebase_frequency();
    200                 double dFreq=((double) freq)/ 1000000.0;
    201                 typedef uint64_t  ClockSize;
    202                 ClockSize newTime;
    203                 //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
    204                 SYS_TIMEBASE_GET( newTime );
    205 
    206                 return (unsigned long int)((double(newTime-mStartTime)) / dFreq);
    207 #else
    208 
    209                 struct timeval currentTime;
    210                 gettimeofday(&currentTime, 0);
    211                 return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 +
    212                         (currentTime.tv_usec - mStartTime.tv_usec);
    213 #endif//__CELLOS_LV2__
    214 #endif
    215         }
    216 
     54        unsigned long int getTimeMicroseconds();
    21755private:
    218 #ifdef USE_WINDOWS_TIMERS
    219         LARGE_INTEGER mClockFrequency;
    220         DWORD mStartTick;
    221         LONGLONG mPrevElapsedTime;
    222         LARGE_INTEGER mStartTime;
    223 #else
    224 #ifdef __CELLOS_LV2__
    225         uint64_t        mStartTime;
    226 #else
    227         struct timeval mStartTime;
    228 #endif
    229 #endif //__CELLOS_LV2__
    230 
     56        struct btClockData* m_data;
    23157};
    23258

  • code/branches/kicklib/src/external/bullet/LinearMath/btScalar.h

    r5781 r7983  
    11/*
    2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
     2Copyright (c) 2003-2009 Erwin Coumans  http://bullet.googlecode.com
    33
    44This software is provided 'as-is', without any express or implied warranty.
    … …  
    1818#define SIMD___SCALAR_H
    1919
     20#ifdef BT_MANAGED_CODE
     21//Aligned data types not supported in managed code
     22#pragma unmanaged
     23#endif
     24
     25
    2026#include <math.h>
    21 
    2227#include <stdlib.h>//size_t for MSVC 6.0
    23 
    2428#include <cstdlib>
    2529#include <cfloat>
    2630#include <float.h>
    2731
    28 #define BT_BULLET_VERSION 274
     32/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
     33#define BT_BULLET_VERSION 277
    2934
    3035inline int      btGetVersion()
    … …  
    3843
    3944
    40 #ifdef WIN32
     45#ifdef _WIN32
    4146
    4247                #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
    … …  
    4449                        #define SIMD_FORCE_INLINE inline
    4550                        #define ATTRIBUTE_ALIGNED16(a) a
     51                        #define ATTRIBUTE_ALIGNED64(a) a
    4652                        #define ATTRIBUTE_ALIGNED128(a) a
    4753                #else
    … …  
    5460                        #define SIMD_FORCE_INLINE __forceinline
    5561                        #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
     62                        #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
    5663                        #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
    5764                #ifdef _XBOX
    … …  
    6370                #else
    6471
    65 #if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
     72#if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
    6673                        #define BT_USE_SSE
    6774                        #include <emmintrin.h>
    … …  
    8794       
    8895#if defined     (__CELLOS_LV2__)
    89                 #define SIMD_FORCE_INLINE inline
     96                #define SIMD_FORCE_INLINE inline __attribute__((always_inline))
    9097                #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
     98                #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
     99                #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
     100                #ifndef assert
     101                #include <assert.h>
     102                #endif
     103#ifdef BT_DEBUG
     104#ifdef __SPU__
     105#include <spu_printf.h>
     106#define printf spu_printf
     107        #define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
     108#else
     109        #define btAssert assert
     110#endif
     111       
     112#else
     113                #define btAssert(x)
     114#endif
     115                //btFullAssert is optional, slows down a lot
     116                #define btFullAssert(x)
     117
     118                #define btLikely(_c)  _c
     119                #define btUnlikely(_c) _c
     120
     121#else
     122
     123#ifdef USE_LIBSPE2
     124
     125                #define SIMD_FORCE_INLINE __inline
     126                #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
     127                #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
    91128                #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
    92129                #ifndef assert
    … …  
    101138                #define btFullAssert(x)
    102139
    103                 #define btLikely(_c)  _c
    104                 #define btUnlikely(_c) _c
    105 
    106 #else
    107 
    108 #ifdef USE_LIBSPE2
    109 
    110                 #define SIMD_FORCE_INLINE __inline
    111                 #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
    112                 #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
    113                 #ifndef assert
    114                 #include <assert.h>
    115                 #endif
    116 #ifdef BT_DEBUG
    117                 #define btAssert assert
    118 #else
    119                 #define btAssert(x)
    120 #endif
    121                 //btFullAssert is optional, slows down a lot
    122                 #define btFullAssert(x)
    123 
    124140
    125141                #define btLikely(_c)   __builtin_expect((_c), 1)
    … …  
    130146        //non-windows systems
    131147
     148#if (defined (__APPLE__) && defined (__i386__) && (!defined (BT_USE_DOUBLE_PRECISION)))
     149        #define BT_USE_SSE
     150        #include <emmintrin.h>
     151
     152        #define SIMD_FORCE_INLINE inline
     153///@todo: check out alignment methods for other platforms/compilers
     154        #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
     155        #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
     156        #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
     157        #ifndef assert
     158        #include <assert.h>
     159        #endif
     160
     161        #if defined(DEBUG) || defined (_DEBUG)
     162                #define btAssert assert
     163        #else
     164                #define btAssert(x)
     165        #endif
     166
     167        //btFullAssert is optional, slows down a lot
     168        #define btFullAssert(x)
     169        #define btLikely(_c)  _c
     170        #define btUnlikely(_c) _c
     171
     172#else
     173
    132174                #define SIMD_FORCE_INLINE inline
    133175                ///@todo: check out alignment methods for other platforms/compilers
    134176                ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
     177                ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
    135178                ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
    136179                #define ATTRIBUTE_ALIGNED16(a) a
     180                #define ATTRIBUTE_ALIGNED64(a) a
    137181                #define ATTRIBUTE_ALIGNED128(a) a
    138182                #ifndef assert
    … …  
    150194                #define btLikely(_c)  _c
    151195                #define btUnlikely(_c) _c
    152 
     196#endif //__APPLE__
    153197
    154198#endif // LIBSPE2
    … …  
    157201#endif
    158202
    159 /// older compilers (gcc 3.x) and Sun needs double version of sqrt etc.
    160 /// exclude Apple Intel (i's assumed to be a Macbook or new Intel Dual Core Processor)
    161 #if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__))
    162 //use slow double float precision operation on those platforms
    163 #ifndef BT_USE_DOUBLE_PRECISION
    164 #define BT_FORCE_DOUBLE_FUNCTIONS
    165 #endif
    166 #endif
    167203
    168204///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
    169205#if defined(BT_USE_DOUBLE_PRECISION)
    170206typedef double btScalar;
     207//this number could be bigger in double precision
     208#define BT_LARGE_FLOAT 1e30
    171209#else
    172210typedef float btScalar;
     211//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
     212#define BT_LARGE_FLOAT 1e18f
    173213#endif
    174214
    … …  
    194234SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
    195235SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
    196 SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acos(x); }
    197 SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asin(x); }
     236SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { if (x<btScalar(-1))     x=btScalar(-1); if (x>btScalar(1))      x=btScalar(1); return acos(x); }
     237SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { if (x<btScalar(-1))     x=btScalar(-1); if (x>btScalar(1))      x=btScalar(1); return asin(x); }
    198238SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
    199239SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
    … …  
    201241SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
    202242SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
     243SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); }
    203244
    204245#else
    … …  
    213254        *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
    214255        x =  tempf;
    215         z =  y*btScalar(0.5);                        /* hoist out the “/2”    */
     256        z =  y*btScalar(0.5);
    216257        x = (btScalar(1.5)*x)-(x*x)*(x*z);         /* iteration formula     */
    217258        x = (btScalar(1.5)*x)-(x*x)*(x*z);
    … …  
    229270SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
    230271SIMD_FORCE_INLINE btScalar btAcos(btScalar x) {
    231         btAssert(x <= btScalar(1.));
     272        if (x<btScalar(-1))     
     273                x=btScalar(-1);
     274        if (x>btScalar(1))     
     275                x=btScalar(1);
    232276        return acosf(x);
    233277}
    234 SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); }
     278SIMD_FORCE_INLINE btScalar btAsin(btScalar x) {
     279        if (x<btScalar(-1))     
     280                x=btScalar(-1);
     281        if (x>btScalar(1))     
     282                x=btScalar(1);
     283        return asinf(x);
     284}
    235285SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
    236286SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
    … …  
    242292  SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
    243293  #endif
     294SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
    244295       
    245296#endif
    … …  
    250301#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
    251302#define SIMD_DEGS_PER_RAD  (btScalar(360.0) / SIMD_2_PI)
     303#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
     304
     305#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))          /* reciprocal square root */
     306
    252307
    253308#ifdef BT_USE_DOUBLE_PRECISION
    … …  
    440495}
    441496
    442 
     497// returns normalized value in range [-SIMD_PI, SIMD_PI]
     498SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
     499{
     500        angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
     501        if(angleInRadians < -SIMD_PI)
     502        {
     503                return angleInRadians + SIMD_2_PI;
     504        }
     505        else if(angleInRadians > SIMD_PI)
     506        {
     507                return angleInRadians - SIMD_2_PI;
     508        }
     509        else
     510        {
     511                return angleInRadians;
     512        }
     513}
     514
     515///rudimentary class to provide type info
     516struct btTypedObject
     517{
     518        btTypedObject(int objectType)
     519                :m_objectType(objectType)
     520        {
     521        }
     522        int     m_objectType;
     523        inline int getObjectType() const
     524        {
     525                return m_objectType;
     526        }
     527};
    443528#endif //SIMD___SCALAR_H

  • code/branches/kicklib/src/external/bullet/LinearMath/btTransform.h

    r5781 r7983  
    1818#define btTransform_H
    1919
    20 #include "btVector3.h"
     20
    2121#include "btMatrix3x3.h"
     22
     23#ifdef BT_USE_DOUBLE_PRECISION
     24#define btTransformData btTransformDoubleData
     25#else
     26#define btTransformData btTransformFloatData
     27#endif
     28
     29
    2230
    2331
    … …  
    2634class btTransform {
    2735       
     36  ///Storage for the rotation
     37        btMatrix3x3 m_basis;
     38  ///Storage for the translation
     39        btVector3   m_origin;
    2840
    2941public:
    … …  
    196208                return identityTransform;
    197209        }
    198        
    199 private:
    200   ///Storage for the rotation
    201         btMatrix3x3 m_basis;
    202   ///Storage for the translation
    203         btVector3   m_origin;
     210
     211        void    serialize(struct        btTransformData& dataOut) const;
     212
     213        void    serializeFloat(struct   btTransformFloatData& dataOut) const;
     214
     215        void    deSerialize(const struct        btTransformData& dataIn);
     216
     217        void    deSerializeDouble(const struct  btTransformDoubleData& dataIn);
     218
     219        void    deSerializeFloat(const struct   btTransformFloatData& dataIn);
     220
    204221};
    205222
    … …  
    235252
    236253
     254///for serialization
     255struct  btTransformFloatData
     256{
     257        btMatrix3x3FloatData    m_basis;
     258        btVector3FloatData      m_origin;
     259};
     260
     261struct  btTransformDoubleData
     262{
     263        btMatrix3x3DoubleData   m_basis;
     264        btVector3DoubleData     m_origin;
     265};
     266
     267
     268
     269SIMD_FORCE_INLINE       void    btTransform::serialize(btTransformData& dataOut) const
     270{
     271        m_basis.serialize(dataOut.m_basis);
     272        m_origin.serialize(dataOut.m_origin);
     273}
     274
     275SIMD_FORCE_INLINE       void    btTransform::serializeFloat(btTransformFloatData& dataOut) const
     276{
     277        m_basis.serializeFloat(dataOut.m_basis);
     278        m_origin.serializeFloat(dataOut.m_origin);
     279}
     280
     281
     282SIMD_FORCE_INLINE       void    btTransform::deSerialize(const btTransformData& dataIn)
     283{
     284        m_basis.deSerialize(dataIn.m_basis);
     285        m_origin.deSerialize(dataIn.m_origin);
     286}
     287
     288SIMD_FORCE_INLINE       void    btTransform::deSerializeFloat(const btTransformFloatData& dataIn)
     289{
     290        m_basis.deSerializeFloat(dataIn.m_basis);
     291        m_origin.deSerializeFloat(dataIn.m_origin);
     292}
     293
     294SIMD_FORCE_INLINE       void    btTransform::deSerializeDouble(const btTransformDoubleData& dataIn)
     295{
     296        m_basis.deSerializeDouble(dataIn.m_basis);
     297        m_origin.deSerializeDouble(dataIn.m_origin);
     298}
     299
     300
    237301#endif
    238302

  • code/branches/kicklib/src/external/bullet/LinearMath/btTransformUtil.h

    r5781 r7983  
    2222
    2323
    24 #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
    25 
    26 #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))          /* reciprocal square root */
    2724
    2825SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
    … …  
    3431
    3532
    36 SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
    37 {
    38   if (btFabs(n.z()) > SIMDSQRT12) {
    39     // choose p in y-z plane
    40     btScalar a = n[1]*n[1] + n[2]*n[2];
    41     btScalar k = btRecipSqrt (a);
    42     p.setValue(0,-n[2]*k,n[1]*k);
    43     // set q = n x p
    44     q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
    45   }
    46   else {
    47     // choose p in x-y plane
    48     btScalar a = n.x()*n.x() + n.y()*n.y();
    49     btScalar k = btRecipSqrt (a);
    50     p.setValue(-n.y()*k,n.x()*k,0);
    51     // set q = n x p
    52     q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
    53   }
    54 }
     33
    5534
    5635
    … …  
    11897        static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
    11998        {
    120                 btQuaternion orn1 = orn0.farthest(orn1a);
     99                btQuaternion orn1 = orn0.nearest(orn1a);
    121100                btQuaternion dorn = orn1 * orn0.inverse();
    122                 ///floating point inaccuracy can lead to w component > 1..., which breaks
    123                 dorn.normalize();
    124101                angle = dorn.getAngle();
    125102                axis = btVector3(dorn.x(),dorn.y(),dorn.z());
    … …  
    210187                        btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
    211188                        btVector3 relLinVel = (linVelB-linVelA);
    212                         btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal);
     189                        btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
    213190                        if (relLinVelocLength<0.f)
    214191                        {
    … …  
    228205        void    initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
    229206        {
    230                 m_separatingNormal = separatingVector;
    231207                m_separatingDistance = separatingDistance;
    232                
    233                 const btVector3& toPosA = transA.getOrigin();
    234                 const btVector3& toPosB = transB.getOrigin();
    235                 btQuaternion toOrnA = transA.getRotation();
    236                 btQuaternion toOrnB = transB.getRotation();
    237                 m_posA = toPosA;
    238                 m_posB = toPosB;
    239                 m_ornA = toOrnA;
    240                 m_ornB = toOrnB;
     208
     209                if (m_separatingDistance>0.f)
     210                {
     211                        m_separatingNormal = separatingVector;
     212                       
     213                        const btVector3& toPosA = transA.getOrigin();
     214                        const btVector3& toPosB = transB.getOrigin();
     215                        btQuaternion toOrnA = transA.getRotation();
     216                        btQuaternion toOrnB = transB.getRotation();
     217                        m_posA = toPosA;
     218                        m_posB = toPosB;
     219                        m_ornA = toOrnA;
     220                        m_ornB = toOrnB;
     221                }
    241222        }
    242223

  • code/branches/kicklib/src/external/bullet/LinearMath/btVector3.h

    r5781 r7983  
    2020
    2121#include "btScalar.h"
    22 #include "btScalar.h"
    2322#include "btMinMax.h"
     23
     24#ifdef BT_USE_DOUBLE_PRECISION
     25#define btVector3Data btVector3DoubleData
     26#define btVector3DataName "btVector3DoubleData"
     27#else
     28#define btVector3Data btVector3FloatData
     29#define btVector3DataName "btVector3FloatData"
     30#endif //BT_USE_DOUBLE_PRECISION
     31
     32
     33
     34
    2435/**@brief btVector3 can be used to represent 3D points and vectors.
    2536 * 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
    2637 * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
    2738 */
    28 
    2939ATTRIBUTE_ALIGNED16(class) btVector3
    3040{
    … …  
    3242
    3343#if defined (__SPU__) && defined (__CELLOS_LV2__)
    34         union {
    35                 vec_float4 mVec128;
    3644                btScalar        m_floats[4];
    37         };
    3845public:
    39         vec_float4      get128() const
    40         {
    41                 return mVec128;
     46        SIMD_FORCE_INLINE const vec_float4&     get128() const
     47        {
     48                return *((const vec_float4*)&m_floats[0]);
    4249        }
    4350public:
    4451#else //__CELLOS_LV2__ __SPU__
    45 #ifdef BT_USE_SSE // WIN32
     52#ifdef BT_USE_SSE // _WIN32
    4653        union {
    4754                __m128 mVec128;
    … …  
    142149        SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
    143150
     151        SIMD_FORCE_INLINE btVector3& safeNormalize()
     152        {
     153                btVector3 absVec = this->absolute();
     154                int maxIndex = absVec.maxAxis();
     155                if (absVec[maxIndex]>0)
     156                {
     157                        *this /= absVec[maxIndex];
     158                        return *this /= length();
     159                }
     160                setValue(1,0,0);
     161                return *this;
     162        }
     163
    144164  /**@brief Normalize this vector
    145165   * x^2 + y^2 + z^2 = 1 */
    … …  
    152172        SIMD_FORCE_INLINE btVector3 normalized() const;
    153173
    154   /**@brief Rotate this vector
     174  /**@brief Return a rotated version of this vector
    155175   * @param wAxis The axis to rotate about
    156176   * @param angle The angle to rotate by */
    157         SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle );
     177        SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ) const;
    158178
    159179  /**@brief Return the angle between this and another vector
    … …  
    307327                        m_floats[1]=y;
    308328                        m_floats[2]=z;
    309                         m_floats[3] = 0.f;
     329                        m_floats[3] = btScalar(0.);
    310330                }
    311331
    … …  
    316336                        v2->setValue(-y()       ,x()    ,0.);
    317337                }
     338
     339                void    setZero()
     340                {
     341                        setValue(btScalar(0.),btScalar(0.),btScalar(0.));
     342                }
     343
     344                SIMD_FORCE_INLINE bool isZero() const
     345                {
     346                        return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
     347                }
     348
     349                SIMD_FORCE_INLINE bool fuzzyZero() const
     350                {
     351                        return length2() < SIMD_EPSILON;
     352                }
     353
     354                SIMD_FORCE_INLINE       void    serialize(struct        btVector3Data& dataOut) const;
     355
     356                SIMD_FORCE_INLINE       void    deSerialize(const struct        btVector3Data& dataIn);
     357
     358                SIMD_FORCE_INLINE       void    serializeFloat(struct   btVector3FloatData& dataOut) const;
     359
     360                SIMD_FORCE_INLINE       void    deSerializeFloat(const struct   btVector3FloatData& dataIn);
     361
     362                SIMD_FORCE_INLINE       void    serializeDouble(struct  btVector3DoubleData& dataOut) const;
     363
     364                SIMD_FORCE_INLINE       void    deSerializeDouble(const struct  btVector3DoubleData& dataIn);
    318365
    319366};
    … …  
    377424/**@brief Return the dot product between two vectors */
    378425SIMD_FORCE_INLINE btScalar
    379 dot(const btVector3& v1, const btVector3& v2)
     426btDot(const btVector3& v1, const btVector3& v2)
    380427{
    381428        return v1.dot(v2);
    … …  
    385432/**@brief Return the distance squared between two vectors */
    386433SIMD_FORCE_INLINE btScalar
    387 distance2(const btVector3& v1, const btVector3& v2)
     434btDistance2(const btVector3& v1, const btVector3& v2)
    388435{
    389436        return v1.distance2(v2);
    … …  
    393440/**@brief Return the distance between two vectors */
    394441SIMD_FORCE_INLINE btScalar
    395 distance(const btVector3& v1, const btVector3& v2)
     442btDistance(const btVector3& v1, const btVector3& v2)
    396443{
    397444        return v1.distance(v2);
    … …  
    400447/**@brief Return the angle between two vectors */
    401448SIMD_FORCE_INLINE btScalar
    402 angle(const btVector3& v1, const btVector3& v2)
     449btAngle(const btVector3& v1, const btVector3& v2)
    403450{
    404451        return v1.angle(v2);
    … …  
    407454/**@brief Return the cross product of two vectors */
    408455SIMD_FORCE_INLINE btVector3
    409 cross(const btVector3& v1, const btVector3& v2)
     456btCross(const btVector3& v1, const btVector3& v2)
    410457{
    411458        return v1.cross(v2);
    … …  
    413460
    414461SIMD_FORCE_INLINE btScalar
    415 triple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
     462btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
    416463{
    417464        return v1.triple(v2, v3);
    … …  
    445492}
    446493
    447 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle )
     494SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle ) const
    448495{
    449496        // wAxis must be a unit lenght vector
    … …  
    489536        {
    490537                int maxIndex = -1;
    491                 btScalar maxVal = btScalar(-1e30);
     538                btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
    492539                if (m_floats[0] > maxVal)
    493540                {
    … …  
    522569        {
    523570                int minIndex = -1;
    524                 btScalar minVal = btScalar(1e30);
     571                btScalar minVal = btScalar(BT_LARGE_FLOAT);
    525572                if (m_floats[0] < minVal)
    526573                {
    … …  
    585632                }
    586633
    587 
    588  
    589634
    590635};
    … …  
    636681}
    637682
     683template <class T>
     684SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
     685{
     686  if (btFabs(n[2]) > SIMDSQRT12) {
     687    // choose p in y-z plane
     688    btScalar a = n[1]*n[1] + n[2]*n[2];
     689    btScalar k = btRecipSqrt (a);
     690    p[0] = 0;
     691        p[1] = -n[2]*k;
     692        p[2] = n[1]*k;
     693    // set q = n x p
     694    q[0] = a*k;
     695        q[1] = -n[0]*p[2];
     696        q[2] = n[0]*p[1];
     697  }
     698  else {
     699    // choose p in x-y plane
     700    btScalar a = n[0]*n[0] + n[1]*n[1];
     701    btScalar k = btRecipSqrt (a);
     702    p[0] = -n[1]*k;
     703        p[1] = n[0]*k;
     704        p[2] = 0;
     705    // set q = n x p
     706    q[0] = -n[2]*p[1];
     707        q[1] = n[2]*p[0];
     708        q[2] = a*k;
     709  }
     710}
     711
     712
     713struct  btVector3FloatData
     714{
     715        float   m_floats[4];
     716};
     717
     718struct  btVector3DoubleData
     719{
     720        double  m_floats[4];
     721
     722};
     723
     724SIMD_FORCE_INLINE       void    btVector3::serializeFloat(struct        btVector3FloatData& dataOut) const
     725{
     726        ///could also do a memcpy, check if it is worth it
     727        for (int i=0;i<4;i++)
     728                dataOut.m_floats[i] = float(m_floats[i]);
     729}
     730
     731SIMD_FORCE_INLINE void  btVector3::deSerializeFloat(const struct        btVector3FloatData& dataIn)
     732{
     733        for (int i=0;i<4;i++)
     734                m_floats[i] = btScalar(dataIn.m_floats[i]);
     735}
     736
     737
     738SIMD_FORCE_INLINE       void    btVector3::serializeDouble(struct       btVector3DoubleData& dataOut) const
     739{
     740        ///could also do a memcpy, check if it is worth it
     741        for (int i=0;i<4;i++)
     742                dataOut.m_floats[i] = double(m_floats[i]);
     743}
     744
     745SIMD_FORCE_INLINE void  btVector3::deSerializeDouble(const struct       btVector3DoubleData& dataIn)
     746{
     747        for (int i=0;i<4;i++)
     748                m_floats[i] = btScalar(dataIn.m_floats[i]);
     749}
     750
     751
     752SIMD_FORCE_INLINE       void    btVector3::serialize(struct     btVector3Data& dataOut) const
     753{
     754        ///could also do a memcpy, check if it is worth it
     755        for (int i=0;i<4;i++)
     756                dataOut.m_floats[i] = m_floats[i];
     757}
     758
     759SIMD_FORCE_INLINE void  btVector3::deSerialize(const struct     btVector3Data& dataIn)
     760{
     761        for (int i=0;i<4;i++)
     762                m_floats[i] = dataIn.m_floats[i];
     763}
     764
     765
    638766#endif //SIMD__VECTOR3_H
Note: See TracChangeset for help on using the changeset viewer.