diff options
Diffstat (limited to 'extern/bullet2/src/LinearMath')
18 files changed, 1274 insertions, 379 deletions
diff --git a/extern/bullet2/src/LinearMath/CMakeLists.txt b/extern/bullet2/src/LinearMath/CMakeLists.txt index 02ffaad7228..99d5a6a2fef 100644 --- a/extern/bullet2/src/LinearMath/CMakeLists.txt +++ b/extern/bullet2/src/LinearMath/CMakeLists.txt @@ -3,7 +3,14 @@ INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src } ) -ADD_LIBRARY(LibLinearMath +SET(LinearMath_SRCS + btConvexHull.cpp + btQuickprof.cpp + btGeometryUtil.cpp + btAlignedAllocator.cpp +) + +SET(LinearMath_HDRS btAlignedObjectArray.h btList.h btPoolAllocator.h @@ -16,7 +23,6 @@ ADD_LIBRARY(LibLinearMath btScalar.h btAabbUtil2.h btConvexHull.h - btConvexHull.cpp btMinMax.h btQuaternion.h btStackAlloc.h @@ -25,11 +31,21 @@ ADD_LIBRARY(LibLinearMath btTransform.h btAlignedAllocator.h btIDebugDraw.h - btPoint3.h btQuickprof.h btTransformUtil.h - btQuickprof.cpp - btGeometryUtil.cpp - btAlignedAllocator.cpp ) +ADD_LIBRARY(LinearMath ${LinearMath_SRCS} ${LinearMath_HDRS}) +SET_TARGET_PROPERTIES(LinearMath PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(LinearMath PROPERTIES SOVERSION ${BULLET_VERSION}) + +#FILES_MATCHING requires CMake 2.6 +IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + INSTALL(TARGETS LinearMath DESTINATION lib) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION include FILES_MATCHING PATTERN "*.h") +ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + +IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(LinearMath PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(LinearMath PROPERTIES PUBLIC_HEADER "${LinearMath_HDRS}") +ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) diff --git a/extern/bullet2/src/LinearMath/btAabbUtil2.h b/extern/bullet2/src/LinearMath/btAabbUtil2.h index 275c4914628..532ce1bf633 100644 --- a/extern/bullet2/src/LinearMath/btAabbUtil2.h +++ b/extern/bullet2/src/LinearMath/btAabbUtil2.h @@ -21,6 +21,8 @@ subject to the following restrictions: #include "btVector3.h" #include "btMinMax.h" + + SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin, btVector3& aabbMax, const btVector3& expansionMin, @@ -30,15 +32,26 @@ SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin, aabbMax = aabbMax + expansionMax; } +/// conservative test for overlap between two aabbs +SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, + const btVector3 &point) +{ + bool overlap = true; + overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap; + overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap; + overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap; + return overlap; +} + /// conservative test for overlap between two aabbs SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, const btVector3 &aabbMin2, const btVector3 &aabbMax2) { bool overlap = true; - overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; - overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; - overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap; + overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap; + overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap; return overlap; } @@ -73,6 +86,7 @@ SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent) } + SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom, const btVector3& rayInvDirection, const unsigned int raySign[3], @@ -82,10 +96,10 @@ SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom, btScalar lambda_max) { btScalar tmax, tymin, tymax, tzmin, tzmax; - tmin = (bounds[raySign[0]][0] - rayFrom[0]) * rayInvDirection[0]; - tmax = (bounds[1-raySign[0]][0] - rayFrom[0]) * rayInvDirection[0]; - tymin = (bounds[raySign[1]][1] - rayFrom[1]) * rayInvDirection[1]; - tymax = (bounds[1-raySign[1]][1] - rayFrom[1]) * rayInvDirection[1]; + tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX(); + tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX(); + tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY(); + tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY(); if ( (tmin > tymax) || (tymin > tmax) ) return false; @@ -96,8 +110,8 @@ SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom, if (tymax < tmax) tmax = tymax; - tzmin = (bounds[raySign[2]][2] - rayFrom[2]) * rayInvDirection[2]; - tzmax = (bounds[1-raySign[2]][2] - rayFrom[2]) * rayInvDirection[2]; + tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ(); + tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ(); if ( (tmin > tzmax) || (tzmin > tmax) ) return false; @@ -196,6 +210,26 @@ SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin,const btVec aabbMaxOut = center+extent; } +#define USE_BANCHLESS 1 +#ifdef USE_BANCHLESS + //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360) + SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) + { + return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) + & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) + & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), + 1, 0)); + } +#else + SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) + { + bool overlap = true; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + return overlap; + } +#endif //USE_BANCHLESS #endif diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp index e120289e061..a3d790f8abc 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp @@ -19,6 +19,21 @@ int gNumAlignedAllocs = 0; int gNumAlignedFree = 0; int gTotalBytesAlignedAllocs = 0;//detect memory leaks +static void *btAllocDefault(size_t size) +{ + return malloc(size); +} + +static void btFreeDefault(void *ptr) +{ + free(ptr); +} + +static btAllocFunc *sAllocFunc = btAllocDefault; +static btFreeFunc *sFreeFunc = btFreeDefault; + + + #if defined (BT_HAS_ALIGNED_ALLOCATOR) #include <malloc.h> static void *btAlignedAllocDefault(size_t size, int alignment) @@ -49,7 +64,7 @@ static inline void *btAlignedAllocDefault(size_t size, int alignment) char *real; unsigned long offset; - real = (char *)malloc(size + sizeof(void *) + (alignment-1)); + real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1)); if (real) { offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1); ret = (void *)((real + sizeof(void *)) + offset); @@ -66,25 +81,14 @@ static inline void btAlignedFreeDefault(void *ptr) if (ptr) { real = *((void **)(ptr)-1); - free(real); + sFreeFunc(real); } } #endif -static void *btAllocDefault(size_t size) -{ - return malloc(size); -} - -static void btFreeDefault(void *ptr) -{ - free(ptr); -} static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault; static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault; -static btAllocFunc *sAllocFunc = btAllocDefault; -static btFreeFunc *sFreeFunc = btFreeDefault; void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc) { diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.h b/extern/bullet2/src/LinearMath/btAlignedAllocator.h index a252f324d77..f168f3c66c7 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.h +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.h @@ -38,7 +38,7 @@ void btAlignedFreeInternal (void* ptr,int line,char* filename); void* btAlignedAllocInternal (size_t size, int alignment); void btAlignedFreeInternal (void* ptr); - #define btAlignedAlloc(a,b) btAlignedAllocInternal(a,b) + #define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment) #define btAlignedFree(ptr) btAlignedFreeInternal(ptr) #endif @@ -49,8 +49,11 @@ typedef void (btAlignedFreeFunc)(void *memblock); typedef void *(btAllocFunc)(size_t size); typedef void (btFreeFunc)(void *memblock); -void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc); +///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc); +///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it. +void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc); + ///The btAlignedAllocator is a portable class for aligned memory allocations. ///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned. diff --git a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h index 5598f0d7236..bad1eee1f63 100644 --- a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h +++ b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h @@ -58,7 +58,7 @@ class btAlignedObjectArray { return (size ? size*2 : 1); } - SIMD_FORCE_INLINE void copy(int start,int end, T* dest) + SIMD_FORCE_INLINE void copy(int start,int end, T* dest) const { int i; for (i=start;i<end;++i) @@ -120,13 +120,21 @@ class btAlignedObjectArray clear(); } - SIMD_FORCE_INLINE int capacity() const - { // return current length of allocated storage - return m_capacity; + ///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead. + btAlignedObjectArray(const btAlignedObjectArray& otherArray) + { + init(); + + int otherSize = otherArray.size(); + resize (otherSize); + otherArray.copy(0, otherSize, m_data); } + + + /// return the number of elements in the array SIMD_FORCE_INLINE int size() const - { // return length of sequence + { return m_size; } @@ -141,6 +149,7 @@ class btAlignedObjectArray } + ///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations. SIMD_FORCE_INLINE void clear() { destroy(0,size()); @@ -156,6 +165,8 @@ class btAlignedObjectArray m_data[m_size].~T(); } + ///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument. + ///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations. SIMD_FORCE_INLINE void resize(int newsize, const T& fillData=T()) { int curSize = size(); @@ -219,6 +230,11 @@ class btAlignedObjectArray } + /// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve() + SIMD_FORCE_INLINE int capacity() const + { + return m_capacity; + } SIMD_FORCE_INLINE void reserve(int _Count) { // determine new minimum length of allocated storage diff --git a/extern/bullet2/src/LinearMath/btConvexHull.cpp b/extern/bullet2/src/LinearMath/btConvexHull.cpp index b8929a86808..419c752a1d9 100644 --- a/extern/bullet2/src/LinearMath/btConvexHull.cpp +++ b/extern/bullet2/src/LinearMath/btConvexHull.cpp @@ -262,8 +262,8 @@ int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &al int ma=-1; for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0)) { - btScalar s = sinf(SIMD_RADS_PER_DEG*(x)); - btScalar c = cosf(SIMD_RADS_PER_DEG*(x)); + btScalar s = btSin(SIMD_RADS_PER_DEG*(x)); + btScalar c = btCos(SIMD_RADS_PER_DEG*(x)); int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); if(ma==m && mb==m) { @@ -275,8 +275,8 @@ int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &al int mc = ma; for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0)) { - btScalar s = sinf(SIMD_RADS_PER_DEG*(xx)); - btScalar c = cosf(SIMD_RADS_PER_DEG*(xx)); + btScalar s = btSin(SIMD_RADS_PER_DEG*(xx)); + btScalar c = btCos(SIMD_RADS_PER_DEG*(xx)); int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); if(mc==m && md==m) { @@ -342,30 +342,30 @@ int shareedge(const int3 &a,const int3 &b) return 0; } -class Tri; +class btHullTriangle; -class Tri : public int3 +class btHullTriangle : public int3 { public: int3 n; int id; int vmax; btScalar rise; - Tri(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) + btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) { vmax=-1; rise = btScalar(0.0); } - ~Tri() + ~btHullTriangle() { } int &neib(int a,int b); }; -int &Tri::neib(int a,int b) +int &btHullTriangle::neib(int a,int b) { static int er=-1; int i; @@ -379,7 +379,7 @@ int &Tri::neib(int a,int b) btAssert(0); return er; } -void HullLibrary::b2bfix(Tri* s,Tri*t) +void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t) { int i; for(i=0;i<3;i++) @@ -395,7 +395,7 @@ void HullLibrary::b2bfix(Tri* s,Tri*t) } } -void HullLibrary::removeb2b(Tri* s,Tri*t) +void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t) { b2bfix(s,t); deAllocateTriangle(s); @@ -403,7 +403,7 @@ void HullLibrary::removeb2b(Tri* s,Tri*t) deAllocateTriangle(t); } -void HullLibrary::checkit(Tri *t) +void HullLibrary::checkit(btHullTriangle *t) { (void)t; @@ -427,36 +427,36 @@ void HullLibrary::checkit(Tri *t) } } -Tri* HullLibrary::allocateTriangle(int a,int b,int c) +btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c) { - void* mem = btAlignedAlloc(sizeof(Tri),16); - Tri* tr = new (mem)Tri(a,b,c); + void* mem = btAlignedAlloc(sizeof(btHullTriangle),16); + btHullTriangle* tr = new (mem)btHullTriangle(a,b,c); tr->id = m_tris.size(); m_tris.push_back(tr); return tr; } -void HullLibrary::deAllocateTriangle(Tri* tri) +void HullLibrary::deAllocateTriangle(btHullTriangle* tri) { btAssert(m_tris[tri->id]==tri); m_tris[tri->id]=NULL; - tri->~Tri(); + tri->~btHullTriangle(); btAlignedFree(tri); } -void HullLibrary::extrude(Tri *t0,int v) +void HullLibrary::extrude(btHullTriangle *t0,int v) { int3 t= *t0; int n = m_tris.size(); - Tri* ta = allocateTriangle(v,t[1],t[2]); + btHullTriangle* ta = allocateTriangle(v,t[1],t[2]); ta->n = int3(t0->n[0],n+1,n+2); m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0; - Tri* tb = allocateTriangle(v,t[2],t[0]); + btHullTriangle* tb = allocateTriangle(v,t[2],t[0]); tb->n = int3(t0->n[1],n+2,n+0); m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1; - Tri* tc = allocateTriangle(v,t[0],t[1]); + btHullTriangle* tc = allocateTriangle(v,t[0],t[1]); tc->n = int3(t0->n[2],n+0,n+1); m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2; checkit(ta); @@ -469,10 +469,10 @@ void HullLibrary::extrude(Tri *t0,int v) } -Tri* HullLibrary::extrudable(btScalar epsilon) +btHullTriangle* HullLibrary::extrudable(btScalar epsilon) { int i; - Tri *t=NULL; + btHullTriangle *t=NULL; for(i=0;i<m_tris.size();i++) { if(!t || (m_tris[i] && t->rise<m_tris[i]->rise)) @@ -550,23 +550,23 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0); // a valid interior point - Tri *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1); - Tri *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); - Tri *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); - Tri *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); + btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1); + btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); + btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); + btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; checkit(t0);checkit(t1);checkit(t2);checkit(t3); for(j=0;j<m_tris.size();j++) { - Tri *t=m_tris[j]; + btHullTriangle *t=m_tris[j]; btAssert(t); btAssert(t->vmax<0); btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); t->vmax = maxdirsterid(verts,verts_count,n,allow); t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); } - Tri *te; + btHullTriangle *te; vlimit-=4; while(vlimit >0 && ((te=extrudable(epsilon)) != 0)) { @@ -594,7 +594,7 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) int3 nt=*m_tris[j]; 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) ) { - Tri *nb = m_tris[m_tris[j]->n[0]]; + btHullTriangle *nb = m_tris[m_tris[j]->n[0]]; btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j); extrude(nb,v); j=m_tris.size(); @@ -603,7 +603,7 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) j=m_tris.size(); while(j--) { - Tri *t=m_tris[j]; + btHullTriangle *t=m_tris[j]; if(!t) continue; if(t->vmax>=0) break; btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); diff --git a/extern/bullet2/src/LinearMath/btConvexHull.h b/extern/bullet2/src/LinearMath/btConvexHull.h index 8c36307dfd6..92560bddb9c 100644 --- a/extern/bullet2/src/LinearMath/btConvexHull.h +++ b/extern/bullet2/src/LinearMath/btConvexHull.h @@ -137,13 +137,9 @@ class ConvexH }; ConvexH() { - int i; - i=0; } ~ConvexH() { - int i; - i=0; } btAlignedObjectArray<btVector3> vertices; btAlignedObjectArray<HalfEdge> edges; @@ -188,7 +184,7 @@ public: class HullLibrary { - btAlignedObjectArray<class Tri*> m_tris; + btAlignedObjectArray<class btHullTriangle*> m_tris; public: @@ -203,15 +199,15 @@ private: bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit); - class Tri* allocateTriangle(int a,int b,int c); - void deAllocateTriangle(Tri*); - void b2bfix(Tri* s,Tri*t); + class btHullTriangle* allocateTriangle(int a,int b,int c); + void deAllocateTriangle(btHullTriangle*); + void b2bfix(btHullTriangle* s,btHullTriangle*t); - void removeb2b(Tri* s,Tri*t); + void removeb2b(btHullTriangle* s,btHullTriangle*t); - void checkit(Tri *t); + void checkit(btHullTriangle *t); - Tri* extrudable(btScalar epsilon); + btHullTriangle* extrudable(btScalar epsilon); int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit); @@ -221,7 +217,7 @@ private: class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice); - void extrude(class Tri* t0,int v); + void extrude(class btHullTriangle* t0,int v); ConvexH* test_cube(); diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet2/src/LinearMath/btIDebugDraw.h index 563615a9a32..e5a0061b779 100644 --- a/extern/bullet2/src/LinearMath/btIDebugDraw.h +++ b/extern/bullet2/src/LinearMath/btIDebugDraw.h @@ -29,6 +29,7 @@ DEALINGS IN THE SOFTWARE. #define IDEBUG_DRAW__H #include "btVector3.h" +#include "btTransform.h" ///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations. @@ -52,11 +53,26 @@ class btIDebugDraw DBG_EnableSatComparison = 256, DBG_DisableBulletLCP = 512, DBG_EnableCCD = 1024, + DBG_DrawConstraints = (1 << 11), + DBG_DrawConstraintLimits = (1 << 12), DBG_MAX_DEBUG_DRAW_MODE }; virtual ~btIDebugDraw() {}; + virtual void drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor) + { + drawLine (from, to, fromColor); + } + + virtual void drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha) + { + } + + virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color) + { + } + virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0; 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) @@ -109,6 +125,171 @@ class btIDebugDraw edgecoord[i]*=-1.f; } } + void drawTransform(const btTransform& transform, btScalar orthoLen) + { + btVector3 start = transform.getOrigin(); + drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0)); + drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0)); + drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f)); + } + + void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, + const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f)) + { + const btVector3& vx = axis; + btVector3 vy = normal.cross(axis); + btScalar step = stepDegrees * SIMD_RADS_PER_DEG; + int nSteps = (int)((maxAngle - minAngle) / step); + if(!nSteps) nSteps = 1; + btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle); + if(drawSect) + { + drawLine(center, prev, color); + } + for(int i = 1; i <= nSteps; i++) + { + btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps); + btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle); + drawLine(prev, next, color); + prev = next; + } + if(drawSect) + { + drawLine(center, prev, color); + } + } + void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, + btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f)) + { + btVector3 vA[74]; + btVector3 vB[74]; + btVector3 *pvA = vA, *pvB = vB, *pT; + btVector3 npole = center + up * radius; + btVector3 spole = center - up * radius; + btVector3 arcStart; + btScalar step = stepDegrees * SIMD_RADS_PER_DEG; + const btVector3& kv = up; + const btVector3& iv = axis; + btVector3 jv = kv.cross(iv); + bool drawN = false; + bool drawS = false; + if(minTh <= -SIMD_HALF_PI) + { + minTh = -SIMD_HALF_PI + step; + drawN = true; + } + if(maxTh >= SIMD_HALF_PI) + { + maxTh = SIMD_HALF_PI - step; + drawS = true; + } + if(minTh > maxTh) + { + minTh = -SIMD_HALF_PI + step; + maxTh = SIMD_HALF_PI - step; + drawN = drawS = true; + } + int n_hor = (int)((maxTh - minTh) / step) + 1; + if(n_hor < 2) n_hor = 2; + btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1); + bool isClosed = false; + if(minPs > maxPs) + { + minPs = -SIMD_PI + step; + maxPs = SIMD_PI; + isClosed = true; + } + else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f)) + { + isClosed = true; + } + else + { + isClosed = false; + } + int n_vert = (int)((maxPs - minPs) / step) + 1; + if(n_vert < 2) n_vert = 2; + btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1); + for(int i = 0; i < n_hor; i++) + { + btScalar th = minTh + btScalar(i) * step_h; + btScalar sth = radius * btSin(th); + btScalar cth = radius * btCos(th); + for(int j = 0; j < n_vert; j++) + { + btScalar psi = minPs + btScalar(j) * step_v; + btScalar sps = btSin(psi); + btScalar cps = btCos(psi); + pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv; + if(i) + { + drawLine(pvA[j], pvB[j], color); + } + else if(drawS) + { + drawLine(spole, pvB[j], color); + } + if(j) + { + drawLine(pvB[j-1], pvB[j], color); + } + else + { + arcStart = pvB[j]; + } + if((i == (n_hor - 1)) && drawN) + { + drawLine(npole, pvB[j], color); + } + if(isClosed) + { + if(j == (n_vert-1)) + { + drawLine(arcStart, pvB[j], color); + } + } + else + { + if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1)))) + { + drawLine(center, pvB[j], color); + } + } + } + pT = pvA; pvA = pvB; pvB = pT; + } + } + + void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color) + { + drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color); + drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMin[2]), color); + drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMin[2]), color); + drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMin[2]), color); + drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color); + drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color); + drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color); + drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color); + drawLine(btVector3(bbMin[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color); + drawLine(btVector3(bbMax[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color); + drawLine(btVector3(bbMax[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color); + drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color); + } + void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color) + { + drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color); + drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color); + drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), color); + drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), color); + drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color); + } }; diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h index 14aa4ae2348..e45afc3c055 100644 --- a/extern/bullet2/src/LinearMath/btMatrix3x3.h +++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h @@ -23,14 +23,16 @@ subject to the following restrictions: -///The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3. -///Make sure to only include a pure orthogonal matrix without scaling. +/**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3. + * Make sure to only include a pure orthogonal matrix without scaling. */ class btMatrix3x3 { public: + /** @brief No initializaion constructor */ btMatrix3x3 () {} // explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } + /**@brief Constructor from Quaternion */ explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); } /* template <typename btScalar> @@ -39,6 +41,7 @@ class btMatrix3x3 { setEulerYPR(yaw, pitch, roll); } */ + /** @brief Constructor with row major formatting */ btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz, const btScalar& yx, const btScalar& yy, const btScalar& yz, const btScalar& zx, const btScalar& zy, const btScalar& zz) @@ -47,14 +50,14 @@ class btMatrix3x3 { yx, yy, yz, zx, zy, zz); } - + /** @brief Copy constructor */ SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other) { m_el[0] = other.m_el[0]; m_el[1] = other.m_el[1]; m_el[2] = other.m_el[2]; } - + /** @brief Assignment Operator */ SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) { m_el[0] = other.m_el[0]; @@ -63,34 +66,45 @@ class btMatrix3x3 { return *this; } + /** @brief Get a column of the matrix as a vector + * @param i Column number 0 indexed */ SIMD_FORCE_INLINE btVector3 getColumn(int i) const { return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); } - + /** @brief Get a row of the matrix as a vector + * @param i Row number 0 indexed */ SIMD_FORCE_INLINE const btVector3& getRow(int i) const { + btFullAssert(0 <= i && i < 3); return m_el[i]; } - + /** @brief Get a mutable reference to a row of the matrix as a vector + * @param i Row number 0 indexed */ SIMD_FORCE_INLINE btVector3& operator[](int i) { btFullAssert(0 <= i && i < 3); return m_el[i]; } + /** @brief Get a const reference to a row of the matrix as a vector + * @param i Row number 0 indexed */ SIMD_FORCE_INLINE const btVector3& operator[](int i) const { btFullAssert(0 <= i && i < 3); return m_el[i]; } + /** @brief Multiply by the target matrix on the right + * @param m Rotation matrix to be applied + * Equivilant to this = this * m */ btMatrix3x3& operator*=(const btMatrix3x3& m); - + /** @brief Set from a carray of btScalars + * @param m A pointer to the beginning of an array of 9 btScalars */ void setFromOpenGLSubMatrix(const btScalar *m) { m_el[0].setValue(m[0],m[4],m[8]); @@ -98,7 +112,16 @@ class btMatrix3x3 { m_el[2].setValue(m[2],m[6],m[10]); } - + /** @brief Set the values of the matrix explicitly (row major) + * @param xx Top left + * @param xy Top Middle + * @param xz Top Right + * @param yx Middle Left + * @param yy Middle Middle + * @param yz Middle Right + * @param zx Bottom Left + * @param zy Bottom Middle + * @param zz Bottom Right*/ void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, const btScalar& yx, const btScalar& yy, const btScalar& yz, const btScalar& zx, const btScalar& zy, const btScalar& zz) @@ -107,7 +130,9 @@ class btMatrix3x3 { m_el[1].setValue(yx,yy,yz); m_el[2].setValue(zx,zy,zz); } - + + /** @brief Set the matrix from a quaternion + * @param q The Quaternion to match */ void setRotation(const btQuaternion& q) { btScalar d = q.length2(); @@ -123,35 +148,27 @@ class btMatrix3x3 { } - + /** @brief Set the matrix from euler angles using YPR around YXZ respectively + * @param yaw Yaw about Y axis + * @param pitch Pitch about X axis + * @param roll Roll about Z axis + */ void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { - - btScalar cy(btCos(yaw)); - btScalar sy(btSin(yaw)); - btScalar cp(btCos(pitch)); - btScalar sp(btSin(pitch)); - btScalar cr(btCos(roll)); - btScalar sr(btSin(roll)); - btScalar cc = cy * cr; - btScalar cs = cy * sr; - btScalar sc = sy * cr; - btScalar ss = sy * sr; - setValue(cc - sp * ss, -cs - sp * sc, -sy * cp, - cp * sr, cp * cr, -sp, - sc + sp * cs, -ss + sp * cc, cy * cp); - + setEulerZYX(roll, pitch, yaw); } - /** - * setEulerZYX - * @param euler a const reference to a btVector3 of euler angles + /** @brief Set the matrix from euler angles YPR around ZYX axes + * @param eulerX Roll about X axis + * @param eulerY Pitch around Y axis + * @param eulerZ Yaw aboud Z axis + * * These angles are used to produce a rotation matrix. The euler * angles are applied in ZYX order. I.e a vector is first rotated * about X then Y and then Z **/ - - void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { + void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { + ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code btScalar ci ( btCos(eulerX)); btScalar cj ( btCos(eulerY)); btScalar ch ( btCos(eulerZ)); @@ -168,13 +185,24 @@ class btMatrix3x3 { -sj, cj * si, cj * ci); } + /**@brief Set the matrix to the identity */ void setIdentity() { setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), btScalar(0.0), btScalar(1.0), btScalar(0.0), btScalar(0.0), btScalar(0.0), btScalar(1.0)); } - + + static const btMatrix3x3& getIdentity() + { + static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0), + btScalar(0.0), btScalar(1.0), btScalar(0.0), + btScalar(0.0), btScalar(0.0), btScalar(1.0)); + return identityMatrix; + } + + /**@brief Fill the values of the matrix into a 9 element array + * @param m The array to be filled */ void getOpenGLSubMatrix(btScalar *m) const { m[0] = btScalar(m_el[0].x()); @@ -191,6 +219,8 @@ class btMatrix3x3 { m[11] = btScalar(0.0); } + /**@brief Get the matrix represented as a quaternion + * @param q The quaternion which will be set */ void getRotation(btQuaternion& q) const { btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); @@ -224,35 +254,102 @@ class btMatrix3x3 { } q.setValue(temp[0],temp[1],temp[2],temp[3]); } - - void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const + + /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR + * @param yaw Yaw around Y axis + * @param pitch Pitch around X axis + * @param roll around Z axis */ + void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const { - if (btScalar(m_el[1].z()) < btScalar(1)) - { - if (btScalar(m_el[1].z()) > -btScalar(1)) - { - yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); - pitch = btScalar(btAsin(-m_el[1].y())); - roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); - } - else - { - yaw = btScalar(-btAtan2(-m_el[0].y(), m_el[0].z())); - pitch = SIMD_HALF_PI; - roll = btScalar(0.0); - } - } - else + // first use the normal calculus + yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); + pitch = btScalar(btAsin(-m_el[2].x())); + roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); + + // on pitch = +/-HalfPI + if (btFabs(pitch)==SIMD_HALF_PI) { - yaw = btScalar(btAtan2(-m_el[0].y(), m_el[0].z())); - pitch = -SIMD_HALF_PI; - roll = btScalar(0.0); + if (yaw>0) + yaw-=SIMD_PI; + else + yaw+=SIMD_PI; + + if (roll>0) + roll-=SIMD_PI; + else + roll+=SIMD_PI; } - } - - + }; + + + /**@brief Get the matrix represented as euler angles around ZYX + * @param yaw Yaw around X axis + * @param pitch Pitch around Y axis + * @param roll around X axis + * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ + void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const + { + struct Euler{btScalar yaw, pitch, roll;}; + Euler euler_out; + Euler euler_out2; //second solution + //get the pointer to the raw data + + // Check that pitch is not at a singularity + if (btFabs(m_el[2].x()) >= 1) + { + euler_out.yaw = 0; + euler_out2.yaw = 0; + // From difference of angles formula + btScalar delta = btAtan2(m_el[0].x(),m_el[0].z()); + if (m_el[2].x() > 0) //gimbal locked up + { + euler_out.pitch = SIMD_PI / btScalar(2.0); + euler_out2.pitch = SIMD_PI / btScalar(2.0); + euler_out.roll = euler_out.pitch + delta; + euler_out2.roll = euler_out.pitch + delta; + } + else // gimbal locked down + { + euler_out.pitch = -SIMD_PI / btScalar(2.0); + euler_out2.pitch = -SIMD_PI / btScalar(2.0); + euler_out.roll = -euler_out.pitch + delta; + euler_out2.roll = -euler_out.pitch + delta; + } + } + else + { + euler_out.pitch = - btAsin(m_el[2].x()); + euler_out2.pitch = SIMD_PI - euler_out.pitch; + + euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), + m_el[2].z()/btCos(euler_out.pitch)); + euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), + m_el[2].z()/btCos(euler_out2.pitch)); + + euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), + m_el[0].x()/btCos(euler_out.pitch)); + euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), + m_el[0].x()/btCos(euler_out2.pitch)); + } + + if (solution_number == 1) + { + yaw = euler_out.yaw; + pitch = euler_out.pitch; + roll = euler_out.roll; + } + else + { + yaw = euler_out2.yaw; + pitch = euler_out2.pitch; + roll = euler_out2.roll; + } + } + + /**@brief Create a scaled copy of the matrix + * @param s Scaling vector The elements of the vector will scale each column */ btMatrix3x3 scaled(const btVector3& s) const { @@ -261,10 +358,15 @@ class btMatrix3x3 { m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); } + /**@brief Return the determinant of the matrix */ btScalar determinant() const; + /**@brief Return the adjoint of the matrix */ btMatrix3x3 adjoint() const; + /**@brief Return the matrix with all values non negative */ btMatrix3x3 absolute() const; + /**@brief Return the transpose of the matrix */ btMatrix3x3 transpose() const; + /**@brief Return the inverse of the matrix */ btMatrix3x3 inverse() const; btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; @@ -284,12 +386,15 @@ class btMatrix3x3 { } - ///diagonalizes this matrix by the Jacobi method. rot stores the rotation - ///from the coordinate system in which the matrix is diagonal to the original - ///coordinate system, i.e., old_this = rot * new_this * rot^T. The iteration - ///stops when all off-diagonal elements are less than the threshold multiplied - ///by the sum of the absolute values of the diagonal, or when maxSteps have - ///been executed. Note that this matrix is assumed to be symmetric. + /**@brief diagonalizes this matrix by the Jacobi method. + * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original + * coordinate system, i.e., old_this = rot * new_this * rot^T. + * @param threshold See iteration + * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied + * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. + * + * Note that this matrix is assumed to be symmetric. + */ void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) { rot.setIdentity(); @@ -371,11 +476,18 @@ class btMatrix3x3 { protected: + /**@brief Calculate the matrix cofactor + * @param r1 The first row to use for calculating the cofactor + * @param c1 The first column to use for calculating the cofactor + * @param r1 The second row to use for calculating the cofactor + * @param c1 The second column to use for calculating the cofactor + * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details + */ btScalar cofac(int r1, int c1, int r2, int c2) const { return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; } - + ///Data storage for the matrix, each vector is a row of the matrix btVector3 m_el[3]; }; @@ -494,6 +606,8 @@ class btMatrix3x3 { } */ +/**@brief Equality operator between two matrices + * It will test all elements are equal. */ SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) { return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && diff --git a/extern/bullet2/src/LinearMath/btQuadWord.h b/extern/bullet2/src/LinearMath/btQuadWord.h index 2e80fc2ca47..c657afd2bb1 100644 --- a/extern/bullet2/src/LinearMath/btQuadWord.h +++ b/extern/bullet2/src/LinearMath/btQuadWord.h @@ -18,121 +18,159 @@ subject to the following restrictions: #include "btScalar.h" #include "btMinMax.h" -#include <math.h> +#if defined (__CELLOS_LV2) && defined (__SPU__) +#include <altivec.h> +#endif -///The btQuadWordStorage class is base class for btVector3 and btQuaternion. -///Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. todo: look into this -///ATTRIBUTE_ALIGNED16(class) btQuadWordStorage -class btQuadWordStorage +/**@brief The btQuadWord class is base class for btVector3 and btQuaternion. + * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. + */ +#ifndef USE_LIBSPE2 +ATTRIBUTE_ALIGNED16(class) btQuadWord +#else +class btQuadWord +#endif { protected: - btScalar m_x; - btScalar m_y; - btScalar m_z; - btScalar m_unusedW; - +#if defined (__SPU__) && defined (__CELLOS_LV2__) + union { + vec_float4 mVec128; + btScalar m_floats[4]; + }; public: + vec_float4 get128() const + { + return mVec128; + } +protected: +#else //__CELLOS_LV2__ __SPU__ + btScalar m_floats[4]; +#endif //__CELLOS_LV2__ __SPU__ -}; - - -///btQuadWord is base-class for vectors, points -class btQuadWord : public btQuadWordStorage -{ public: - -// SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; } -// SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; } - - SIMD_FORCE_INLINE const btScalar& getX() const { return m_x; } - - SIMD_FORCE_INLINE const btScalar& getY() const { return m_y; } - - SIMD_FORCE_INLINE const btScalar& getZ() const { return m_z; } - - SIMD_FORCE_INLINE void setX(btScalar x) { m_x = x;}; - - SIMD_FORCE_INLINE void setY(btScalar y) { m_y = y;}; - - SIMD_FORCE_INLINE void setZ(btScalar z) { m_z = z;}; - - SIMD_FORCE_INLINE void setW(btScalar w) { m_unusedW = w;}; - - SIMD_FORCE_INLINE const btScalar& x() const { return m_x; } - - SIMD_FORCE_INLINE const btScalar& y() const { return m_y; } - - SIMD_FORCE_INLINE const btScalar& z() const { return m_z; } - - SIMD_FORCE_INLINE const btScalar& w() const { return m_unusedW; } - - - SIMD_FORCE_INLINE operator btScalar *() { return &m_x; } - SIMD_FORCE_INLINE operator const btScalar *() const { return &m_x; } - - - + + + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; } + /**@brief Set the x value */ + SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;}; + /**@brief Set the y value */ + SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;}; + /**@brief Set the z value */ + SIMD_FORCE_INLINE void setZ(btScalar z) { m_floats[2] = z;}; + /**@brief Set the w value */ + SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;}; + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; } + /**@brief Return the w value */ + SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; } + + //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } + //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; } + ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. + SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; } + SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; } + + SIMD_FORCE_INLINE bool operator==(const btQuadWord& other) const + { + return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); + } + + SIMD_FORCE_INLINE bool operator!=(const btQuadWord& other) const + { + return !(*this == other); + } + + /**@brief Set x,y,z and zero w + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z) { - m_x=x; - m_y=y; - m_z=z; - m_unusedW = 0.f; + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3] = 0.f; } /* void getValue(btScalar *m) const { - m[0] = m_x; - m[1] = m_y; - m[2] = m_z; + m[0] = m_floats[0]; + m[1] = m_floats[1]; + m[2] = m_floats[2]; } */ +/**@brief Set the values + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) { - m_x=x; - m_y=y; - m_z=z; - m_unusedW=w; + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3]=w; } - + /**@brief No initialization constructor */ SIMD_FORCE_INLINE btQuadWord() - // :m_x(btScalar(0.)),m_y(btScalar(0.)),m_z(btScalar(0.)),m_unusedW(btScalar(0.)) - { - } - - SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q) + // :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.)) { - *((btQuadWordStorage*)this) = q; } - + + /**@brief Three argument constructor (zeros w) + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z) { - m_x = x, m_y = y, m_z = z, m_unusedW = 0.0f; + m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f; } +/**@brief Initializing constructor + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) { - m_x = x, m_y = y, m_z = z, m_unusedW = w; + m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w; } - + /**@brief Set each element to the max of the current values and the values of another btQuadWord + * @param other The other btQuadWord to compare with + */ SIMD_FORCE_INLINE void setMax(const btQuadWord& other) { - btSetMax(m_x, other.m_x); - btSetMax(m_y, other.m_y); - btSetMax(m_z, other.m_z); - btSetMax(m_unusedW, other.m_unusedW); + btSetMax(m_floats[0], other.m_floats[0]); + btSetMax(m_floats[1], other.m_floats[1]); + btSetMax(m_floats[2], other.m_floats[2]); + btSetMax(m_floats[3], other.m_floats[3]); } - + /**@brief Set each element to the min of the current values and the values of another btQuadWord + * @param other The other btQuadWord to compare with + */ SIMD_FORCE_INLINE void setMin(const btQuadWord& other) { - btSetMin(m_x, other.m_x); - btSetMin(m_y, other.m_y); - btSetMin(m_z, other.m_z); - btSetMin(m_unusedW, other.m_unusedW); + btSetMin(m_floats[0], other.m_floats[0]); + btSetMin(m_floats[1], other.m_floats[1]); + btSetMin(m_floats[2], other.m_floats[2]); + btSetMin(m_floats[3], other.m_floats[3]); } diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h index 264751b33e7..cbeca2681cc 100644 --- a/extern/bullet2/src/LinearMath/btQuaternion.h +++ b/extern/bullet2/src/LinearMath/btQuaternion.h @@ -17,39 +17,56 @@ subject to the following restrictions: #ifndef SIMD__QUATERNION_H_ #define SIMD__QUATERNION_H_ + #include "btVector3.h" +#include "btQuadWord.h" -///The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. +/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */ class btQuaternion : public btQuadWord { public: + /**@brief No initialization constructor */ btQuaternion() {} // template <typename btScalar> // explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {} - + /**@brief Constructor from scalars */ btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w) : btQuadWord(x, y, z, w) {} - + /**@brief Axis angle Constructor + * @param axis The axis which the rotation is around + * @param angle The magnitude of the rotation around the angle (Radians) */ btQuaternion(const btVector3& axis, const btScalar& angle) { setRotation(axis, angle); } - + /**@brief Constructor from Euler angles + * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z + * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y + * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */ btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { +#ifndef BT_EULER_DEFAULT_ZYX setEuler(yaw, pitch, roll); +#else + setEulerZYX(yaw, pitch, roll); +#endif } - + /**@brief Set the rotation using axis angle notation + * @param axis The axis around which to rotate + * @param angle The magnitude of the rotation in Radians */ void setRotation(const btVector3& axis, const btScalar& angle) { btScalar d = axis.length(); - assert(d != btScalar(0.0)); + btAssert(d != btScalar(0.0)); btScalar s = btSin(angle * btScalar(0.5)) / d; setValue(axis.x() * s, axis.y() * s, axis.z() * s, btCos(angle * btScalar(0.5))); } - + /**@brief Set the quaternion using Euler angles + * @param yaw Angle around Y + * @param pitch Angle around X + * @param roll Angle around Z */ void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { btScalar halfYaw = btScalar(yaw) * btScalar(0.5); @@ -66,122 +83,165 @@ public: sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); } - - btQuaternion& operator+=(const btQuaternion& q) + /**@brief Set the quaternion using euler angles + * @param yaw Angle around Z + * @param pitch Angle around Y + * @param roll Angle around X */ + void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { - m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW; + btScalar halfYaw = btScalar(yaw) * btScalar(0.5); + btScalar halfPitch = btScalar(pitch) * btScalar(0.5); + btScalar halfRoll = btScalar(roll) * btScalar(0.5); + btScalar cosYaw = btCos(halfYaw); + btScalar sinYaw = btSin(halfYaw); + btScalar cosPitch = btCos(halfPitch); + btScalar sinPitch = btSin(halfPitch); + btScalar cosRoll = btCos(halfRoll); + btScalar sinRoll = btSin(halfRoll); + setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x + cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y + cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z + cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx + } + /**@brief Add two quaternions + * @param q The quaternion to add to this one */ + SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q) + { + m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3]; return *this; } + /**@brief Subtract out a quaternion + * @param q The quaternion to subtract from this one */ btQuaternion& operator-=(const btQuaternion& q) { - m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW; + m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3]; return *this; } + /**@brief Scale this quaternion + * @param s The scalar to scale by */ btQuaternion& operator*=(const btScalar& s) { - m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s; + m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s; return *this; } - + /**@brief Multiply this quaternion by q on the right + * @param q The other quaternion + * Equivilant to this = this * q */ btQuaternion& operator*=(const btQuaternion& q) { - setValue(m_unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z * q.y(), - m_unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x * q.z(), - m_unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y * q.x(), - m_unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z * q.z()); + setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(), + m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(), + m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(), + m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z()); return *this; } - + /**@brief Return the dot product between this quaternion and another + * @param q The other quaternion */ btScalar dot(const btQuaternion& q) const { - return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW; + return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3]; } + /**@brief Return the length squared of the quaternion */ btScalar length2() const { return dot(*this); } + /**@brief Return the length of the quaternion */ btScalar length() const { return btSqrt(length2()); } + /**@brief Normalize the quaternion + * Such that x^2 + y^2 + z^2 +w^2 = 1 */ btQuaternion& normalize() { return *this /= length(); } + /**@brief Return a scaled version of this quaternion + * @param s The scale factor */ SIMD_FORCE_INLINE btQuaternion operator*(const btScalar& s) const { - return btQuaternion(x() * s, y() * s, z() * s, m_unusedW * s); + return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s); } - + /**@brief Return an inversely scaled versionof this quaternion + * @param s The inverse scale factor */ btQuaternion operator/(const btScalar& s) const { - assert(s != btScalar(0.0)); + btAssert(s != btScalar(0.0)); return *this * (btScalar(1.0) / s); } - + /**@brief Inversely scale this quaternion + * @param s The scale factor */ btQuaternion& operator/=(const btScalar& s) { - assert(s != btScalar(0.0)); + btAssert(s != btScalar(0.0)); return *this *= btScalar(1.0) / s; } - + /**@brief Return a normalized version of this quaternion */ btQuaternion normalized() const { return *this / length(); } - + /**@brief Return the angle between this quaternion and the other + * @param q The other quaternion */ btScalar angle(const btQuaternion& q) const { btScalar s = btSqrt(length2() * q.length2()); - assert(s != btScalar(0.0)); + btAssert(s != btScalar(0.0)); return btAcos(dot(q) / s); } - + /**@brief Return the angle of rotation represented by this quaternion */ btScalar getAngle() const { - btScalar s = btScalar(2.) * btAcos(m_unusedW); + btScalar s = btScalar(2.) * btAcos(m_floats[3]); return s; } - + /**@brief Return the inverse of this quaternion */ btQuaternion inverse() const { - return btQuaternion(-m_x, -m_y, -m_z, m_unusedW); + return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); } + /**@brief Return the sum of this quaternion and the other + * @param q2 The other quaternion */ SIMD_FORCE_INLINE btQuaternion operator+(const btQuaternion& q2) const { const btQuaternion& q1 = *this; - return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW); + return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]); } + /**@brief Return the difference between this quaternion and the other + * @param q2 The other quaternion */ SIMD_FORCE_INLINE btQuaternion operator-(const btQuaternion& q2) const { const btQuaternion& q1 = *this; - return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW); + return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]); } + /**@brief Return the negative of this quaternion + * This simply negates each element */ SIMD_FORCE_INLINE btQuaternion operator-() const { const btQuaternion& q2 = *this; - return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_unusedW); + return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]); } - + /**@todo document this and it's use */ SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const { btQuaternion diff,sum; @@ -192,6 +252,10 @@ public: return (-qd); } + /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion + * @param q The other quaternion to interpolate with + * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. + * Slerp interpolates assuming constant velocity. */ btQuaternion slerp(const btQuaternion& q, const btScalar& t) const { btScalar theta = angle(q); @@ -200,10 +264,10 @@ public: btScalar d = btScalar(1.0) / btSin(theta); btScalar s0 = btSin((btScalar(1.0) - t) * theta); btScalar s1 = btSin(t * theta); - return btQuaternion((m_x * s0 + q.x() * s1) * d, - (m_y * s0 + q.y() * s1) * d, - (m_z * s0 + q.z() * s1) * d, - (m_unusedW * s0 + q.m_unusedW * s1) * d); + return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d, + (m_floats[1] * s0 + q.y() * s1) * d, + (m_floats[2] * s0 + q.z() * s1) * d, + (m_floats[3] * s0 + q.m_floats[3] * s1) * d); } else { @@ -211,13 +275,19 @@ public: } } - SIMD_FORCE_INLINE const btScalar& getW() const { return m_unusedW; } + static const btQuaternion& getIdentity() + { + static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.)); + return identityQuat; + } + + SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; } }; - +/**@brief Return the negative of a quaternion */ SIMD_FORCE_INLINE btQuaternion operator-(const btQuaternion& q) { @@ -226,7 +296,7 @@ operator-(const btQuaternion& q) - +/**@brief Return the product of two quaternions */ SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q1, const btQuaternion& q2) { return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), @@ -253,6 +323,7 @@ operator*(const btVector3& w, const btQuaternion& q) -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); } +/**@brief Calculate the dot product between two quaternions */ SIMD_FORCE_INLINE btScalar dot(const btQuaternion& q1, const btQuaternion& q2) { @@ -260,25 +331,32 @@ dot(const btQuaternion& q1, const btQuaternion& q2) } +/**@brief Return the length of a quaternion */ SIMD_FORCE_INLINE btScalar length(const btQuaternion& q) { return q.length(); } +/**@brief Return the angle between two quaternions*/ SIMD_FORCE_INLINE btScalar angle(const btQuaternion& q1, const btQuaternion& q2) { return q1.angle(q2); } - +/**@brief Return the inverse of a quaternion*/ SIMD_FORCE_INLINE btQuaternion inverse(const btQuaternion& q) { return q.inverse(); } +/**@brief Return the result of spherical linear interpolation betwen two quaternions + * @param q1 The first quaternion + * @param q2 The second quaternion + * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 + * Slerp assumes constant velocity between positions. */ SIMD_FORCE_INLINE btQuaternion slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) { diff --git a/extern/bullet2/src/LinearMath/btQuickprof.cpp b/extern/bullet2/src/LinearMath/btQuickprof.cpp index e5b1196149b..fa45d02b3d3 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.cpp +++ b/extern/bullet2/src/LinearMath/btQuickprof.cpp @@ -110,7 +110,7 @@ void CProfileNode::Reset( void ) { TotalCalls = 0; TotalTime = 0.0f; - gProfileClock.reset(); + if ( Child ) { Child->Reset(); @@ -251,6 +251,7 @@ void CProfileManager::Stop_Profile( void ) *=============================================================================================*/ void CProfileManager::Reset( void ) { + gProfileClock.reset(); Root.Reset(); Root.Call(); FrameCounter = 0; @@ -278,5 +279,68 @@ float CProfileManager::Get_Time_Since_Reset( void ) return (float)time / Profile_Get_Tick_Rate(); } +#include <stdio.h> + +void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spacing) +{ + profileIterator->First(); + if (profileIterator->Is_Done()) + return; + + float accumulated_time=0,parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time(); + int i; + int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset(); + for (i=0;i<spacing;i++) printf("."); + printf("----------------------------------\n"); + for (i=0;i<spacing;i++) printf("."); + printf("Profiling: %s (total running time: %.3f ms) ---\n", profileIterator->Get_Current_Parent_Name(), parent_time ); + float totalTime = 0.f; + + + int numChildren = 0; + + for (i = 0; !profileIterator->Is_Done(); i++,profileIterator->Next()) + { + numChildren++; + float current_total_time = profileIterator->Get_Current_Total_Time(); + accumulated_time += current_total_time; + float fraction = parent_time > SIMD_EPSILON ? (current_total_time / parent_time) * 100 : 0.f; + { + int i; for (i=0;i<spacing;i++) printf("."); + } + printf("%d -- %s (%.2f %%) :: %.3f ms / frame (%d calls)\n",i, profileIterator->Get_Current_Name(), fraction,(current_total_time / (double)frames_since_reset),profileIterator->Get_Current_Total_Calls()); + totalTime += current_total_time; + //recurse into children + } + + if (parent_time < accumulated_time) + { + printf("what's wrong\n"); + } + for (i=0;i<spacing;i++) printf("."); + printf("%s (%.3f %%) :: %.3f ms\n", "Unaccounted:",parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time); + + for (i=0;i<numChildren;i++) + { + profileIterator->Enter_Child(i); + dumpRecursive(profileIterator,spacing+3); + profileIterator->Enter_Parent(); + } +} + + + +void CProfileManager::dumpAll() +{ + CProfileIterator* profileIterator = 0; + profileIterator = CProfileManager::Get_Iterator(); + + dumpRecursive(profileIterator,0); + + CProfileManager::Release_Iterator(profileIterator); +} + + + #endif //USE_BT_CLOCK diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet2/src/LinearMath/btQuickprof.h index b033940ca5c..f8d47c36861 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.h +++ b/extern/bullet2/src/LinearMath/btQuickprof.h @@ -10,14 +10,20 @@ // Credits: The Clock class was inspired by the Timer classes in // Ogre (www.ogre3d.org). + + #ifndef QUICK_PROF_H #define QUICK_PROF_H +//To disable built-in profiling, please comment out next line +//#define BT_NO_PROFILE 1 +#ifndef BT_NO_PROFILE + #include "btScalar.h" #include "LinearMath/btAlignedAllocator.h" #include <new> -//To disable built-in profiling, please comment out next line -//#define BT_NO_PROFILE 1 + + //if you don't need btClock, you can comment next line @@ -321,6 +327,10 @@ public: } static void Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); } + static void dumpRecursive(CProfileIterator* profileIterator, int spacing); + + static void dumpAll(); + private: static CProfileNode Root; static CProfileNode * CurrentNode; @@ -344,12 +354,14 @@ public: } }; -#if !defined(BT_NO_PROFILE) + #define BT_PROFILE( name ) CProfileSample __profile( name ) + #else + #define BT_PROFILE( name ) -#endif +#endif //#ifndef BT_NO_PROFILE diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index e8433405b20..822296164c1 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -18,13 +18,14 @@ subject to the following restrictions: #define SIMD___SCALAR_H #include <math.h> + #include <stdlib.h>//size_t for MSVC 6.0 #include <cstdlib> #include <cfloat> #include <float.h> -#define BT_BULLET_VERSION 272 +#define BT_BULLET_VERSION 274 inline int btGetVersion() { @@ -44,7 +45,7 @@ inline int btGetVersion() #define ATTRIBUTE_ALIGNED16(a) a #define ATTRIBUTE_ALIGNED128(a) a #else - #define BT_HAS_ALIGNED_ALLOCATOR + //#define BT_HAS_ALIGNED_ALLOCATOR #pragma warning(disable : 4324) // disable padding warning // #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. // #pragma warning(disable:4996) //Turn off warnings about deprecated C routines @@ -60,12 +61,18 @@ inline int btGetVersion() #define BT_HAVE_NATIVE_FSEL #define btFsel(a,b,c) __fsel((a),(b),(c)) #else + +#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION)) #define BT_USE_SSE - #endif + #include <emmintrin.h> +#endif + + #endif//_XBOX + #endif //__MINGW32__ #include <assert.h> -#if defined(DEBUG) || defined (_DEBUG) +#ifdef BT_DEBUG #define btAssert assert #else #define btAssert(x) @@ -85,7 +92,11 @@ inline int btGetVersion() #ifndef assert #include <assert.h> #endif +#ifdef BT_DEBUG #define btAssert assert +#else + #define btAssert(x) +#endif //btFullAssert is optional, slows down a lot #define btFullAssert(x) @@ -102,7 +113,11 @@ inline int btGetVersion() #ifndef assert #include <assert.h> #endif +#ifdef BT_DEBUG #define btAssert assert +#else + #define btAssert(x) +#endif //btFullAssert is optional, slows down a lot #define btFullAssert(x) @@ -115,6 +130,9 @@ inline int btGetVersion() //non-windows systems #define SIMD_FORCE_INLINE inline + ///@todo: check out alignment methods for other platforms/compilers + ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #define ATTRIBUTE_ALIGNED16(a) a #define ATTRIBUTE_ALIGNED128(a) a #ifndef assert @@ -141,10 +159,6 @@ inline int btGetVersion() /// older compilers (gcc 3.x) and Sun needs double version of sqrt etc. /// exclude Apple Intel (i's assumed to be a Macbook or new Intel Dual Core Processor) #if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__)) -/* XXX Need to fix these... needed for SunOS 5.8 */ -#define sinf(a) sin((double)(a)) -#define cosf(a) cos((double)(a)) -#define fabsf(a) fabs((double)(a)) //use slow double float precision operation on those platforms #ifndef BT_USE_DOUBLE_PRECISION #define BT_FORCE_DOUBLE_FUNCTIONS @@ -198,7 +212,7 @@ SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) tempf = y; *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ x = tempf; - z = y*btScalar(0.5); /* hoist out the ô/2ö */ + z = y*btScalar(0.5); /* hoist out the “/2” */ x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ x = (btScalar(1.5)*x)-(x*x)*(x*z); x = (btScalar(1.5)*x)-(x*x)*(x*z); diff --git a/extern/bullet2/src/LinearMath/btStackAlloc.h b/extern/bullet2/src/LinearMath/btStackAlloc.h index 050d44bdfe9..397b084877f 100644 --- a/extern/bullet2/src/LinearMath/btStackAlloc.h +++ b/extern/bullet2/src/LinearMath/btStackAlloc.h @@ -23,6 +23,7 @@ Nov.2006 #include "btScalar.h" //for btAssert #include "btAlignedAllocator.h" +///The btBlock class is an internal structure for the btStackAlloc memory allocator. struct btBlock { btBlock* previous; diff --git a/extern/bullet2/src/LinearMath/btTransform.h b/extern/bullet2/src/LinearMath/btTransform.h index a8cdb428100..c4fe33eecd7 100644 --- a/extern/bullet2/src/LinearMath/btTransform.h +++ b/extern/bullet2/src/LinearMath/btTransform.h @@ -21,34 +21,39 @@ subject to the following restrictions: #include "btMatrix3x3.h" -///The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear. -///It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. +/**@brief The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear. + *It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. */ class btTransform { public: - + /**@brief No initialization constructor */ btTransform() {} - + /**@brief Constructor from btQuaternion (optional btVector3 ) + * @param q Rotation from quaternion + * @param c Translation from Vector (default 0,0,0) */ explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q, const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) : m_basis(q), m_origin(c) {} + /**@brief Constructor from btMatrix3x3 (optional btVector3) + * @param b Rotation from Matrix + * @param c Translation from Vector default (0,0,0)*/ explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b, const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) : m_basis(b), m_origin(c) {} - + /**@brief Copy constructor */ SIMD_FORCE_INLINE btTransform (const btTransform& other) : m_basis(other.m_basis), m_origin(other.m_origin) { } - + /**@brief Assignment Operator */ SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other) { m_basis = other.m_basis; @@ -57,6 +62,10 @@ public: } + /**@brief Set the current transform as the value of the product of two transforms + * @param t1 Transform 1 + * @param t2 Transform 2 + * This = Transform1 * Transform2 */ SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) { m_basis = t1.m_basis * t2.m_basis; m_origin = t1(t2.m_origin); @@ -69,7 +78,7 @@ public: } */ - +/**@brief Return the transform of the vector */ SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const { return btVector3(m_basis[0].dot(x) + m_origin.x(), @@ -77,17 +86,29 @@ public: m_basis[2].dot(x) + m_origin.z()); } + /**@brief Return the transform of the vector */ SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const { return (*this)(x); } + /**@brief Return the transform of the btQuaternion */ + SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q) const + { + return getRotation() * q; + } + + /**@brief Return the basis matrix for the rotation */ SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; } + /**@brief Return the basis matrix for the rotation */ SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; } + /**@brief Return the origin vector translation */ SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; } + /**@brief Return the origin vector translation */ SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; } + /**@brief Return a quaternion representing the rotation */ btQuaternion getRotation() const { btQuaternion q; m_basis.getRotation(q); @@ -95,12 +116,16 @@ public: } + /**@brief Set from an array + * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ void setFromOpenGLMatrix(const btScalar *m) { m_basis.setFromOpenGLSubMatrix(m); m_origin.setValue(m[12],m[13],m[14]); } + /**@brief Fill an array representation + * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ void getOpenGLMatrix(btScalar *m) const { m_basis.getOpenGLSubMatrix(m); @@ -110,6 +135,8 @@ public: m[15] = btScalar(1.0); } + /**@brief Set the translational element + * @param origin The vector to set the translation to */ SIMD_FORCE_INLINE void setOrigin(const btVector3& origin) { m_origin = origin; @@ -118,26 +145,28 @@ public: SIMD_FORCE_INLINE btVector3 invXform(const btVector3& inVec) const; - + /**@brief Set the rotational element by btMatrix3x3 */ SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis) { m_basis = basis; } + /**@brief Set the rotational element by btQuaternion */ SIMD_FORCE_INLINE void setRotation(const btQuaternion& q) { m_basis.setRotation(q); } - + /**@brief Set this transformation to the identity */ void setIdentity() { m_basis.setIdentity(); m_origin.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); } - + /**@brief Multiply this Transform by another(this = this * another) + * @param t The other transform */ btTransform& operator*=(const btTransform& t) { m_origin += m_basis * t.m_origin; @@ -145,26 +174,32 @@ public: return *this; } + /**@brief Return the inverse of this transform */ btTransform inverse() const { btMatrix3x3 inv = m_basis.transpose(); return btTransform(inv, inv * -m_origin); } + /**@brief Return the inverse of this transform times the other transform + * @param t The other transform + * return this.inverse() * the other */ btTransform inverseTimes(const btTransform& t) const; + /**@brief Return the product of this transform and the other */ btTransform operator*(const btTransform& t) const; - static btTransform getIdentity() + /**@brief Return an identity transform */ + static const btTransform& getIdentity() { - btTransform tr; - tr.setIdentity(); - return tr; + static const btTransform identityTransform(btMatrix3x3::getIdentity()); + return identityTransform; } private: - + ///Storage for the rotation btMatrix3x3 m_basis; + ///Storage for the translation btVector3 m_origin; }; @@ -191,6 +226,7 @@ btTransform::operator*(const btTransform& t) const (*this)(t.m_origin)); } +/**@brief Test if two transforms have all elements equal */ SIMD_FORCE_INLINE bool operator==(const btTransform& t1, const btTransform& t2) { return ( t1.getBasis() == t2.getBasis() && diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h index 86ee1da5edf..e8328da4ca6 100644 --- a/extern/bullet2/src/LinearMath/btTransformUtil.h +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -100,6 +100,38 @@ public: predictedTransform.setRotation(predictedOrn); } + static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel) + { + linVel = (pos1 - pos0) / timeStep; + btVector3 axis; + btScalar angle; + if (orn0 != orn1) + { + calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle); + angVel = axis * angle / timeStep; + } else + { + angVel.setValue(0,0,0); + } + } + + static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle) + { + btQuaternion orn1 = orn0.farthest(orn1a); + btQuaternion dorn = orn1 * orn0.inverse(); + ///floating point inaccuracy can lead to w component > 1..., which breaks + dorn.normalize(); + angle = dorn.getAngle(); + axis = btVector3(dorn.x(),dorn.y(),dorn.z()); + axis[3] = btScalar(0.); + //check for axis length + btScalar len = axis.length2(); + if (len < SIMD_EPSILON*SIMD_EPSILON) + axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.)); + else + axis /= btSqrt(len); + } + static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel) { linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep; @@ -111,20 +143,11 @@ public: static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle) { - - #ifdef USE_QUATERNION_DIFF - btQuaternion orn0 = transform0.getRotation(); - btQuaternion orn1a = transform1.getRotation(); - btQuaternion orn1 = orn0.farthest(orn1a); - btQuaternion dorn = orn1 * orn0.inverse(); -#else btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse(); btQuaternion dorn; dmat.getRotation(dorn); -#endif//USE_QUATERNION_DIFF - - ///floating point inaccuracy can lead to w component > 1..., which breaks + ///floating point inaccuracy can lead to w component > 1..., which breaks dorn.normalize(); angle = dorn.getAngle(); @@ -140,5 +163,85 @@ public: }; + +///The btConvexSeparatingDistanceUtil can help speed up convex collision detection +///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance +class btConvexSeparatingDistanceUtil +{ + btQuaternion m_ornA; + btQuaternion m_ornB; + btVector3 m_posA; + btVector3 m_posB; + + btVector3 m_separatingNormal; + + btScalar m_boundingRadiusA; + btScalar m_boundingRadiusB; + btScalar m_separatingDistance; + +public: + + btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar boundingRadiusB) + :m_boundingRadiusA(boundingRadiusA), + m_boundingRadiusB(boundingRadiusB), + m_separatingDistance(0.f) + { + } + + btScalar getConservativeSeparatingDistance() + { + return m_separatingDistance; + } + + void updateSeparatingDistance(const btTransform& transA,const btTransform& transB) + { + const btVector3& toPosA = transA.getOrigin(); + const btVector3& toPosB = transB.getOrigin(); + btQuaternion toOrnA = transA.getRotation(); + btQuaternion toOrnB = transB.getRotation(); + + if (m_separatingDistance>0.f) + { + + + btVector3 linVelA,angVelA,linVelB,angVelB; + btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA); + btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB); + btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB; + btVector3 relLinVel = (linVelB-linVelA); + btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal); + if (relLinVelocLength<0.f) + { + relLinVelocLength = 0.f; + } + + btScalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength; + m_separatingDistance -= projectedMotion; + } + + m_posA = toPosA; + m_posB = toPosB; + m_ornA = toOrnA; + m_ornB = toOrnB; + } + + void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB) + { + m_separatingNormal = separatingVector; + m_separatingDistance = separatingDistance; + + const btVector3& toPosA = transA.getOrigin(); + const btVector3& toPosB = transB.getOrigin(); + btQuaternion toOrnA = transA.getRotation(); + btQuaternion toOrnB = transB.getRotation(); + m_posA = toPosA; + m_posB = toPosB; + m_ornA = toOrnA; + m_ornB = toOrnB; + } + +}; + + #endif //SIMD_TRANSFORM_UTIL_H diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h index 96548c6ba60..5d5c39e8587 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -17,127 +17,190 @@ subject to the following restrictions: #ifndef SIMD__VECTOR3_H #define SIMD__VECTOR3_H -#include "btQuadWord.h" -///btVector3 can be used to represent 3D points and vectors. -///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 -///Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers -class btVector3 : public btQuadWord { +#include "btScalar.h" +#include "btScalar.h" +#include "btMinMax.h" +/**@brief btVector3 can be used to represent 3D points and vectors. + * 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 + * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers + */ +ATTRIBUTE_ALIGNED16(class) btVector3 +{ public: - SIMD_FORCE_INLINE btVector3() {} - SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) - : btQuadWord(q) +#if defined (__SPU__) && defined (__CELLOS_LV2__) + union { + vec_float4 mVec128; + btScalar m_floats[4]; + }; +public: + vec_float4 get128() const { + return mVec128; } - - - SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) - :btQuadWord(x,y,z,btScalar(0.)) +public: +#else //__CELLOS_LV2__ __SPU__ +#ifdef BT_USE_SSE // WIN32 + union { + __m128 mVec128; + btScalar m_floats[4]; + }; + SIMD_FORCE_INLINE __m128 get128() const { + return mVec128; } + SIMD_FORCE_INLINE void set128(__m128 v128) + { + mVec128 = v128; + } +#else + btScalar m_floats[4]; +#endif +#endif //__CELLOS_LV2__ __SPU__ + + public: -// SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) -// : btQuadWord(x,y,z,w) -// { -// } + /**@brief No initialization constructor */ + SIMD_FORCE_INLINE btVector3() {} + + /**@brief Constructor from scalars + * @param x X value + * @param y Y value + * @param z Z value + */ + SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) + { + m_floats[0] = x; + m_floats[1] = y; + m_floats[2] = z; + m_floats[3] = btScalar(0.); + } + +/**@brief Add a vector to this one + * @param The vector to add to this one */ SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v) { - m_x += v.x(); m_y += v.y(); m_z += v.z(); + m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2]; return *this; } - + /**@brief Subtract a vector from this one + * @param The vector to subtract */ SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) { - m_x -= v.x(); m_y -= v.y(); m_z -= v.z(); + m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2]; return *this; } - + /**@brief Scale the vector + * @param s Scale factor */ SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s) { - m_x *= s; m_y *= s; m_z *= s; + m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s; return *this; } + /**@brief Inversely scale the vector + * @param s Scale factor to divide by */ SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) { btFullAssert(s != btScalar(0.0)); return *this *= btScalar(1.0) / s; } + /**@brief Return the dot product + * @param v The other vector in the dot product */ SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const { - return m_x * v.x() + m_y * v.y() + m_z * v.z(); + return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2]; } + /**@brief Return the length of the vector squared */ SIMD_FORCE_INLINE btScalar length2() const { return dot(*this); } + /**@brief Return the length of the vector */ SIMD_FORCE_INLINE btScalar length() const { return btSqrt(length2()); } + /**@brief Return the distance squared between the ends of this and another vector + * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const; + /**@brief Return the distance between the ends of this and another vector + * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const; + /**@brief Normalize this vector + * x^2 + y^2 + z^2 = 1 */ SIMD_FORCE_INLINE btVector3& normalize() { return *this /= length(); } + /**@brief Return a normalized version of this vector */ SIMD_FORCE_INLINE btVector3 normalized() const; + /**@brief Rotate this vector + * @param wAxis The axis to rotate about + * @param angle The angle to rotate by */ SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ); + /**@brief Return the angle between this and another vector + * @param v The other vector */ SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const { btScalar s = btSqrt(length2() * v.length2()); btFullAssert(s != btScalar(0.0)); return btAcos(dot(v) / s); } - + /**@brief Return a vector will the absolute values of each element */ SIMD_FORCE_INLINE btVector3 absolute() const { return btVector3( - btFabs(m_x), - btFabs(m_y), - btFabs(m_z)); + btFabs(m_floats[0]), + btFabs(m_floats[1]), + btFabs(m_floats[2])); } - + /**@brief Return the cross product between this and another vector + * @param v The other vector */ SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const { return btVector3( - m_y * v.z() - m_z * v.y(), - m_z * v.x() - m_x * v.z(), - m_x * v.y() - m_y * v.x()); + m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1], + m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2], + m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]); } SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const { - return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) + - m_y * (v1.z() * v2.x() - v1.x() * v2.z()) + - m_z * (v1.x() * v2.y() - v1.y() * v2.x()); + return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + + m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]); } + /**@brief Return the axis with the smallest value + * Note return values are 0,1,2 for x, y, or z */ SIMD_FORCE_INLINE int minAxis() const { - return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2); + return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2); } + /**@brief Return the axis with the largest value + * Note return values are 0,1,2 for x, y, or z */ SIMD_FORCE_INLINE int maxAxis() const { - return m_x < m_y ? (m_y < m_z ? 2 : 1) : (m_x < m_z ? 2 : 0); + return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0); } SIMD_FORCE_INLINE int furthestAxis() const @@ -153,67 +216,150 @@ public: SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt) { btScalar s = btScalar(1.0) - rt; - m_x = s * v0.x() + rt * v1.x(); - m_y = s * v0.y() + rt * v1.y(); - m_z = s * v0.z() + rt * v1.z(); + m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0]; + m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1]; + m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2]; //don't do the unused w component // m_co[3] = s * v0[3] + rt * v1[3]; } + /**@brief Return the linear interpolation between this and another vector + * @param v The other vector + * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */ SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const { - return btVector3(m_x + (v.x() - m_x) * t, - m_y + (v.y() - m_y) * t, - m_z + (v.z() - m_z) * t); + return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t, + m_floats[1] + (v.m_floats[1] - m_floats[1]) * t, + m_floats[2] + (v.m_floats[2] -m_floats[2]) * t); } - + /**@brief Elementwise multiply this vector by the other + * @param v The other vector */ SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v) { - m_x *= v.x(); m_y *= v.y(); m_z *= v.z(); + m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2]; return *this; } - + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; } + /**@brief Set the x value */ + SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;}; + /**@brief Set the y value */ + SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;}; + /**@brief Set the z value */ + SIMD_FORCE_INLINE void setZ(btScalar z) {m_floats[2] = z;}; + /**@brief Set the w value */ + SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;}; + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; } + /**@brief Return the w value */ + SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; } + + //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } + //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; } + ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. + SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; } + SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; } + + SIMD_FORCE_INLINE bool operator==(const btVector3& other) const + { + return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); + } + + SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const + { + return !(*this == other); + } + + /**@brief Set each element to the max of the current values and the values of another btVector3 + * @param other The other btVector3 to compare with + */ + SIMD_FORCE_INLINE void setMax(const btVector3& other) + { + btSetMax(m_floats[0], other.m_floats[0]); + btSetMax(m_floats[1], other.m_floats[1]); + btSetMax(m_floats[2], other.m_floats[2]); + btSetMax(m_floats[3], other.w()); + } + /**@brief Set each element to the min of the current values and the values of another btVector3 + * @param other The other btVector3 to compare with + */ + SIMD_FORCE_INLINE void setMin(const btVector3& other) + { + btSetMin(m_floats[0], other.m_floats[0]); + btSetMin(m_floats[1], other.m_floats[1]); + btSetMin(m_floats[2], other.m_floats[2]); + btSetMin(m_floats[3], other.w()); + } + + SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z) + { + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3] = 0.f; + } + + void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const + { + v0->setValue(0. ,-z() ,y()); + v1->setValue(z() ,0. ,-x()); + v2->setValue(-y() ,x() ,0.); + } }; +/**@brief Return the sum of two vectors (Point symantics)*/ SIMD_FORCE_INLINE btVector3 operator+(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z()); + return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]); } +/**@brief Return the elementwise product of two vectors */ SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z()); + return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]); } +/**@brief Return the difference between two vectors */ SIMD_FORCE_INLINE btVector3 operator-(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z()); + return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]); } - +/**@brief Return the negative of the vector */ SIMD_FORCE_INLINE btVector3 operator-(const btVector3& v) { - return btVector3(-v.x(), -v.y(), -v.z()); + return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]); } +/**@brief Return the vector scaled by s */ SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v, const btScalar& s) { - return btVector3(v.x() * s, v.y() * s, v.z() * s); + return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s); } +/**@brief Return the vector scaled by s */ SIMD_FORCE_INLINE btVector3 operator*(const btScalar& s, const btVector3& v) { return v * s; } +/**@brief Return the vector inversely scaled by s */ SIMD_FORCE_INLINE btVector3 operator/(const btVector3& v, const btScalar& s) { @@ -221,12 +367,14 @@ operator/(const btVector3& v, const btScalar& s) return v * (btScalar(1.0) / s); } +/**@brief Return the vector inversely scaled by s */ SIMD_FORCE_INLINE btVector3 operator/(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z()); + return btVector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]); } +/**@brief Return the dot product between two vectors */ SIMD_FORCE_INLINE btScalar dot(const btVector3& v1, const btVector3& v2) { @@ -234,7 +382,7 @@ dot(const btVector3& v1, const btVector3& v2) } - +/**@brief Return the distance squared between two vectors */ SIMD_FORCE_INLINE btScalar distance2(const btVector3& v1, const btVector3& v2) { @@ -242,18 +390,21 @@ distance2(const btVector3& v1, const btVector3& v2) } +/**@brief Return the distance between two vectors */ SIMD_FORCE_INLINE btScalar distance(const btVector3& v1, const btVector3& v2) { return v1.distance(v2); } +/**@brief Return the angle between two vectors */ SIMD_FORCE_INLINE btScalar angle(const btVector3& v1, const btVector3& v2) { return v1.angle(v2); } +/**@brief Return the cross product of two vectors */ SIMD_FORCE_INLINE btVector3 cross(const btVector3& v1, const btVector3& v2) { @@ -266,6 +417,10 @@ triple(const btVector3& v1, const btVector3& v2, const btVector3& v3) return v1.triple(v2, v3); } +/**@brief Return the linear interpolation between two vectors + * @param v1 One vector + * @param v2 The other vector + * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */ SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) { @@ -273,10 +428,6 @@ lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) } -SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) -{ - return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z(); -} SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const { @@ -316,47 +467,47 @@ public: SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) : btVector3(x,y,z) { - m_unusedW = w; + m_floats[3] = w; } SIMD_FORCE_INLINE btVector4 absolute4() const { return btVector4( - btFabs(m_x), - btFabs(m_y), - btFabs(m_z), - btFabs(m_unusedW)); + btFabs(m_floats[0]), + btFabs(m_floats[1]), + btFabs(m_floats[2]), + btFabs(m_floats[3])); } - btScalar getW() const { return m_unusedW;} + btScalar getW() const { return m_floats[3];} SIMD_FORCE_INLINE int maxAxis4() const { int maxIndex = -1; btScalar maxVal = btScalar(-1e30); - if (m_x > maxVal) + if (m_floats[0] > maxVal) { maxIndex = 0; - maxVal = m_x; + maxVal = m_floats[0]; } - if (m_y > maxVal) + if (m_floats[1] > maxVal) { maxIndex = 1; - maxVal = m_y; + maxVal = m_floats[1]; } - if (m_z > maxVal) + if (m_floats[2] > maxVal) { maxIndex = 2; - maxVal = m_z; + maxVal =m_floats[2]; } - if (m_unusedW > maxVal) + if (m_floats[3] > maxVal) { maxIndex = 3; - maxVal = m_unusedW; + maxVal = m_floats[3]; } @@ -371,25 +522,25 @@ public: { int minIndex = -1; btScalar minVal = btScalar(1e30); - if (m_x < minVal) + if (m_floats[0] < minVal) { minIndex = 0; - minVal = m_x; + minVal = m_floats[0]; } - if (m_y < minVal) + if (m_floats[1] < minVal) { minIndex = 1; - minVal = m_y; + minVal = m_floats[1]; } - if (m_z < minVal) + if (m_floats[2] < minVal) { minIndex = 2; - minVal = m_z; + minVal =m_floats[2]; } - if (m_unusedW < minVal) + if (m_floats[3] < minVal) { minIndex = 3; - minVal = m_unusedW; + minVal = m_floats[3]; } return minIndex; @@ -402,6 +553,40 @@ public: return absolute4().maxAxis4(); } + + + + /**@brief Set x,y,z and zero w + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ + + +/* void getValue(btScalar *m) const + { + m[0] = m_floats[0]; + m[1] = m_floats[1]; + m[2] =m_floats[2]; + } +*/ +/**@brief Set the values + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ + SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) + { + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3]=w; + } + + + + }; |