diff options
Diffstat (limited to 'extern/bullet2/src/LinearMath')
25 files changed, 3194 insertions, 732 deletions
diff --git a/extern/bullet2/src/LinearMath/CMakeLists.txt b/extern/bullet2/src/LinearMath/CMakeLists.txt index 207eed94a3e..02ffaad7228 100644 --- a/extern/bullet2/src/LinearMath/CMakeLists.txt +++ b/extern/bullet2/src/LinearMath/CMakeLists.txt @@ -4,7 +4,32 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src } ) ADD_LIBRARY(LibLinearMath -btQuickprof.cpp -btGeometryUtil.cpp + btAlignedObjectArray.h + btList.h + btPoolAllocator.h + btRandom.h + btVector3.h + btDefaultMotionState.h + btMatrix3x3.h + btQuadWord.h + btHashMap.h + btScalar.h + btAabbUtil2.h + btConvexHull.h + btConvexHull.cpp + btMinMax.h + btQuaternion.h + btStackAlloc.h + btGeometryUtil.h + btMotionState.h + btTransform.h + btAlignedAllocator.h + btIDebugDraw.h + btPoint3.h + btQuickprof.h + btTransformUtil.h + btQuickprof.cpp + btGeometryUtil.cpp + btAlignedAllocator.cpp ) diff --git a/extern/bullet2/src/LinearMath/btAabbUtil2.h b/extern/bullet2/src/LinearMath/btAabbUtil2.h index 429163c8138..275c4914628 100644 --- a/extern/bullet2/src/LinearMath/btAabbUtil2.h +++ b/extern/bullet2/src/LinearMath/btAabbUtil2.h @@ -17,12 +17,18 @@ subject to the following restrictions: #ifndef AABB_UTIL2 #define AABB_UTIL2 +#include "btTransform.h" #include "btVector3.h" -#include "btSimdMinMax.h" +#include "btMinMax.h" - -#define btMin(a,b) ((a < b ? a : b)) -#define btMax(a,b) ((a > b ? a : b)) +SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin, + btVector3& aabbMax, + const btVector3& expansionMin, + const btVector3& expansionMax) +{ + aabbMin = aabbMin + expansionMin; + aabbMax = aabbMax + expansionMax; +} /// conservative test for overlap between two aabbs @@ -67,6 +73,41 @@ 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], + const btVector3 bounds[2], + btScalar& tmin, + btScalar lambda_min, + 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]; + + if ( (tmin > tymax) || (tymin > tmax) ) + return false; + + if (tymin > tmin) + tmin = tymin; + + if (tymax < tmax) + tmax = tymax; + + tzmin = (bounds[raySign[2]][2] - rayFrom[2]) * rayInvDirection[2]; + tzmax = (bounds[1-raySign[2]][2] - rayFrom[2]) * rayInvDirection[2]; + + if ( (tmin > tzmax) || (tzmin > tmax) ) + return false; + if (tzmin > tmin) + tmin = tzmin; + if (tzmax < tmax) + tmax = tzmax; + return ( (tmin < lambda_max) && (tmax > lambda_min) ); +} + SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, const btVector3& rayTo, const btVector3& aabbMin, @@ -123,5 +164,39 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, } + +SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut) +{ + btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = btVector3(abs_b[0].dot(halfExtentsWithMargin), + abs_b[1].dot(halfExtentsWithMargin), + abs_b[2].dot(halfExtentsWithMargin)); + aabbMinOut = center - extent; + aabbMaxOut = center + extent; +} + + +SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut) +{ + btAssert(localAabbMin.getX() <= localAabbMax.getX()); + btAssert(localAabbMin.getY() <= localAabbMax.getY()); + btAssert(localAabbMin.getZ() <= localAabbMax.getZ()); + btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin); + localHalfExtents+=btVector3(margin,margin,margin); + + btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin); + btMatrix3x3 abs_b = trans.getBasis().absolute(); + btVector3 center = trans(localCenter); + btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + aabbMinOut = center-extent; + aabbMaxOut = center+extent; +} + + #endif + diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp index 1f5877fa37e..e120289e061 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp @@ -15,56 +15,187 @@ subject to the following restrictions: #include "btAlignedAllocator.h" +int gNumAlignedAllocs = 0; +int gNumAlignedFree = 0; +int gTotalBytesAlignedAllocs = 0;//detect memory leaks -#if defined (BT_HAS_ALIGNED_ALOCATOR) - +#if defined (BT_HAS_ALIGNED_ALLOCATOR) #include <malloc.h> -void* btAlignedAlloc (int size, int alignment) +static void *btAlignedAllocDefault(size_t size, int alignment) { - return _aligned_malloc(size,alignment); + return _aligned_malloc(size, (size_t)alignment); } -void btAlignedFree (void* ptr) +static void btAlignedFreeDefault(void *ptr) { _aligned_free(ptr); } +#elif defined(__CELLOS_LV2__) +#include <stdlib.h> -#else +static inline void *btAlignedAllocDefault(size_t size, int alignment) +{ + return memalign(alignment, size); +} -#ifdef __CELLOS_LV2__ +static inline void btAlignedFreeDefault(void *ptr) +{ + free(ptr); +} +#else +static inline void *btAlignedAllocDefault(size_t size, int alignment) +{ + void *ret; + char *real; + unsigned long offset; + + real = (char *)malloc(size + sizeof(void *) + (alignment-1)); + if (real) { + offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1); + ret = (void *)((real + sizeof(void *)) + offset); + *((void **)(ret)-1) = (void *)(real); + } else { + ret = (void *)(real); + } + return (ret); +} -#include <stdlib.h> +static inline void btAlignedFreeDefault(void *ptr) +{ + void* real; -int numAllocs = 0; -int numFree = 0; + if (ptr) { + real = *((void **)(ptr)-1); + free(real); + } +} +#endif -void* btAlignedAlloc (int size, int alignment) +static void *btAllocDefault(size_t size) { - numAllocs++; - return memalign(alignment, size); + return malloc(size); } -void btAlignedFree (void* ptr) +static void btFreeDefault(void *ptr) { - numFree++; free(ptr); } -#else -///todo -///will add some multi-platform version that works without _aligned_malloc/_aligned_free +static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault; +static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault; +static btAllocFunc *sAllocFunc = btAllocDefault; +static btFreeFunc *sFreeFunc = btFreeDefault; -void* btAlignedAlloc (int size, int alignment) +void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc) { - return new char[size]; + sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault; + sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault; } -void btAlignedFree (void* ptr) +void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc) { - delete [] (char*) ptr; + sAllocFunc = allocFunc ? allocFunc : btAllocDefault; + sFreeFunc = freeFunc ? freeFunc : btFreeDefault; } -#endif // -#endif +#ifdef BT_DEBUG_MEMORY_ALLOCATIONS +//this generic allocator provides the total allocated number of bytes +#include <stdio.h> + +void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename) +{ + void *ret; + char *real; + unsigned long offset; + + gTotalBytesAlignedAllocs += size; + gNumAlignedAllocs++; + + + real = (char *)sAllocFunc(size + 2*sizeof(void *) + (alignment-1)); + if (real) { + offset = (alignment - (unsigned long)(real + 2*sizeof(void *))) & +(alignment-1); + ret = (void *)((real + 2*sizeof(void *)) + offset); + *((void **)(ret)-1) = (void *)(real); + *((int*)(ret)-2) = size; + + } else { + ret = (void *)(real);//?? + } + + printf("allocation#%d at address %x, from %s,line %d, size %d\n",gNumAlignedAllocs,real, filename,line,size); + + int* ptr = (int*)ret; + *ptr = 12; + return (ret); +} + +void btAlignedFreeInternal (void* ptr,int line,char* filename) +{ + + void* real; + gNumAlignedFree++; + + if (ptr) { + real = *((void **)(ptr)-1); + int size = *((int*)(ptr)-2); + gTotalBytesAlignedAllocs -= size; + + printf("free #%d at address %x, from %s,line %d, size %d\n",gNumAlignedFree,real, filename,line,size); + + sFreeFunc(real); + } else + { + printf("NULL ptr\n"); + } +} + +#else //BT_DEBUG_MEMORY_ALLOCATIONS + +void* btAlignedAllocInternal (size_t size, int alignment) +{ + gNumAlignedAllocs++; + void* ptr; +#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) + ptr = sAlignedAllocFunc(size, alignment); +#else + char *real; + unsigned long offset; + + real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1)); + if (real) { + offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1); + ptr = (void *)((real + sizeof(void *)) + offset); + *((void **)(ptr)-1) = (void *)(real); + } else { + ptr = (void *)(real); + } +#endif // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) +// printf("btAlignedAllocInternal %d, %x\n",size,ptr); + return ptr; +} + +void btAlignedFreeInternal (void* ptr) +{ + if (!ptr) + { + return; + } + + gNumAlignedFree++; +// printf("btAlignedFreeInternal %x\n",ptr); +#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) + sAlignedFreeFunc(ptr); +#else + void* real; + + if (ptr) { + real = *((void **)(ptr)-1); + sFreeFunc(real); + } +#endif // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) +} +#endif //BT_DEBUG_MEMORY_ALLOCATIONS diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.h b/extern/bullet2/src/LinearMath/btAlignedAllocator.h index 07585717f45..a252f324d77 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.h +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.h @@ -21,15 +21,39 @@ subject to the following restrictions: ///that is better portable and more predictable #include "btScalar.h" +//#define BT_DEBUG_MEMORY_ALLOCATIONS 1 +#ifdef BT_DEBUG_MEMORY_ALLOCATIONS -void* btAlignedAlloc (int size, int alignment); +#define btAlignedAlloc(a,b) \ + btAlignedAllocInternal(a,b,__LINE__,__FILE__) -void btAlignedFree (void* ptr); +#define btAlignedFree(ptr) \ + btAlignedFreeInternal(ptr,__LINE__,__FILE__) +void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename); +void btAlignedFreeInternal (void* ptr,int line,char* filename); + +#else + void* btAlignedAllocInternal (size_t size, int alignment); + void btAlignedFreeInternal (void* ptr); + + #define btAlignedAlloc(a,b) btAlignedAllocInternal(a,b) + #define btAlignedFree(ptr) btAlignedFreeInternal(ptr) + +#endif typedef int size_type; +typedef void *(btAlignedAllocFunc)(size_t size, int alignment); +typedef void (btAlignedFreeFunc)(void *memblock); +typedef void *(btAllocFunc)(size_t size); +typedef void (btFreeFunc)(void *memblock); + +void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc); +void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *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. template < typename T , unsigned Alignment > class btAlignedAllocator { diff --git a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h index 8bef5eb5d06..5598f0d7236 100644 --- a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h +++ b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h @@ -39,8 +39,8 @@ subject to the following restrictions: #endif //BT_USE_PLACEMENT_NEW -///btAlignedObjectArray uses a subset of the stl::vector interface for its methods -///It is developed to replace stl::vector to avoid STL alignment issues to add SIMD/SSE data +///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods +///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data template <typename T> //template <class T> class btAlignedObjectArray @@ -50,6 +50,8 @@ class btAlignedObjectArray int m_size; int m_capacity; T* m_data; + //PCK: added this line + bool m_ownsMemory; protected: SIMD_FORCE_INLINE int allocSize(int size) @@ -69,6 +71,8 @@ class btAlignedObjectArray SIMD_FORCE_INLINE void init() { + //PCK: added this line + m_ownsMemory = true; m_data = 0; m_size = 0; m_capacity = 0; @@ -92,7 +96,11 @@ class btAlignedObjectArray SIMD_FORCE_INLINE void deallocate() { if(m_data) { - m_allocator.deallocate(m_data); + //PCK: enclosed the deallocation in this block + if (m_ownsMemory) + { + m_allocator.deallocate(m_data); + } m_data = 0; } } @@ -223,6 +231,9 @@ class btAlignedObjectArray destroy(0,size()); deallocate(); + + //PCK: added this line + m_ownsMemory = true; m_data = s; @@ -242,6 +253,46 @@ class btAlignedObjectArray } }; + template <typename L> + void quickSortInternal(L CompareFunc,int lo, int hi) + { + // lo is the lower index, hi is the upper index + // of the region of array a that is to be sorted + int i=lo, j=hi; + T x=m_data[(lo+hi)/2]; + + // partition + do + { + while (CompareFunc(m_data[i],x)) + i++; + while (CompareFunc(x,m_data[j])) + j--; + if (i<=j) + { + swap(i,j); + i++; j--; + } + } while (i<=j); + + // recursion + if (lo<j) + quickSortInternal( CompareFunc, lo, j); + if (i<hi) + quickSortInternal( CompareFunc, i, hi); + } + + + template <typename L> + void quickSort(L CompareFunc) + { + //don't sort 0 or 1 elements + if (size()>1) + { + quickSortInternal(CompareFunc,0,size()-1); + } + } + ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/ template <typename L> @@ -360,8 +411,16 @@ class btAlignedObjectArray } } + //PCK: whole function + void initializeFromBuffer(void *buffer, int size, int capacity) + { + clear(); + m_ownsMemory = false; + m_data = (T*)buffer; + m_size = size; + m_capacity = capacity; + } + }; #endif //BT_OBJECT_ARRAY__ - - diff --git a/extern/bullet2/src/LinearMath/btConvexHull.cpp b/extern/bullet2/src/LinearMath/btConvexHull.cpp new file mode 100644 index 00000000000..b8929a86808 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btConvexHull.cpp @@ -0,0 +1,1174 @@ +/* +Stan Melax Convex Hull Computation +Copyright (c) 2003-2006 Stan Melax http://www.melax.com/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include <string.h> + +#include "btConvexHull.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btMinMax.h" +#include "LinearMath/btVector3.h" + + + +template <class T> +void Swap(T &a,T &b) +{ + T tmp = a; + a=b; + b=tmp; +} + + +//---------------------------------- + +class int3 +{ +public: + int x,y,z; + int3(){}; + int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;} + const int& operator[](int i) const {return (&x)[i];} + int& operator[](int i) {return (&x)[i];} +}; + + +//------- btPlane ---------- + + +inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);} +inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); } +inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); } + + +//--------- Utility Functions ------ + +btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1); +btVector3 PlaneProject(const btPlane &plane, const btVector3 &point); + +btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2); +btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2) +{ + btVector3 N1 = p0.normal; + btVector3 N2 = p1.normal; + btVector3 N3 = p2.normal; + + btVector3 n2n3; n2n3 = N2.cross(N3); + btVector3 n3n1; n3n1 = N3.cross(N1); + btVector3 n1n2; n1n2 = N1.cross(N2); + + btScalar quotient = (N1.dot(n2n3)); + + btAssert(btFabs(quotient) > btScalar(0.000001)); + + quotient = btScalar(-1.) / quotient; + n2n3 *= p0.dist; + n3n1 *= p1.dist; + n1n2 *= p2.dist; + btVector3 potentialVertex = n2n3; + potentialVertex += n3n1; + potentialVertex += n1n2; + potentialVertex *= quotient; + + btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ()); + return result; + +} + +btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL); +btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2); +btVector3 NormalOf(const btVector3 *vert, const int n); + + +btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1) +{ + // returns the point where the line p0-p1 intersects the plane n&d + static btVector3 dif; + dif = p1-p0; + btScalar dn= dot(plane.normal,dif); + btScalar t = -(plane.dist+dot(plane.normal,p0) )/dn; + return p0 + (dif*t); +} + +btVector3 PlaneProject(const btPlane &plane, const btVector3 &point) +{ + return point - plane.normal * (dot(point,plane.normal)+plane.dist); +} + +btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) +{ + // return the normal of the triangle + // inscribed by v0, v1, and v2 + btVector3 cp=cross(v1-v0,v2-v1); + btScalar m=cp.length(); + if(m==0) return btVector3(1,0,0); + return cp*(btScalar(1.0)/m); +} + + +btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint) +{ + static btVector3 cp; + cp = cross(udir,vdir).normalized(); + + btScalar distu = -dot(cp,ustart); + btScalar distv = -dot(cp,vstart); + btScalar dist = (btScalar)fabs(distu-distv); + if(upoint) + { + btPlane plane; + plane.normal = cross(vdir,cp).normalized(); + plane.dist = -dot(plane.normal,vstart); + *upoint = PlaneLineIntersection(plane,ustart,ustart+udir); + } + if(vpoint) + { + btPlane plane; + plane.normal = cross(udir,cp).normalized(); + plane.dist = -dot(plane.normal,ustart); + *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); + } + return dist; +} + + + + + + + +#define COPLANAR (0) +#define UNDER (1) +#define OVER (2) +#define SPLIT (OVER|UNDER) +#define PAPERWIDTH (btScalar(0.001)) + +btScalar planetestepsilon = PAPERWIDTH; + + + +typedef ConvexH::HalfEdge HalfEdge; + +ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size) +{ + vertices.resize(vertices_size); + edges.resize(edges_size); + facets.resize(facets_size); +} + + +int PlaneTest(const btPlane &p, const btVector3 &v); +int PlaneTest(const btPlane &p, const btVector3 &v) { + btScalar a = dot(v,p.normal)+p.dist; + int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR); + return flag; +} + +int SplitTest(ConvexH &convex,const btPlane &plane); +int SplitTest(ConvexH &convex,const btPlane &plane) { + int flag=0; + for(int i=0;i<convex.vertices.size();i++) { + flag |= PlaneTest(plane,convex.vertices[i]); + } + return flag; +} + +class VertFlag +{ +public: + unsigned char planetest; + unsigned char junk; + unsigned char undermap; + unsigned char overmap; +}; +class EdgeFlag +{ +public: + unsigned char planetest; + unsigned char fixes; + short undermap; + short overmap; +}; +class PlaneFlag +{ +public: + unsigned char undermap; + unsigned char overmap; +}; +class Coplanar{ +public: + unsigned short ea; + unsigned char v0; + unsigned char v1; +}; + + + + + + + + +template<class T> +int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow) +{ + btAssert(count); + int m=-1; + for(int i=0;i<count;i++) + if(allow[i]) + { + if(m==-1 || dot(p[i],dir)>dot(p[m],dir)) + m=i; + } + btAssert(m!=-1); + return m; +} + +btVector3 orth(const btVector3 &v); +btVector3 orth(const btVector3 &v) +{ + btVector3 a=cross(v,btVector3(0,0,1)); + btVector3 b=cross(v,btVector3(0,1,0)); + if (a.length() > b.length()) + { + return a.normalized(); + } else { + return b.normalized(); + } +} + + +template<class T> +int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow) +{ + int m=-1; + while(m==-1) + { + m = maxdirfiltered(p,count,dir,allow); + if(allow[m]==3) return m; + T u = orth(dir); + T v = cross(u,dir); + 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)); + int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); + if(ma==m && mb==m) + { + allow[m]=3; + return m; + } + if(ma!=-1 && ma!=mb) // Yuck - this is really ugly + { + 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)); + int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); + if(mc==m && md==m) + { + allow[m]=3; + return m; + } + mc=md; + } + } + ma=mb; + } + allow[m]=0; + m=-1; + } + btAssert(0); + return m; +} + + + + +int operator ==(const int3 &a,const int3 &b); +int operator ==(const int3 &a,const int3 &b) +{ + for(int i=0;i<3;i++) + { + if(a[i]!=b[i]) return 0; + } + return 1; +} + + +int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon); +int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) +{ + btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); + return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? +} +int hasedge(const int3 &t, int a,int b); +int hasedge(const int3 &t, int a,int b) +{ + for(int i=0;i<3;i++) + { + int i1= (i+1)%3; + if(t[i]==a && t[i1]==b) return 1; + } + return 0; +} +int hasvert(const int3 &t, int v); +int hasvert(const int3 &t, int v) +{ + return (t[0]==v || t[1]==v || t[2]==v) ; +} +int shareedge(const int3 &a,const int3 &b); +int shareedge(const int3 &a,const int3 &b) +{ + int i; + for(i=0;i<3;i++) + { + int i1= (i+1)%3; + if(hasedge(a,b[i1],b[i])) return 1; + } + return 0; +} + +class Tri; + + + +class Tri : 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) + { + vmax=-1; + rise = btScalar(0.0); + } + ~Tri() + { + } + int &neib(int a,int b); +}; + + +int &Tri::neib(int a,int b) +{ + static int er=-1; + int i; + for(i=0;i<3;i++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + if((*this)[i]==a && (*this)[i1]==b) return n[i2]; + if((*this)[i]==b && (*this)[i1]==a) return n[i2]; + } + btAssert(0); + return er; +} +void HullLibrary::b2bfix(Tri* s,Tri*t) +{ + int i; + for(i=0;i<3;i++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + int a = (*s)[i1]; + int b = (*s)[i2]; + btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id); + btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id); + m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a); + m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b); + } +} + +void HullLibrary::removeb2b(Tri* s,Tri*t) +{ + b2bfix(s,t); + deAllocateTriangle(s); + + deAllocateTriangle(t); +} + +void HullLibrary::checkit(Tri *t) +{ + (void)t; + + int i; + btAssert(m_tris[t->id]==t); + for(i=0;i<3;i++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + int a = (*t)[i1]; + int b = (*t)[i2]; + + // release compile fix + (void)i1; + (void)i2; + (void)a; + (void)b; + + btAssert(a!=b); + btAssert( m_tris[t->n[i]]->neib(b,a) == t->id); + } +} + +Tri* HullLibrary::allocateTriangle(int a,int b,int c) +{ + void* mem = btAlignedAlloc(sizeof(Tri),16); + Tri* tr = new (mem)Tri(a,b,c); + tr->id = m_tris.size(); + m_tris.push_back(tr); + + return tr; +} + +void HullLibrary::deAllocateTriangle(Tri* tri) +{ + btAssert(m_tris[tri->id]==tri); + m_tris[tri->id]=NULL; + tri->~Tri(); + btAlignedFree(tri); +} + + +void HullLibrary::extrude(Tri *t0,int v) +{ + int3 t= *t0; + int n = m_tris.size(); + Tri* 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]); + 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]); + tc->n = int3(t0->n[2],n+0,n+1); + m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2; + checkit(ta); + checkit(tb); + checkit(tc); + if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]); + if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]); + if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]); + deAllocateTriangle(t0); + +} + +Tri* HullLibrary::extrudable(btScalar epsilon) +{ + int i; + Tri *t=NULL; + for(i=0;i<m_tris.size();i++) + { + if(!t || (m_tris[i] && t->rise<m_tris[i]->rise)) + { + t = m_tris[i]; + } + } + return (t->rise >epsilon)?t:NULL ; +} + + + + +int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow) +{ + btVector3 basis[3]; + basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) ); + int p0 = maxdirsterid(verts,verts_count, basis[0],allow); + int p1 = maxdirsterid(verts,verts_count,-basis[0],allow); + basis[0] = verts[p0]-verts[p1]; + if(p0==p1 || basis[0]==btVector3(0,0,0)) + return int4(-1,-1,-1,-1); + basis[1] = cross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]); + basis[2] = cross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]); + if (basis[1].length() > basis[2].length()) + { + basis[1].normalize(); + } else { + basis[1] = basis[2]; + basis[1].normalize (); + } + int p2 = maxdirsterid(verts,verts_count,basis[1],allow); + if(p2 == p0 || p2 == p1) + { + p2 = maxdirsterid(verts,verts_count,-basis[1],allow); + } + if(p2 == p0 || p2 == p1) + return int4(-1,-1,-1,-1); + basis[1] = verts[p2] - verts[p0]; + basis[2] = cross(basis[1],basis[0]).normalized(); + int p3 = maxdirsterid(verts,verts_count,basis[2],allow); + if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow); + if(p3==p0||p3==p1||p3==p2) + return int4(-1,-1,-1,-1); + btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3)); + if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);} + return int4(p0,p1,p2,p3); +} + +int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) +{ + if(verts_count <4) return 0; + if(vlimit==0) vlimit=1000000000; + int j; + btVector3 bmin(*verts),bmax(*verts); + btAlignedObjectArray<int> isextreme; + isextreme.reserve(verts_count); + btAlignedObjectArray<int> allow; + allow.reserve(verts_count); + + for(j=0;j<verts_count;j++) + { + allow.push_back(1); + isextreme.push_back(0); + bmin.setMin (verts[j]); + bmax.setMax (verts[j]); + } + btScalar epsilon = (bmax-bmin).length() * btScalar(0.001); + btAssert (epsilon != 0.0); + + + int4 p = FindSimplex(verts,verts_count,allow); + if(p.x==-1) return 0; // simplex failed + + + + 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); + 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]; + 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; + vlimit-=4; + while(vlimit >0 && ((te=extrudable(epsilon)) != 0)) + { + int3 ti=*te; + int v=te->vmax; + btAssert(v != -1); + btAssert(!isextreme[v]); // wtf we've already done this vertex + isextreme[v]=1; + //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already + j=m_tris.size(); + while(j--) { + if(!m_tris[j]) continue; + int3 t=*m_tris[j]; + if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) + { + extrude(m_tris[j],v); + } + } + // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle + j=m_tris.size(); + while(j--) + { + if(!m_tris[j]) continue; + if(!hasvert(*m_tris[j],v)) break; + 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]]; + btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j); + extrude(nb,v); + j=m_tris.size(); + } + } + j=m_tris.size(); + while(j--) + { + Tri *t=m_tris[j]; + if(!t) continue; + if(t->vmax>=0) break; + btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); + t->vmax = maxdirsterid(verts,verts_count,n,allow); + if(isextreme[t->vmax]) + { + t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate. + } + else + { + t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); + } + } + vlimit --; + } + return 1; +} + +int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit) +{ + int rc=calchullgen(verts,verts_count, vlimit) ; + if(!rc) return 0; + btAlignedObjectArray<int> ts; + int i; + + for(i=0;i<m_tris.size();i++) + { + if(m_tris[i]) + { + for(int j=0;j<3;j++) + ts.push_back((*m_tris[i])[j]); + deAllocateTriangle(m_tris[i]); + } + } + tris_count = ts.size()/3; + tris_out.resize(ts.size()); + + for (i=0;i<ts.size();i++) + { + tris_out[i] = static_cast<unsigned int>(ts[i]); + } + m_tris.resize(0); + + return 1; +} + + + + + +bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit) +{ + + int tris_count; + int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) ); + if(!ret) return false; + result.mIndexCount = (unsigned int) (tris_count*3); + result.mFaceCount = (unsigned int) tris_count; + result.mVertices = (btVector3*) vertices; + result.mVcount = (unsigned int) vcount; + return true; + +} + + +void ReleaseHull(PHullResult &result); +void ReleaseHull(PHullResult &result) +{ + if ( result.m_Indices.size() ) + { + result.m_Indices.clear(); + } + + result.mVcount = 0; + result.mIndexCount = 0; + result.mVertices = 0; +} + + +//********************************************************************* +//********************************************************************* +//******** HullLib header +//********************************************************************* +//********************************************************************* + +//********************************************************************* +//********************************************************************* +//******** HullLib implementation +//********************************************************************* +//********************************************************************* + +HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request + HullResult &result) // contains the resulst +{ + HullError ret = QE_FAIL; + + + PHullResult hr; + + unsigned int vcount = desc.mVcount; + if ( vcount < 8 ) vcount = 8; + + btAlignedObjectArray<btVector3> vertexSource; + vertexSource.resize(static_cast<int>(vcount)); + + btVector3 scale; + + unsigned int ovcount; + + bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates! + + if ( ok ) + { + + +// if ( 1 ) // scale vertices back to their original size. + { + for (unsigned int i=0; i<ovcount; i++) + { + btVector3& v = vertexSource[static_cast<int>(i)]; + v[0]*=scale[0]; + v[1]*=scale[1]; + v[2]*=scale[2]; + } + } + + ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices); + + if ( ok ) + { + + // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table. + btAlignedObjectArray<btVector3> vertexScratch; + vertexScratch.resize(static_cast<int>(hr.mVcount)); + + BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount ); + + ret = QE_OK; + + if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle! + { + result.mPolygons = false; + result.mNumOutputVertices = ovcount; + result.m_OutputVertices.resize(static_cast<int>(ovcount)); + result.mNumFaces = hr.mFaceCount; + result.mNumIndices = hr.mIndexCount; + + result.m_Indices.resize(static_cast<int>(hr.mIndexCount)); + + memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); + + if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) + { + + const unsigned int *source = &hr.m_Indices[0]; + unsigned int *dest = &result.m_Indices[0]; + + for (unsigned int i=0; i<hr.mFaceCount; i++) + { + dest[0] = source[2]; + dest[1] = source[1]; + dest[2] = source[0]; + dest+=3; + source+=3; + } + + } + else + { + memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int)*hr.mIndexCount); + } + } + else + { + result.mPolygons = true; + result.mNumOutputVertices = ovcount; + result.m_OutputVertices.resize(static_cast<int>(ovcount)); + result.mNumFaces = hr.mFaceCount; + result.mNumIndices = hr.mIndexCount+hr.mFaceCount; + result.m_Indices.resize(static_cast<int>(result.mNumIndices)); + memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); + +// if ( 1 ) + { + const unsigned int *source = &hr.m_Indices[0]; + unsigned int *dest = &result.m_Indices[0]; + for (unsigned int i=0; i<hr.mFaceCount; i++) + { + dest[0] = 3; + if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) + { + dest[1] = source[2]; + dest[2] = source[1]; + dest[3] = source[0]; + } + else + { + dest[1] = source[0]; + dest[2] = source[1]; + dest[3] = source[2]; + } + + dest+=4; + source+=3; + } + } + } + ReleaseHull(hr); + } + } + + return ret; +} + + + +HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it. +{ + if ( result.m_OutputVertices.size()) + { + result.mNumOutputVertices=0; + result.m_OutputVertices.clear(); + } + if ( result.m_Indices.size() ) + { + result.mNumIndices=0; + result.m_Indices.clear(); + } + return QE_OK; +} + + +static void addPoint(unsigned int &vcount,btVector3 *p,btScalar x,btScalar y,btScalar z) +{ + // XXX, might be broken + btVector3& dest = p[vcount]; + dest[0] = x; + dest[1] = y; + dest[2] = z; + vcount++; +} + +btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2); +btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2) +{ + + btScalar dx = px - p2[0]; + btScalar dy = py - p2[1]; + btScalar dz = pz - p2[2]; + + return dx*dx+dy*dy+dz*dz; +} + + + +bool HullLibrary::CleanupVertices(unsigned int svcount, + const btVector3 *svertices, + unsigned int stride, + unsigned int &vcount, // output number of vertices + btVector3 *vertices, // location to store the results. + btScalar normalepsilon, + btVector3& scale) +{ + if ( svcount == 0 ) return false; + + m_vertexIndexMapping.resize(0); + + +#define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */ + + vcount = 0; + + btScalar recip[3]; + + if ( scale ) + { + scale[0] = 1; + scale[1] = 1; + scale[2] = 1; + } + + btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; + btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; + + const char *vtx = (const char *) svertices; + +// if ( 1 ) + { + for (unsigned int i=0; i<svcount; i++) + { + const btScalar *p = (const btScalar *) vtx; + + vtx+=stride; + + for (int j=0; j<3; j++) + { + if ( p[j] < bmin[j] ) bmin[j] = p[j]; + if ( p[j] > bmax[j] ) bmax[j] = p[j]; + } + } + } + + btScalar dx = bmax[0] - bmin[0]; + btScalar dy = bmax[1] - bmin[1]; + btScalar dz = bmax[2] - bmin[2]; + + btVector3 center; + + center[0] = dx*btScalar(0.5) + bmin[0]; + center[1] = dy*btScalar(0.5) + bmin[1]; + center[2] = dz*btScalar(0.5) + bmin[2]; + + if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 ) + { + + btScalar len = FLT_MAX; + + if ( dx > EPSILON && dx < len ) len = dx; + if ( dy > EPSILON && dy < len ) len = dy; + if ( dz > EPSILON && dz < len ) len = dz; + + if ( len == FLT_MAX ) + { + dx = dy = dz = btScalar(0.01); // one centimeter + } + else + { + if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. + if ( dy < EPSILON ) dy = len * btScalar(0.05); + if ( dz < EPSILON ) dz = len * btScalar(0.05); + } + + btScalar x1 = center[0] - dx; + btScalar x2 = center[0] + dx; + + btScalar y1 = center[1] - dy; + btScalar y2 = center[1] + dy; + + btScalar z1 = center[2] - dz; + btScalar z2 = center[2] + dz; + + addPoint(vcount,vertices,x1,y1,z1); + addPoint(vcount,vertices,x2,y1,z1); + addPoint(vcount,vertices,x2,y2,z1); + addPoint(vcount,vertices,x1,y2,z1); + addPoint(vcount,vertices,x1,y1,z2); + addPoint(vcount,vertices,x2,y1,z2); + addPoint(vcount,vertices,x2,y2,z2); + addPoint(vcount,vertices,x1,y2,z2); + + return true; // return cube + + + } + else + { + if ( scale ) + { + scale[0] = dx; + scale[1] = dy; + scale[2] = dz; + + recip[0] = 1 / dx; + recip[1] = 1 / dy; + recip[2] = 1 / dz; + + center[0]*=recip[0]; + center[1]*=recip[1]; + center[2]*=recip[2]; + + } + + } + + + + vtx = (const char *) svertices; + + for (unsigned int i=0; i<svcount; i++) + { + const btVector3 *p = (const btVector3 *)vtx; + vtx+=stride; + + btScalar px = p->getX(); + btScalar py = p->getY(); + btScalar pz = p->getZ(); + + if ( scale ) + { + px = px*recip[0]; // normalize + py = py*recip[1]; // normalize + pz = pz*recip[2]; // normalize + } + +// if ( 1 ) + { + unsigned int j; + + for (j=0; j<vcount; j++) + { + /// XXX might be broken + btVector3& v = vertices[j]; + + btScalar x = v[0]; + btScalar y = v[1]; + btScalar z = v[2]; + + btScalar dx = fabsf(x - px ); + btScalar dy = fabsf(y - py ); + btScalar dz = fabsf(z - pz ); + + if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon ) + { + // ok, it is close enough to the old one + // now let us see if it is further from the center of the point cloud than the one we already recorded. + // in which case we keep this one instead. + + btScalar dist1 = GetDist(px,py,pz,center); + btScalar dist2 = GetDist(v[0],v[1],v[2],center); + + if ( dist1 > dist2 ) + { + v[0] = px; + v[1] = py; + v[2] = pz; + + } + + break; + } + } + + if ( j == vcount ) + { + btVector3& dest = vertices[vcount]; + dest[0] = px; + dest[1] = py; + dest[2] = pz; + vcount++; + } + m_vertexIndexMapping.push_back(j); + } + } + + // ok..now make sure we didn't prune so many vertices it is now invalid. +// if ( 1 ) + { + btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; + btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; + + for (unsigned int i=0; i<vcount; i++) + { + const btVector3& p = vertices[i]; + for (int j=0; j<3; j++) + { + if ( p[j] < bmin[j] ) bmin[j] = p[j]; + if ( p[j] > bmax[j] ) bmax[j] = p[j]; + } + } + + btScalar dx = bmax[0] - bmin[0]; + btScalar dy = bmax[1] - bmin[1]; + btScalar dz = bmax[2] - bmin[2]; + + if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3) + { + btScalar cx = dx*btScalar(0.5) + bmin[0]; + btScalar cy = dy*btScalar(0.5) + bmin[1]; + btScalar cz = dz*btScalar(0.5) + bmin[2]; + + btScalar len = FLT_MAX; + + if ( dx >= EPSILON && dx < len ) len = dx; + if ( dy >= EPSILON && dy < len ) len = dy; + if ( dz >= EPSILON && dz < len ) len = dz; + + if ( len == FLT_MAX ) + { + dx = dy = dz = btScalar(0.01); // one centimeter + } + else + { + if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. + if ( dy < EPSILON ) dy = len * btScalar(0.05); + if ( dz < EPSILON ) dz = len * btScalar(0.05); + } + + btScalar x1 = cx - dx; + btScalar x2 = cx + dx; + + btScalar y1 = cy - dy; + btScalar y2 = cy + dy; + + btScalar z1 = cz - dz; + btScalar z2 = cz + dz; + + vcount = 0; // add box + + addPoint(vcount,vertices,x1,y1,z1); + addPoint(vcount,vertices,x2,y1,z1); + addPoint(vcount,vertices,x2,y2,z1); + addPoint(vcount,vertices,x1,y2,z1); + addPoint(vcount,vertices,x1,y1,z2); + addPoint(vcount,vertices,x2,y1,z2); + addPoint(vcount,vertices,x2,y2,z2); + addPoint(vcount,vertices,x1,y2,z2); + + return true; + } + } + + return true; +} + +void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount) +{ + btAlignedObjectArray<int>tmpIndices; + tmpIndices.resize(m_vertexIndexMapping.size()); + int i; + + for (i=0;i<m_vertexIndexMapping.size();i++) + { + tmpIndices[i] = m_vertexIndexMapping[i]; + } + + TUIntArray usedIndices; + usedIndices.resize(static_cast<int>(vcount)); + memset(&usedIndices[0],0,sizeof(unsigned int)*vcount); + + ocount = 0; + + for (i=0; i<indexcount; i++) + { + unsigned int v = indices[i]; // original array index + + btAssert( v >= 0 && v < vcount ); + + if ( usedIndices[static_cast<int>(v)] ) // if already remapped + { + indices[i] = usedIndices[static_cast<int>(v)]-1; // index to new array + } + else + { + + indices[i] = ocount; // new index mapping + + overts[ocount][0] = verts[v][0]; // copy old vert to new vert array + overts[ocount][1] = verts[v][1]; + overts[ocount][2] = verts[v][2]; + + for (int k=0;k<m_vertexIndexMapping.size();k++) + { + if (tmpIndices[k]==v) + m_vertexIndexMapping[k]=ocount; + } + + ocount++; // increment output vert count + + btAssert( ocount >=0 && ocount <= vcount ); + + usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping + + + } + } + + +} diff --git a/extern/bullet2/src/LinearMath/btConvexHull.h b/extern/bullet2/src/LinearMath/btConvexHull.h new file mode 100644 index 00000000000..8c36307dfd6 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btConvexHull.h @@ -0,0 +1,245 @@ + +/* +Stan Melax Convex Hull Computation +Copyright (c) 2008 Stan Melax http://www.melax.com/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///includes modifications/improvements by John Ratcliff, see BringOutYourDead below. + +#ifndef CD_HULL_H +#define CD_HULL_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +typedef btAlignedObjectArray<unsigned int> TUIntArray; + +class HullResult +{ +public: + HullResult(void) + { + mPolygons = true; + mNumOutputVertices = 0; + mNumFaces = 0; + mNumIndices = 0; + } + bool mPolygons; // true if indices represents polygons, false indices are triangles + unsigned int mNumOutputVertices; // number of vertices in the output hull + btAlignedObjectArray<btVector3> m_OutputVertices; // array of vertices + unsigned int mNumFaces; // the number of faces produced + unsigned int mNumIndices; // the total number of indices + btAlignedObjectArray<unsigned int> m_Indices; // pointer to indices. + +// If triangles, then indices are array indexes into the vertex list. +// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. +}; + +enum HullFlag +{ + QF_TRIANGLES = (1<<0), // report results as triangles, not polygons. + QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices. + QF_DEFAULT = QF_TRIANGLES +}; + + +class HullDesc +{ +public: + HullDesc(void) + { + mFlags = QF_DEFAULT; + mVcount = 0; + mVertices = 0; + mVertexStride = sizeof(btVector3); + mNormalEpsilon = 0.001f; + mMaxVertices = 4096; // maximum number of points to be considered for a convex hull. + mMaxFaces = 4096; + }; + + HullDesc(HullFlag flag, + unsigned int vcount, + const btVector3 *vertices, + unsigned int stride = sizeof(btVector3)) + { + mFlags = flag; + mVcount = vcount; + mVertices = vertices; + mVertexStride = stride; + mNormalEpsilon = btScalar(0.001); + mMaxVertices = 4096; + } + + bool HasHullFlag(HullFlag flag) const + { + if ( mFlags & flag ) return true; + return false; + } + + void SetHullFlag(HullFlag flag) + { + mFlags|=flag; + } + + void ClearHullFlag(HullFlag flag) + { + mFlags&=~flag; + } + + unsigned int mFlags; // flags to use when generating the convex hull. + unsigned int mVcount; // number of vertices in the input point cloud + const btVector3 *mVertices; // the array of vertices. + unsigned int mVertexStride; // the stride of each vertex, in bytes. + btScalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. + unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull! + unsigned int mMaxFaces; +}; + +enum HullError +{ + QE_OK, // success! + QE_FAIL // failed. +}; + +class btPlane +{ + public: + btVector3 normal; + btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 + btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){} + btPlane():normal(),dist(0){} + +}; + + + +class ConvexH +{ + public: + class HalfEdge + { + public: + short ea; // the other half of the edge (index into edges list) + unsigned char v; // the vertex at the start of this edge (index into vertices list) + unsigned char p; // the facet on which this edge lies (index into facets list) + HalfEdge(){} + HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){} + }; + ConvexH() + { + int i; + i=0; + } + ~ConvexH() + { + int i; + i=0; + } + btAlignedObjectArray<btVector3> vertices; + btAlignedObjectArray<HalfEdge> edges; + btAlignedObjectArray<btPlane> facets; + ConvexH(int vertices_size,int edges_size,int facets_size); +}; + + +class int4 +{ +public: + int x,y,z,w; + int4(){}; + int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;} + const int& operator[](int i) const {return (&x)[i];} + int& operator[](int i) {return (&x)[i];} +}; + +class PHullResult +{ +public: + + PHullResult(void) + { + mVcount = 0; + mIndexCount = 0; + mFaceCount = 0; + mVertices = 0; + } + + unsigned int mVcount; + unsigned int mIndexCount; + unsigned int mFaceCount; + btVector3* mVertices; + TUIntArray m_Indices; +}; + + + +///The HullLibrary class can create a convex hull from a collection of vertices, using the ComputeHull method. +///The btShapeHull class uses this HullLibrary to create a approximate convex mesh given a general (non-polyhedral) convex shape. +class HullLibrary +{ + + btAlignedObjectArray<class Tri*> m_tris; + +public: + + btAlignedObjectArray<int> m_vertexIndexMapping; + + + HullError CreateConvexHull(const HullDesc& desc, // describes the input request + HullResult& result); // contains the resulst + HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it. + +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); + + void removeb2b(Tri* s,Tri*t); + + void checkit(Tri *t); + + Tri* extrudable(btScalar epsilon); + + int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit); + + int calchullgen(btVector3 *verts,int verts_count, int vlimit); + + int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow); + + class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice); + + void extrude(class Tri* t0,int v); + + ConvexH* test_cube(); + + //BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'. + //After the hull is generated it give you back a set of polygon faces which index the *original* point cloud. + //The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull. + //The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation. + void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount); + + bool CleanupVertices(unsigned int svcount, + const btVector3* svertices, + unsigned int stride, + unsigned int &vcount, // output number of vertices + btVector3* vertices, // location to store the results. + btScalar normalepsilon, + btVector3& scale); +}; + + +#endif + diff --git a/extern/bullet2/src/LinearMath/btDefaultMotionState.h b/extern/bullet2/src/LinearMath/btDefaultMotionState.h index d4ec8e8879c..d758f77ed81 100644 --- a/extern/bullet2/src/LinearMath/btDefaultMotionState.h +++ b/extern/bullet2/src/LinearMath/btDefaultMotionState.h @@ -1,7 +1,7 @@ #ifndef DEFAULT_MOTION_STATE_H #define DEFAULT_MOTION_STATE_H -///btDefaultMotionState provides a common implementation to synchronize world transforms with offsets +///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets. struct btDefaultMotionState : public btMotionState { btTransform m_graphicsWorldTrans; diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp index 3d0fb122a6b..5ac230f712f 100644 --- a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp +++ b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp @@ -22,7 +22,12 @@ subject to the following restrictions: can be used by probes that are checking whether the library is actually installed. */ -extern "C" void btBulletMathProbe () {} +extern "C" +{ + void btBulletMathProbe (); + + void btBulletMathProbe () {} +} bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin) @@ -57,6 +62,8 @@ bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const return true; } +bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations); + bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations) { int numbrushes = planeEquations.size(); diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.h b/extern/bullet2/src/LinearMath/btGeometryUtil.h index 766cd75c383..a4b13b45609 100644 --- a/extern/bullet2/src/LinearMath/btGeometryUtil.h +++ b/extern/bullet2/src/LinearMath/btGeometryUtil.h @@ -19,6 +19,7 @@ subject to the following restrictions: #include "btVector3.h" #include "btAlignedObjectArray.h" +///The btGeometryUtil helper class provides a few methods to convert between plane equations and vertices. class btGeometryUtil { public: diff --git a/extern/bullet2/src/LinearMath/btHashMap.h b/extern/bullet2/src/LinearMath/btHashMap.h new file mode 100644 index 00000000000..f883e0e489a --- /dev/null +++ b/extern/bullet2/src/LinearMath/btHashMap.h @@ -0,0 +1,303 @@ +#ifndef BT_HASH_MAP_H +#define BT_HASH_MAP_H + +#include "btAlignedObjectArray.h" + +const int BT_HASH_NULL=0xffffffff; + +template <class Value> +class btHashKey +{ + int m_uid; +public: + + btHashKey(int uid) + :m_uid(uid) + { + } + + int getUid() const + { + return m_uid; + } + + //to our success + SIMD_FORCE_INLINE unsigned int getHash()const + { + int key = m_uid; + // Thomas Wang's hash + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return key; + } + + btHashKey getKey(const Value& value) const + { + return btHashKey(value.getUid()); + } +}; + + +template <class Value> +class btHashKeyPtr +{ + int m_uid; +public: + + btHashKeyPtr(int uid) + :m_uid(uid) + { + } + + int getUid() const + { + return m_uid; + } + + //to our success + SIMD_FORCE_INLINE unsigned int getHash()const + { + int key = m_uid; + // Thomas Wang's hash + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return key; + } + + btHashKeyPtr getKey(const Value& value) const + { + return btHashKeyPtr(value->getUid()); + } +}; + +///The btHashMap template class implements a generic and lightweight hashmap. +///A basic sample of how to use btHashMap is located in Demos\BasicDemo\main.cpp +template <class Key, class Value> +class btHashMap +{ + + btAlignedObjectArray<int> m_hashTable; + btAlignedObjectArray<int> m_next; + btAlignedObjectArray<Value> m_valueArray; + + + + void growTables(const Key& key) + { + int newCapacity = m_valueArray.capacity(); + + if (m_hashTable.size() < newCapacity) + { + //grow hashtable and next table + int curHashtableSize = m_hashTable.size(); + + m_hashTable.resize(newCapacity); + m_next.resize(newCapacity); + + int i; + + for (i= 0; i < newCapacity; ++i) + { + m_hashTable[i] = BT_HASH_NULL; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = BT_HASH_NULL; + } + + for(i=0;i<curHashtableSize;i++) + { + const Value& value = m_valueArray[i]; + + int hashValue = key.getKey(value).getHash() & (m_valueArray.capacity()-1); // New hash value with new mask + m_next[i] = m_hashTable[hashValue]; + m_hashTable[hashValue] = i; + } + + + } + } + + public: + + void insert(const Key& key, const Value& value) { + int hash = key.getHash() & (m_valueArray.capacity()-1); + //don't add it if it is already there + if (find(key)) + { + return; + } + int count = m_valueArray.size(); + int oldCapacity = m_valueArray.capacity(); + m_valueArray.push_back(value); + int newCapacity = m_valueArray.capacity(); + if (oldCapacity < newCapacity) + { + growTables(key); + //hash with new capacity + hash = key.getHash() & (m_valueArray.capacity()-1); + } + m_next[count] = m_hashTable[hash]; + m_hashTable[hash] = count; + } + + void remove(const Key& key) { + + int hash = key.getHash() & (m_valueArray.capacity()-1); + + int pairIndex = findIndex(key); + + if (pairIndex ==BT_HASH_NULL) + { + return; + } + + // Remove the pair from the hash table. + int index = m_hashTable[hash]; + btAssert(index != BT_HASH_NULL); + + int previous = BT_HASH_NULL; + while (index != pairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_HASH_NULL) + { + btAssert(m_next[previous] == pairIndex); + m_next[previous] = m_next[pairIndex]; + } + else + { + m_hashTable[hash] = m_next[pairIndex]; + } + + // We now move the last pair into spot of the + // pair being removed. We need to fix the hash + // table indices to support the move. + + int lastPairIndex = m_valueArray.size() - 1; + + // If the removed pair is the last pair, we are done. + if (lastPairIndex == pairIndex) + { + m_valueArray.pop_back(); + return; + } + + // Remove the last pair from the hash table. + const Value* lastValue = &m_valueArray[lastPairIndex]; + int lastHash = key.getKey(*lastValue).getHash() & (m_valueArray.capacity()-1); + + index = m_hashTable[lastHash]; + btAssert(index != BT_HASH_NULL); + + previous = BT_HASH_NULL; + while (index != lastPairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_HASH_NULL) + { + btAssert(m_next[previous] == lastPairIndex); + m_next[previous] = m_next[lastPairIndex]; + } + else + { + m_hashTable[lastHash] = m_next[lastPairIndex]; + } + + // Copy the last pair into the remove pair's spot. + m_valueArray[pairIndex] = m_valueArray[lastPairIndex]; + + // Insert the last pair into the hash table + m_next[pairIndex] = m_hashTable[lastHash]; + m_hashTable[lastHash] = pairIndex; + + m_valueArray.pop_back(); + + } + + + int size() const + { + return m_valueArray.size(); + } + + const Value* getAtIndex(int index) const + { + btAssert(index < m_valueArray.size()); + + return &m_valueArray[index]; + } + + Value* getAtIndex(int index) + { + btAssert(index < m_valueArray.size()); + + return &m_valueArray[index]; + } + + Value* operator[](const Key& key) { + return find(key); + } + + const Value* find(const Key& key) const + { + int index = findIndex(key); + if (index == BT_HASH_NULL) + { + return NULL; + } + return &m_valueArray[index]; + } + + Value* find(const Key& key) + { + int index = findIndex(key); + if (index == BT_HASH_NULL) + { + return NULL; + } + return &m_valueArray[index]; + } + + + int findIndex(const Key& key) const + { + int hash = key.getHash() & (m_valueArray.capacity()-1); + + if (hash >= m_hashTable.size()) + { + return BT_HASH_NULL; + } + + int index = m_hashTable[hash]; + while ((index != BT_HASH_NULL) && (key.getUid() == key.getKey(m_valueArray[index]).getUid()) == false) + { + index = m_next[index]; + } + return index; + } + + void clear() + { + m_hashTable.clear(); + m_next.clear(); + m_valueArray.clear(); + } + +}; + +#endif //BT_HASH_MAP_H diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet2/src/LinearMath/btIDebugDraw.h index 5f40ca39157..563615a9a32 100644 --- a/extern/bullet2/src/LinearMath/btIDebugDraw.h +++ b/extern/bullet2/src/LinearMath/btIDebugDraw.h @@ -31,6 +31,9 @@ DEALINGS IN THE SOFTWARE. #include "btVector3.h" +///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations. +///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld. +///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum. class btIDebugDraw { public: @@ -55,11 +58,24 @@ class btIDebugDraw virtual ~btIDebugDraw() {}; 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) + { + drawTriangle(v0,v1,v2,color,alpha); + } + virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& color, btScalar /*alpha*/) + { + drawLine(v0,v1,color); + drawLine(v1,v2,color); + drawLine(v2,v0,color); + } virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0; virtual void reportErrorWarning(const char* warningString) = 0; + virtual void draw3dText(const btVector3& location,const char* textString) = 0; + virtual void setDebugMode(int debugMode) =0; virtual int getDebugMode() const = 0; diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h index 94f53c3c0a5..14aa4ae2348 100644 --- a/extern/bullet2/src/LinearMath/btMatrix3x3.h +++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h @@ -22,6 +22,9 @@ subject to the following restrictions: #include "btQuaternion.h" + +///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: btMatrix3x3 () {} @@ -281,6 +284,91 @@ 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. + void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) + { + rot.setIdentity(); + for (int step = maxSteps; step > 0; step--) + { + // find off-diagonal element [p][q] with largest magnitude + int p = 0; + int q = 1; + int r = 2; + btScalar max = btFabs(m_el[0][1]); + btScalar v = btFabs(m_el[0][2]); + if (v > max) + { + q = 2; + r = 1; + max = v; + } + v = btFabs(m_el[1][2]); + if (v > max) + { + p = 1; + q = 2; + r = 0; + max = v; + } + + btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2])); + if (max <= t) + { + if (max <= SIMD_EPSILON * t) + { + return; + } + step = 1; + } + + // compute Jacobi rotation J which leads to a zero for element [p][q] + btScalar mpq = m_el[p][q]; + btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq); + btScalar theta2 = theta * theta; + btScalar cos; + btScalar sin; + if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON)) + { + t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2)) + : 1 / (theta - btSqrt(1 + theta2)); + cos = 1 / btSqrt(1 + t * t); + sin = cos * t; + } + else + { + // approximation for large theta-value, i.e., a nearly diagonal matrix + t = 1 / (theta * (2 + btScalar(0.5) / theta2)); + cos = 1 - btScalar(0.5) * t * t; + sin = cos * t; + } + + // apply rotation to matrix (this = J^T * this * J) + m_el[p][q] = m_el[q][p] = 0; + m_el[p][p] -= t * mpq; + m_el[q][q] += t * mpq; + btScalar mrp = m_el[r][p]; + btScalar mrq = m_el[r][q]; + m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq; + m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp; + + // apply rotation to rot (rot = rot * J) + for (int i = 0; i < 3; i++) + { + btVector3& row = rot[i]; + mrp = row[p]; + mrq = row[q]; + row[p] = cos * mrp - sin * mrq; + row[q] = cos * mrq + sin * mrp; + } + } + } + + protected: btScalar cofac(int r1, int c1, int r2, int c2) const @@ -356,7 +444,7 @@ class btMatrix3x3 { m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), - m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].x()); + m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); } SIMD_FORCE_INLINE btMatrix3x3 @@ -406,5 +494,11 @@ class btMatrix3x3 { } */ +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] && + m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && + m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); +} #endif diff --git a/extern/bullet2/src/LinearMath/btMinMax.h b/extern/bullet2/src/LinearMath/btMinMax.h index 1b8a3633f38..5e27d62a4a4 100644 --- a/extern/bullet2/src/LinearMath/btMinMax.h +++ b/extern/bullet2/src/LinearMath/btMinMax.h @@ -18,15 +18,15 @@ subject to the following restrictions: #define GEN_MINMAX_H template <class T> -SIMD_FORCE_INLINE const T& GEN_min(const T& a, const T& b) +SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) { - return b < a ? b : a; + return a < b ? a : b ; } template <class T> -SIMD_FORCE_INLINE const T& GEN_max(const T& a, const T& b) +SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b) { - return a < b ? b : a; + return a > b ? a : b; } template <class T> @@ -36,7 +36,7 @@ SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub) } template <class T> -SIMD_FORCE_INLINE void GEN_set_min(T& a, const T& b) +SIMD_FORCE_INLINE void btSetMin(T& a, const T& b) { if (b < a) { @@ -45,7 +45,7 @@ SIMD_FORCE_INLINE void GEN_set_min(T& a, const T& b) } template <class T> -SIMD_FORCE_INLINE void GEN_set_max(T& a, const T& b) +SIMD_FORCE_INLINE void btSetMax(T& a, const T& b) { if (a < b) { diff --git a/extern/bullet2/src/LinearMath/btMotionState.h b/extern/bullet2/src/LinearMath/btMotionState.h index 1975e5ff900..94318140902 100644 --- a/extern/bullet2/src/LinearMath/btMotionState.h +++ b/extern/bullet2/src/LinearMath/btMotionState.h @@ -18,7 +18,7 @@ subject to the following restrictions: #include "btTransform.h" -///btMotionState allows the dynamics world to synchronize the updated world transforms with graphics +///The btMotionState interface class allows the dynamics world to synchronize and interpolate the updated world transforms with graphics ///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation) class btMotionState { diff --git a/extern/bullet2/src/LinearMath/btPoolAllocator.h b/extern/bullet2/src/LinearMath/btPoolAllocator.h new file mode 100644 index 00000000000..39d2559c747 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btPoolAllocator.h @@ -0,0 +1,102 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef _BT_POOL_ALLOCATOR_H +#define _BT_POOL_ALLOCATOR_H + +#include "btScalar.h" +#include "btAlignedAllocator.h" + +///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately. +class btPoolAllocator +{ + int m_elemSize; + int m_maxElements; + int m_freeCount; + void* m_firstFree; + unsigned char* m_pool; + +public: + + btPoolAllocator(int elemSize, int maxElements) + :m_elemSize(elemSize), + m_maxElements(maxElements) + { + m_pool = (unsigned char*) btAlignedAlloc( static_cast<unsigned int>(m_elemSize*m_maxElements),16); + + unsigned char* p = m_pool; + m_firstFree = p; + m_freeCount = m_maxElements; + int count = m_maxElements; + while (--count) { + *(void**)p = (p + m_elemSize); + p += m_elemSize; + } + *(void**)p = 0; + } + + ~btPoolAllocator() + { + btAlignedFree( m_pool); + } + + int getFreeCount() const + { + return m_freeCount; + } + + void* allocate(int size) + { + // release mode fix + (void)size; + btAssert(!size || size<=m_elemSize); + btAssert(m_freeCount>0); + void* result = m_firstFree; + m_firstFree = *(void**)m_firstFree; + --m_freeCount; + return result; + } + + bool validPtr(void* ptr) + { + if (ptr) { + if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize)) + { + return true; + } + } + return false; + } + + void freeMemory(void* ptr) + { + if (ptr) { + btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize); + + *(void**)ptr = m_firstFree; + m_firstFree = ptr; + ++m_freeCount; + } + } + + int getElementSize() const + { + return m_elemSize; + } + + +}; + +#endif //_BT_POOL_ALLOCATOR_H diff --git a/extern/bullet2/src/LinearMath/btQuadWord.h b/extern/bullet2/src/LinearMath/btQuadWord.h index 961ac484d20..2e80fc2ca47 100644 --- a/extern/bullet2/src/LinearMath/btQuadWord.h +++ b/extern/bullet2/src/LinearMath/btQuadWord.h @@ -17,19 +17,31 @@ subject to the following restrictions: #define SIMD_QUADWORD_H #include "btScalar.h" +#include "btMinMax.h" +#include <math.h> +///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 +{ +protected: + + btScalar m_x; + btScalar m_y; + btScalar m_z; + btScalar m_unusedW; + +public: + +}; + ///btQuadWord is base-class for vectors, points -class btQuadWord +class btQuadWord : public btQuadWordStorage { - protected: - btScalar m_x; - btScalar m_y; - btScalar m_z; - btScalar m_unusedW; - public: // SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; } @@ -61,6 +73,8 @@ class btQuadWord SIMD_FORCE_INLINE operator btScalar *() { return &m_x; } SIMD_FORCE_INLINE operator const btScalar *() const { return &m_x; } + + SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z) { m_x=x; @@ -89,47 +103,36 @@ class btQuadWord { } - SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z) - :m_x(x),m_y(y),m_z(z) - //todo, remove this in release/simd ? - ,m_unusedW(btScalar(0.)) + SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q) { + *((btQuadWordStorage*)this) = q; + } + + 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; } 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_x = x, m_y = y, m_z = z, m_unusedW = w; } SIMD_FORCE_INLINE void setMax(const btQuadWord& other) { - if (other.m_x > m_x) - m_x = other.m_x; - - if (other.m_y > m_y) - m_y = other.m_y; - - if (other.m_z > m_z) - m_z = other.m_z; - - if (other.m_unusedW > m_unusedW) - m_unusedW = other.m_unusedW; + btSetMax(m_x, other.m_x); + btSetMax(m_y, other.m_y); + btSetMax(m_z, other.m_z); + btSetMax(m_unusedW, other.m_unusedW); } SIMD_FORCE_INLINE void setMin(const btQuadWord& other) { - if (other.m_x < m_x) - m_x = other.m_x; - - if (other.m_y < m_y) - m_y = other.m_y; - - if (other.m_z < m_z) - m_z = other.m_z; - - if (other.m_unusedW < m_unusedW) - m_unusedW = other.m_unusedW; + btSetMin(m_x, other.m_x); + btSetMin(m_y, other.m_y); + btSetMin(m_z, other.m_z); + btSetMin(m_unusedW, other.m_unusedW); } diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h index 50334970ba6..264751b33e7 100644 --- a/extern/bullet2/src/LinearMath/btQuaternion.h +++ b/extern/bullet2/src/LinearMath/btQuaternion.h @@ -19,6 +19,7 @@ subject to the following restrictions: #include "btVector3.h" +///The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. class btQuaternion : public btQuadWord { public: btQuaternion() {} @@ -158,7 +159,7 @@ public: btQuaternion inverse() const { - return btQuaternion(m_x, m_y, m_z, -m_unusedW); + return btQuaternion(-m_x, -m_y, -m_z, m_unusedW); } SIMD_FORCE_INLINE btQuaternion @@ -285,7 +286,7 @@ slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) } SIMD_FORCE_INLINE btVector3 -quatRotate(btQuaternion& rotation, btVector3& v) +quatRotate(const btQuaternion& rotation, const btVector3& v) { btQuaternion q = rotation * v; q *= rotation.inverse(); @@ -293,7 +294,7 @@ quatRotate(btQuaternion& rotation, btVector3& v) } SIMD_FORCE_INLINE btQuaternion -shortestArcQuat(btVector3& v0,btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized +shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized { btVector3 c = v0.cross(v1); btScalar d = v0.dot(v1); @@ -308,7 +309,7 @@ shortestArcQuat(btVector3& v0,btVector3& v1) // Game Programming Gems 2.10. make } SIMD_FORCE_INLINE btQuaternion -shortestArcQuatNormalize(btVector3& v0,btVector3& v1) +shortestArcQuatNormalize2(btVector3& v0,btVector3& v1) { v0.normalize(); v1.normalize(); @@ -319,3 +320,4 @@ shortestArcQuatNormalize(btVector3& v0,btVector3& v1) + diff --git a/extern/bullet2/src/LinearMath/btQuickprof.cpp b/extern/bullet2/src/LinearMath/btQuickprof.cpp index 37a0c8c3be5..e5b1196149b 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.cpp +++ b/extern/bullet2/src/LinearMath/btQuickprof.cpp @@ -1,38 +1,282 @@ /* -Copyright (c) 2006 Tyler Streeter -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: +/*************************************************************************************************** +** +** profile.cpp +** +** Real-Time Hierarchical Profiling for Game Programming Gems 3 +** +** by Greg Hjelstrom & Byon Garrabrant +** +***************************************************************************************************/ -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. +// Credits: The Clock class was inspired by the Timer classes in +// Ogre (www.ogre3d.org). -*/ +#include "LinearMath/btQuickprof.h" -// Please visit the project website (http://quickprof.sourceforge.net) -// for usage instructions. +#ifdef USE_BT_CLOCK -// Credits: The Clock class was inspired by the Timer classes in -// Ogre (www.ogre3d.org). +static btClock gProfileClock; -#include "LinearMath/btQuickprof.h" +inline void Profile_Get_Ticks(unsigned long int * ticks) +{ + *ticks = gProfileClock.getTimeMicroseconds(); +} + +inline float Profile_Get_Tick_Rate(void) +{ +// return 1000000.f; + return 1000.f; + +} + + + +/*************************************************************************************************** +** +** CProfileNode +** +***************************************************************************************************/ + +/*********************************************************************************************** + * INPUT: * + * name - pointer to a static string which is the name of this profile node * + * parent - parent pointer * + * * + * WARNINGS: * + * The name is assumed to be a static pointer, only the pointer is stored and compared for * + * efficiency reasons. * + *=============================================================================================*/ +CProfileNode::CProfileNode( const char * name, CProfileNode * parent ) : + Name( name ), + TotalCalls( 0 ), + TotalTime( 0 ), + StartTime( 0 ), + RecursionCounter( 0 ), + Parent( parent ), + Child( NULL ), + Sibling( NULL ) +{ + Reset(); +} + + +void CProfileNode::CleanupMemory() +{ + delete ( Child); + Child = NULL; + delete ( Sibling); + Sibling = NULL; +} + +CProfileNode::~CProfileNode( void ) +{ + delete ( Child); + delete ( Sibling); +} + + +/*********************************************************************************************** + * INPUT: * + * name - static string pointer to the name of the node we are searching for * + * * + * WARNINGS: * + * All profile names are assumed to be static strings so this function uses pointer compares * + * to find the named node. * + *=============================================================================================*/ +CProfileNode * CProfileNode::Get_Sub_Node( const char * name ) +{ + // Try to find this sub node + CProfileNode * child = Child; + while ( child ) { + if ( child->Name == name ) { + return child; + } + child = child->Sibling; + } + + // We didn't find it, so add it + + CProfileNode * node = new CProfileNode( name, this ); + node->Sibling = Child; + Child = node; + return node; +} + + +void CProfileNode::Reset( void ) +{ + TotalCalls = 0; + TotalTime = 0.0f; + gProfileClock.reset(); + + if ( Child ) { + Child->Reset(); + } + if ( Sibling ) { + Sibling->Reset(); + } +} + + +void CProfileNode::Call( void ) +{ + TotalCalls++; + if (RecursionCounter++ == 0) { + Profile_Get_Ticks(&StartTime); + } +} + + +bool CProfileNode::Return( void ) +{ + if ( --RecursionCounter == 0 && TotalCalls != 0 ) { + unsigned long int time; + Profile_Get_Ticks(&time); + time-=StartTime; + TotalTime += (float)time / Profile_Get_Tick_Rate(); + } + return ( RecursionCounter == 0 ); +} + + +/*************************************************************************************************** +** +** CProfileIterator +** +***************************************************************************************************/ +CProfileIterator::CProfileIterator( CProfileNode * start ) +{ + CurrentParent = start; + CurrentChild = CurrentParent->Get_Child(); +} + + +void CProfileIterator::First(void) +{ + CurrentChild = CurrentParent->Get_Child(); +} + + +void CProfileIterator::Next(void) +{ + CurrentChild = CurrentChild->Get_Sibling(); +} + + +bool CProfileIterator::Is_Done(void) +{ + return CurrentChild == NULL; +} + + +void CProfileIterator::Enter_Child( int index ) +{ + CurrentChild = CurrentParent->Get_Child(); + while ( (CurrentChild != NULL) && (index != 0) ) { + index--; + CurrentChild = CurrentChild->Get_Sibling(); + } + + if ( CurrentChild != NULL ) { + CurrentParent = CurrentChild; + CurrentChild = CurrentParent->Get_Child(); + } +} + + +void CProfileIterator::Enter_Parent( void ) +{ + if ( CurrentParent->Get_Parent() != NULL ) { + CurrentParent = CurrentParent->Get_Parent(); + } + CurrentChild = CurrentParent->Get_Child(); +} + + +/*************************************************************************************************** +** +** CProfileManager +** +***************************************************************************************************/ + +CProfileNode CProfileManager::Root( "Root", NULL ); +CProfileNode * CProfileManager::CurrentNode = &CProfileManager::Root; +int CProfileManager::FrameCounter = 0; +unsigned long int CProfileManager::ResetTime = 0; + + +/*********************************************************************************************** + * CProfileManager::Start_Profile -- Begin a named profile * + * * + * Steps one level deeper into the tree, if a child already exists with the specified name * + * then it accumulates the profiling; otherwise a new child node is added to the profile tree. * + * * + * INPUT: * + * name - name of this profiling record * + * * + * WARNINGS: * + * The string used is assumed to be a static string; pointer compares are used throughout * + * the profiling code for efficiency. * + *=============================================================================================*/ +void CProfileManager::Start_Profile( const char * name ) +{ + if (name != CurrentNode->Get_Name()) { + CurrentNode = CurrentNode->Get_Sub_Node( name ); + } + + CurrentNode->Call(); +} + + +/*********************************************************************************************** + * CProfileManager::Stop_Profile -- Stop timing and record the results. * + *=============================================================================================*/ +void CProfileManager::Stop_Profile( void ) +{ + // Return will indicate whether we should back up to our parent (we may + // be profiling a recursive function) + if (CurrentNode->Return()) { + CurrentNode = CurrentNode->Get_Parent(); + } +} + + +/*********************************************************************************************** + * CProfileManager::Reset -- Reset the contents of the profiling system * + * * + * This resets everything except for the tree structure. All of the timing data is reset. * + *=============================================================================================*/ +void CProfileManager::Reset( void ) +{ + Root.Reset(); + Root.Call(); + FrameCounter = 0; + Profile_Get_Ticks(&ResetTime); +} + + +/*********************************************************************************************** + * CProfileManager::Increment_Frame_Counter -- Increment the frame counter * + *=============================================================================================*/ +void CProfileManager::Increment_Frame_Counter( void ) +{ + FrameCounter++; +} + + +/*********************************************************************************************** + * CProfileManager::Get_Time_Since_Reset -- returns the elapsed time since last reset * + *=============================================================================================*/ +float CProfileManager::Get_Time_Since_Reset( void ) +{ + unsigned long int time; + Profile_Get_Ticks(&time); + time -= ResetTime; + return (float)time / Profile_Get_Tick_Rate(); +} + +#endif //USE_BT_CLOCK -#ifdef USE_QUICKPROF - -// Note: We must declare these private static variables again here to -// avoid link errors. -bool btProfiler::mEnabled = false; -btClock btProfiler::mClock; -unsigned long int btProfiler::mCurrentCycleStartMicroseconds = 0; -unsigned long int btProfiler::mLastCycleDurationMicroseconds = 0; -std::map<std::string, hidden::ProfileBlock*> btProfiler::mProfileBlocks; -std::ofstream btProfiler::mOutputFile; -bool btProfiler::mFirstFileOutput = true; -btProfiler::BlockTimingMethod btProfiler::mFileOutputMethod; -unsigned long int btProfiler::mCycleNumber = 0; -#endif //USE_QUICKPROF diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet2/src/LinearMath/btQuickprof.h index a885967c5fa..b033940ca5c 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.h +++ b/extern/bullet2/src/LinearMath/btQuickprof.h @@ -1,20 +1,11 @@ -/* -Copyright (c) 2006 Tyler Streeter -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -*/ - -// Please visit the project website (http://quickprof.sourceforge.net) -// for usage instructions. +/*************************************************************************************************** +** +** Real-Time Hierarchical Profiling for Game Programming Gems 3 +** +** by Greg Hjelstrom & Byon Garrabrant +** +***************************************************************************************************/ // Credits: The Clock class was inspired by the Timer classes in // Ogre (www.ogre3d.org). @@ -23,9 +14,10 @@ subject to the following restrictions: #define QUICK_PROF_H #include "btScalar.h" - -//#define USE_QUICKPROF 1 -//Don't use quickprof for now, because it contains STL. TODO: replace STL by Bullet container classes. +#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 @@ -34,678 +26,332 @@ subject to the following restrictions: #ifdef USE_BT_CLOCK #ifdef __CELLOS_LV2__ #include <sys/sys_time.h> +#include <sys/time_util.h> #include <stdio.h> -typedef uint64_t __int64; #endif #if defined (SUNOS) || defined (__SUNOS__) - #include <stdio.h> +#include <stdio.h> #endif #if defined(WIN32) || defined(_WIN32) - #define USE_WINDOWS_TIMERS - #define WIN32_LEAN_AND_MEAN - #define NOWINRES - #define NOMCX - #define NOIME +#define USE_WINDOWS_TIMERS +#define WIN32_LEAN_AND_MEAN +#define NOWINRES +#define NOMCX +#define NOIME #ifdef _XBOX - #include <Xtl.h> +#include <Xtl.h> #else - #include <windows.h> +#include <windows.h> #endif - #include <time.h> +#include <time.h> #else - #include <sys/time.h> +#include <sys/time.h> #endif #define mymin(a,b) (a > b ? a : b) -/// basic clock +///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling. class btClock +{ +public: + btClock() { - public: - btClock() - { #ifdef USE_WINDOWS_TIMERS - QueryPerformanceFrequency(&mClockFrequency); + QueryPerformanceFrequency(&mClockFrequency); #endif - reset(); - } + reset(); + } - ~btClock() - { - } + ~btClock() + { + } - /// Resets the initial reference time. - void reset() - { + /// Resets the initial reference time. + void reset() + { #ifdef USE_WINDOWS_TIMERS - QueryPerformanceCounter(&mStartTime); - mStartTick = GetTickCount(); - mPrevElapsedTime = 0; + QueryPerformanceCounter(&mStartTime); + mStartTick = GetTickCount(); + mPrevElapsedTime = 0; #else #ifdef __CELLOS_LV2__ - typedef uint64_t __int64; - typedef __int64 ClockSize; - ClockSize newTime; - __asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); - mStartTime = newTime; + typedef uint64_t ClockSize; + ClockSize newTime; + //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); + SYS_TIMEBASE_GET( newTime ); + mStartTime = newTime; #else - gettimeofday(&mStartTime, 0); + gettimeofday(&mStartTime, 0); #endif #endif - } + } - /// Returns the time in ms since the last call to reset or since - /// the btClock was created. - unsigned long int getTimeMilliseconds() - { + /// Returns the time in ms since the last call to reset or since + /// the btClock was created. + unsigned long int getTimeMilliseconds() + { #ifdef USE_WINDOWS_TIMERS - LARGE_INTEGER currentTime; - QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - - mStartTime.QuadPart; - - // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / + LARGE_INTEGER currentTime; + QueryPerformanceCounter(¤tTime); + LONGLONG elapsedTime = currentTime.QuadPart - + mStartTime.QuadPart; + + // Compute the number of millisecond ticks elapsed. + unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / + mClockFrequency.QuadPart); + + // Check for unexpected leaps in the Win32 performance counter. + // (This is caused by unexpected data across the PCI to ISA + // bridge, aka south bridge. See Microsoft KB274323.) + unsigned long elapsedTicks = GetTickCount() - mStartTick; + signed long msecOff = (signed long)(msecTicks - elapsedTicks); + if (msecOff < -100 || msecOff > 100) + { + // Adjust the starting time forwards. + LONGLONG msecAdjustment = mymin(msecOff * + mClockFrequency.QuadPart / 1000, elapsedTime - + mPrevElapsedTime); + mStartTime.QuadPart += msecAdjustment; + elapsedTime -= msecAdjustment; + + // Recompute the number of millisecond ticks elapsed. + msecTicks = (unsigned long)(1000 * elapsedTime / mClockFrequency.QuadPart); + } - // Check for unexpected leaps in the Win32 performance counter. - // (This is caused by unexpected data across the PCI to ISA - // bridge, aka south bridge. See Microsoft KB274323.) - unsigned long elapsedTicks = GetTickCount() - mStartTick; - signed long msecOff = (signed long)(msecTicks - elapsedTicks); - if (msecOff < -100 || msecOff > 100) - { - // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - mClockFrequency.QuadPart / 1000, elapsedTime - - mPrevElapsedTime); - mStartTime.QuadPart += msecAdjustment; - elapsedTime -= msecAdjustment; - - // Recompute the number of millisecond ticks elapsed. - msecTicks = (unsigned long)(1000 * elapsedTime / - mClockFrequency.QuadPart); - } - - // Store the current elapsed time for adjustments next time. - mPrevElapsedTime = elapsedTime; - - return msecTicks; + // Store the current elapsed time for adjustments next time. + mPrevElapsedTime = elapsedTime; + + return msecTicks; #else - + #ifdef __CELLOS_LV2__ - __int64 freq=sys_time_get_timebase_frequency(); - double dFreq=((double) freq) / 1000.0; - typedef uint64_t __int64; - typedef __int64 ClockSize; - ClockSize newTime; - __asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); - - return (newTime-mStartTime) / dFreq; + uint64_t freq=sys_time_get_timebase_frequency(); + double dFreq=((double) freq) / 1000.0; + typedef uint64_t ClockSize; + ClockSize newTime; + SYS_TIMEBASE_GET( newTime ); + //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); + + return (unsigned long int)((double(newTime-mStartTime)) / dFreq); #else - struct timeval currentTime; - gettimeofday(¤tTime, 0); - return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 + - (currentTime.tv_usec - mStartTime.tv_usec) / 1000; + struct timeval currentTime; + gettimeofday(¤tTime, 0); + return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 + + (currentTime.tv_usec - mStartTime.tv_usec) / 1000; #endif //__CELLOS_LV2__ #endif - } + } - /// Returns the time in us since the last call to reset or since - /// the Clock was created. - unsigned long int getTimeMicroseconds() - { + /// Returns the time in us since the last call to reset or since + /// the Clock was created. + unsigned long int getTimeMicroseconds() + { #ifdef USE_WINDOWS_TIMERS - LARGE_INTEGER currentTime; - QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - - mStartTime.QuadPart; + LARGE_INTEGER currentTime; + QueryPerformanceCounter(¤tTime); + LONGLONG elapsedTime = currentTime.QuadPart - + mStartTime.QuadPart; + + // Compute the number of millisecond ticks elapsed. + unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / + mClockFrequency.QuadPart); + + // Check for unexpected leaps in the Win32 performance counter. + // (This is caused by unexpected data across the PCI to ISA + // bridge, aka south bridge. See Microsoft KB274323.) + unsigned long elapsedTicks = GetTickCount() - mStartTick; + signed long msecOff = (signed long)(msecTicks - elapsedTicks); + if (msecOff < -100 || msecOff > 100) + { + // Adjust the starting time forwards. + LONGLONG msecAdjustment = mymin(msecOff * + mClockFrequency.QuadPart / 1000, elapsedTime - + mPrevElapsedTime); + mStartTime.QuadPart += msecAdjustment; + elapsedTime -= msecAdjustment; + } - // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / - mClockFrequency.QuadPart); + // Store the current elapsed time for adjustments next time. + mPrevElapsedTime = elapsedTime; - // Check for unexpected leaps in the Win32 performance counter. - // (This is caused by unexpected data across the PCI to ISA - // bridge, aka south bridge. See Microsoft KB274323.) - unsigned long elapsedTicks = GetTickCount() - mStartTick; - signed long msecOff = (signed long)(msecTicks - elapsedTicks); - if (msecOff < -100 || msecOff > 100) - { - // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - mClockFrequency.QuadPart / 1000, elapsedTime - - mPrevElapsedTime); - mStartTime.QuadPart += msecAdjustment; - elapsedTime -= msecAdjustment; - } - - // Store the current elapsed time for adjustments next time. - mPrevElapsedTime = elapsedTime; - - // Convert to microseconds. - unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / - mClockFrequency.QuadPart); + // Convert to microseconds. + unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / + mClockFrequency.QuadPart); - return usecTicks; + return usecTicks; #else #ifdef __CELLOS_LV2__ - __int64 freq=sys_time_get_timebase_frequency(); - double dFreq=((double) freq)/ 1000000.0; - typedef uint64_t __int64; - typedef __int64 ClockSize; - ClockSize newTime; - __asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); - - return (newTime-mStartTime) / dFreq; + uint64_t freq=sys_time_get_timebase_frequency(); + double dFreq=((double) freq)/ 1000000.0; + typedef uint64_t ClockSize; + ClockSize newTime; + //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); + SYS_TIMEBASE_GET( newTime ); + + return (unsigned long int)((double(newTime-mStartTime)) / dFreq); #else - struct timeval currentTime; - gettimeofday(¤tTime, 0); - return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 + - (currentTime.tv_usec - mStartTime.tv_usec); + struct timeval currentTime; + gettimeofday(¤tTime, 0); + return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 + + (currentTime.tv_usec - mStartTime.tv_usec); #endif//__CELLOS_LV2__ #endif - } + } - private: +private: #ifdef USE_WINDOWS_TIMERS - LARGE_INTEGER mClockFrequency; - DWORD mStartTick; - LONGLONG mPrevElapsedTime; - LARGE_INTEGER mStartTime; + LARGE_INTEGER mClockFrequency; + DWORD mStartTick; + LONGLONG mPrevElapsedTime; + LARGE_INTEGER mStartTime; #else #ifdef __CELLOS_LV2__ - uint64_t mStartTime; + uint64_t mStartTime; #else - struct timeval mStartTime; + struct timeval mStartTime; #endif #endif //__CELLOS_LV2__ - }; +}; #endif //USE_BT_CLOCK -#ifdef USE_QUICKPROF - -#include <iostream> -#include <fstream> -#include <string> -#include <map> +///A node in the Profile Hierarchy Tree +class CProfileNode { +public: + CProfileNode( const char * name, CProfileNode * parent ); + ~CProfileNode( void ); + CProfileNode * Get_Sub_Node( const char * name ); -namespace hidden -{ - /// A simple data structure representing a single timed block - /// of code. - struct ProfileBlock - { - ProfileBlock() - { - currentBlockStartMicroseconds = 0; - currentCycleTotalMicroseconds = 0; - lastCycleTotalMicroseconds = 0; - totalMicroseconds = 0; - } + CProfileNode * Get_Parent( void ) { return Parent; } + CProfileNode * Get_Sibling( void ) { return Sibling; } + CProfileNode * Get_Child( void ) { return Child; } - /// The starting time (in us) of the current block update. - unsigned long int currentBlockStartMicroseconds; + void CleanupMemory(); + void Reset( void ); + void Call( void ); + bool Return( void ); - /// The accumulated time (in us) spent in this block during the - /// current profiling cycle. - unsigned long int currentCycleTotalMicroseconds; + const char * Get_Name( void ) { return Name; } + int Get_Total_Calls( void ) { return TotalCalls; } + float Get_Total_Time( void ) { return TotalTime; } - /// The accumulated time (in us) spent in this block during the - /// past profiling cycle. - unsigned long int lastCycleTotalMicroseconds; +protected: - /// The total accumulated time (in us) spent in this block. - unsigned long int totalMicroseconds; - }; + const char * Name; + int TotalCalls; + float TotalTime; + unsigned long int StartTime; + int RecursionCounter; + CProfileNode * Parent; + CProfileNode * Child; + CProfileNode * Sibling; }; -/// A static class that manages timing for a set of profiling blocks. -class btProfiler +///An iterator to navigate through the tree +class CProfileIterator { public: - /// A set of ways to retrieve block timing data. - enum BlockTimingMethod - { - /// The total time spent in the block (in seconds) since the - /// profiler was initialized. - BLOCK_TOTAL_SECONDS, - - /// The total time spent in the block (in ms) since the - /// profiler was initialized. - BLOCK_TOTAL_MILLISECONDS, - - /// The total time spent in the block (in us) since the - /// profiler was initialized. - BLOCK_TOTAL_MICROSECONDS, - - /// The total time spent in the block, as a % of the total - /// elapsed time since the profiler was initialized. - BLOCK_TOTAL_PERCENT, - - /// The time spent in the block (in seconds) in the most recent - /// profiling cycle. - BLOCK_CYCLE_SECONDS, - - /// The time spent in the block (in ms) in the most recent - /// profiling cycle. - BLOCK_CYCLE_MILLISECONDS, - - /// The time spent in the block (in us) in the most recent - /// profiling cycle. - BLOCK_CYCLE_MICROSECONDS, - - /// The time spent in the block (in seconds) in the most recent - /// profiling cycle, as a % of the total cycle time. - BLOCK_CYCLE_PERCENT - }; - - /// Initializes the profiler. This must be called first. If this is - /// never called, the profiler is effectively disabled; all other - /// functions will return immediately. The first parameter - /// is the name of an output data file; if this string is not empty, - /// data will be saved on every profiling cycle; if this string is - /// empty, no data will be saved to a file. The second parameter - /// determines which timing method is used when printing data to the - /// output file. - inline static void init(const std::string outputFilename="", - BlockTimingMethod outputMethod=BLOCK_CYCLE_MILLISECONDS); - - /// Cleans up allocated memory. - inline static void destroy(); - - /// Begins timing the named block of code. - inline static void beginBlock(const std::string& name); - - /// Updates the accumulated time spent in the named block by adding - /// the elapsed time since the last call to startBlock for this block - /// name. - inline static void endBlock(const std::string& name); - - /// Returns the time spent in the named block according to the - /// given timing method. See comments on BlockTimingMethod for details. - inline static double getBlockTime(const std::string& name, - BlockTimingMethod method=BLOCK_CYCLE_MILLISECONDS); - - /// Defines the end of a profiling cycle. Use this regularly if you - /// want to generate detailed timing information. This must not be - /// called within a timing block. - inline static void endProfilingCycle(); - - /// A helper function that creates a string of statistics for - /// each timing block. This is mainly for printing an overall - /// summary to the command line. - inline static std::string createStatsString( - BlockTimingMethod method=BLOCK_TOTAL_PERCENT); - -//private: - inline btProfiler(); - - inline ~btProfiler(); - - /// Prints an error message to standard output. - inline static void printError(const std::string& msg) - { - //btAssert(0); - std::cout << "[QuickProf error] " << msg << std::endl; - } - - /// Determines whether the profiler is enabled. - static bool mEnabled; + // Access all the children of the current parent + void First(void); + void Next(void); + bool Is_Done(void); + bool Is_Root(void) { return (CurrentParent->Get_Parent() == 0); } - /// The clock used to time profile blocks. - static btClock mClock; + void Enter_Child( int index ); // Make the given child the new parent + void Enter_Largest_Child( void ); // Make the largest child the new parent + void Enter_Parent( void ); // Make the current parent's parent the new parent - /// The starting time (in us) of the current profiling cycle. - static unsigned long int mCurrentCycleStartMicroseconds; + // Access the current child + const char * Get_Current_Name( void ) { return CurrentChild->Get_Name(); } + int Get_Current_Total_Calls( void ) { return CurrentChild->Get_Total_Calls(); } + float Get_Current_Total_Time( void ) { return CurrentChild->Get_Total_Time(); } - /// The duration (in us) of the most recent profiling cycle. - static unsigned long int mLastCycleDurationMicroseconds; + // Access the current parent + const char * Get_Current_Parent_Name( void ) { return CurrentParent->Get_Name(); } + int Get_Current_Parent_Total_Calls( void ) { return CurrentParent->Get_Total_Calls(); } + float Get_Current_Parent_Total_Time( void ) { return CurrentParent->Get_Total_Time(); } - /// Internal map of named profile blocks. - static std::map<std::string, hidden::ProfileBlock*> mProfileBlocks; +protected: - /// The data file used if this feature is enabled in 'init.' - static std::ofstream mOutputFile; + CProfileNode * CurrentParent; + CProfileNode * CurrentChild; - /// Tracks whether we have begun print data to the output file. - static bool mFirstFileOutput; - - /// The method used when printing timing data to an output file. - static BlockTimingMethod mFileOutputMethod; - - /// The number of the current profiling cycle. - static unsigned long int mCycleNumber; + CProfileIterator( CProfileNode * start ); + friend class CProfileManager; }; -btProfiler::btProfiler() -{ - // This never gets called because a btProfiler instance is never - // created. -} - -btProfiler::~btProfiler() -{ - // This never gets called because a btProfiler instance is never - // created. -} - -void btProfiler::init(const std::string outputFilename, - BlockTimingMethod outputMethod) -{ - mEnabled = true; - - if (!outputFilename.empty()) - { - mOutputFile.open(outputFilename.c_str()); - } - - mFileOutputMethod = outputMethod; - - mClock.reset(); - - // Set the start time for the first cycle. - mCurrentCycleStartMicroseconds = mClock.getTimeMicroseconds(); -} - -void btProfiler::destroy() -{ - if (!mEnabled) - { - return; - } - - if (mOutputFile.is_open()) - { - mOutputFile.close(); - } - - // Destroy all ProfileBlocks. - while (!mProfileBlocks.empty()) - { - delete (*mProfileBlocks.begin()).second; - mProfileBlocks.erase(mProfileBlocks.begin()); - } -} - -void btProfiler::beginBlock(const std::string& name) -{ - if (!mEnabled) - { - return; - } - - if (name.empty()) - { - printError("Cannot allow unnamed profile blocks."); - return; - } - - hidden::ProfileBlock* block = mProfileBlocks[name]; - - if (!block) - { - // Create a new ProfileBlock. - mProfileBlocks[name] = new hidden::ProfileBlock(); - block = mProfileBlocks[name]; - } - - // We do this at the end to get more accurate results. - block->currentBlockStartMicroseconds = mClock.getTimeMicroseconds(); -} - -void btProfiler::endBlock(const std::string& name) -{ - if (!mEnabled) - { - return; - } - - // We do this at the beginning to get more accurate results. - unsigned long int endTick = mClock.getTimeMicroseconds(); - - hidden::ProfileBlock* block = mProfileBlocks[name]; - - if (!block) - { - // The named block does not exist. Print an error. - printError("The profile block named '" + name + - "' does not exist."); - return; - } - - unsigned long int blockDuration = endTick - - block->currentBlockStartMicroseconds; - block->currentCycleTotalMicroseconds += blockDuration; - block->totalMicroseconds += blockDuration; -} - -double btProfiler::getBlockTime(const std::string& name, - BlockTimingMethod method) -{ - if (!mEnabled) - { - return 0; - } - - hidden::ProfileBlock* block = mProfileBlocks[name]; - - if (!block) - { - // The named block does not exist. Print an error. - printError("The profile block named '" + name + - "' does not exist."); - return 0; - } - - double result = 0; - - switch(method) - { - case BLOCK_TOTAL_SECONDS: - result = (double)block->totalMicroseconds * (double)0.000001; - break; - case BLOCK_TOTAL_MILLISECONDS: - result = (double)block->totalMicroseconds * (double)0.001; - break; - case BLOCK_TOTAL_MICROSECONDS: - result = (double)block->totalMicroseconds; - break; - case BLOCK_TOTAL_PERCENT: - { - double timeSinceInit = (double)mClock.getTimeMicroseconds(); - if (timeSinceInit <= 0) - { - result = 0; - } - else - { - result = 100.0 * (double)block->totalMicroseconds / - timeSinceInit; - } - break; - } - case BLOCK_CYCLE_SECONDS: - result = (double)block->lastCycleTotalMicroseconds * - (double)0.000001; - break; - case BLOCK_CYCLE_MILLISECONDS: - result = (double)block->lastCycleTotalMicroseconds * - (double)0.001; - break; - case BLOCK_CYCLE_MICROSECONDS: - result = (double)block->lastCycleTotalMicroseconds; - break; - case BLOCK_CYCLE_PERCENT: - { - if (0 == mLastCycleDurationMicroseconds) - { - // We have not yet finished a cycle, so just return zero - // percent to avoid a divide by zero error. - result = 0; - } - else - { - result = 100.0 * (double)block->lastCycleTotalMicroseconds / - mLastCycleDurationMicroseconds; - } - break; - } - default: - break; - } - - return result; -} +///The Manager for the Profile system +class CProfileManager { +public: + static void Start_Profile( const char * name ); + static void Stop_Profile( void ); -void btProfiler::endProfilingCycle() -{ - if (!mEnabled) + static void CleanupMemory(void) { - return; + Root.CleanupMemory(); } - // Store the duration of the cycle that just finished. - mLastCycleDurationMicroseconds = mClock.getTimeMicroseconds() - - mCurrentCycleStartMicroseconds; + static void Reset( void ); + static void Increment_Frame_Counter( void ); + static int Get_Frame_Count_Since_Reset( void ) { return FrameCounter; } + static float Get_Time_Since_Reset( void ); - // For each block, update data for the cycle that just finished. - std::map<std::string, hidden::ProfileBlock*>::iterator iter; - for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); ++iter) - { - hidden::ProfileBlock* block = (*iter).second; - block->lastCycleTotalMicroseconds = - block->currentCycleTotalMicroseconds; - block->currentCycleTotalMicroseconds = 0; + static CProfileIterator * Get_Iterator( void ) + { + + return new CProfileIterator( &Root ); } + static void Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); } - if (mOutputFile.is_open()) - { - // Print data to the output file. - if (mFirstFileOutput) - { - // On the first iteration, print a header line that shows the - // names of each profiling block. - mOutputFile << "#cycle, "; - - for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); - ++iter) - { - mOutputFile << (*iter).first << ", "; - } - - mOutputFile << std::endl; - mFirstFileOutput = false; - } - - mOutputFile << mCycleNumber << ", "; - - for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); - ++iter) - { - mOutputFile << getBlockTime((*iter).first, mFileOutputMethod) - << ", "; - } - - mOutputFile << std::endl; - } - - ++mCycleNumber; - mCurrentCycleStartMicroseconds = mClock.getTimeMicroseconds(); -} - -std::string btProfiler::createStatsString(BlockTimingMethod method) -{ - if (!mEnabled) - { - return ""; - } +private: + static CProfileNode Root; + static CProfileNode * CurrentNode; + static int FrameCounter; + static unsigned long int ResetTime; +}; - std::string s; - std::string suffix; - switch(method) - { - case BLOCK_TOTAL_SECONDS: - suffix = "s"; - break; - case BLOCK_TOTAL_MILLISECONDS: - suffix = "ms"; - break; - case BLOCK_TOTAL_MICROSECONDS: - suffix = "us"; - break; - case BLOCK_TOTAL_PERCENT: - { - suffix = "%"; - break; - } - case BLOCK_CYCLE_SECONDS: - suffix = "s"; - break; - case BLOCK_CYCLE_MILLISECONDS: - suffix = "ms"; - break; - case BLOCK_CYCLE_MICROSECONDS: - suffix = "us"; - break; - case BLOCK_CYCLE_PERCENT: - { - suffix = "%"; - break; - } - default: - break; +///ProfileSampleClass is a simple way to profile a function's scope +///Use the BT_PROFILE macro at the start of scope to time +class CProfileSample { +public: + CProfileSample( const char * name ) + { + CProfileManager::Start_Profile( name ); } - std::map<std::string, hidden::ProfileBlock*>::iterator iter; - for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); ++iter) - { - if (iter != mProfileBlocks.begin()) - { - s += "\n"; - } - - char blockTime[64]; - sprintf(blockTime, "%lf", getBlockTime((*iter).first, method)); - - s += (*iter).first; - s += ": "; - s += blockTime; - s += " "; - s += suffix; + ~CProfileSample( void ) + { + CProfileManager::Stop_Profile(); } +}; - return s; -} - +#if !defined(BT_NO_PROFILE) +#define BT_PROFILE( name ) CProfileSample __profile( name ) +#else +#define BT_PROFILE( name ) +#endif -#define BEGIN_PROFILE(a) btProfiler::beginBlock(a) -#define END_PROFILE(a) btProfiler::endBlock(a) -#else //USE_QUICKPROF -#define BEGIN_PROFILE(a) -#define END_PROFILE(a) -#endif //USE_QUICKPROF #endif //QUICK_PROF_H diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index 01ad93e786a..e8433405b20 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -18,69 +18,140 @@ 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 + +inline int btGetVersion() +{ + return BT_BULLET_VERSION; +} + +#if defined(DEBUG) || defined (_DEBUG) +#define BT_DEBUG +#endif + + #ifdef WIN32 #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) + #define SIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED128(a) a #else - #define BT_HAS_ALIGNED_ALOCATOR - #pragma warning(disable:4530) - #pragma warning(disable:4996) - #pragma warning(disable:4786) + #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 +// #pragma warning(disable:4786) // Disable the "debug name too long" warning + #define SIMD_FORCE_INLINE __forceinline #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a + #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a #ifdef _XBOX #define BT_USE_VMX128 + + #include <ppcintrinsics.h> + #define BT_HAVE_NATIVE_FSEL + #define btFsel(a,b,c) __fsel((a),(b),(c)) #else #define BT_USE_SSE #endif #endif //__MINGW32__ #include <assert.h> +#if defined(DEBUG) || defined (_DEBUG) #define btAssert assert +#else + #define btAssert(x) +#endif //btFullAssert is optional, slows down a lot #define btFullAssert(x) + + #define btLikely(_c) _c + #define btUnlikely(_c) _c + #else #if defined (__CELLOS_LV2__) #define SIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #ifndef assert #include <assert.h> #endif #define btAssert assert //btFullAssert is optional, slows down a lot #define btFullAssert(x) + + #define btLikely(_c) _c + #define btUnlikely(_c) _c + #else +#ifdef USE_LIBSPE2 + + #define SIMD_FORCE_INLINE __inline + #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #ifndef assert + #include <assert.h> + #endif + #define btAssert assert + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) + + + #define btLikely(_c) __builtin_expect((_c), 1) + #define btUnlikely(_c) __builtin_expect((_c), 0) + + +#else //non-windows systems #define SIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED128(a) a #ifndef assert #include <assert.h> #endif + +#if defined(DEBUG) || defined (_DEBUG) #define btAssert assert +#else + #define btAssert(x) +#endif + //btFullAssert is optional, slows down a lot #define btFullAssert(x) + #define btLikely(_c) _c + #define btUnlikely(_c) _c + + +#endif // LIBSPE2 + #endif //__CELLOS_LV2__ #endif /// 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 #endif #endif +///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision. #if defined(BT_USE_DOUBLE_PRECISION) typedef double btScalar; #else @@ -88,6 +159,19 @@ typedef float btScalar; #endif + +#define BT_DECLARE_ALIGNED_ALLOCATOR() \ + SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ + SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); } \ + SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ + SIMD_FORCE_INLINE void operator delete(void*, void*) { } \ + SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ + SIMD_FORCE_INLINE void operator delete[](void* ptr) { btAlignedFree(ptr); } \ + SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \ + SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \ + + + #if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS) SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); } @@ -105,12 +189,34 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); } #else -SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrtf(x); } +SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) +{ +#ifdef USE_APPROXIMATION + double x, z, tempf; + unsigned long *tfptr = ((unsigned long *)&tempf) + 1; + + tempf = y; + *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ + x = tempf; + 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); + x = (btScalar(1.5)*x)-(x*x)*(x*z); + x = (btScalar(1.5)*x)-(x*x)*(x*z); + return x*y; +#else + return sqrtf(y); +#endif +} SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); } SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); } SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); } -SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acosf(x); } +SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { + btAssert(x <= btScalar(1.)); + return acosf(x); +} SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); } SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } @@ -159,14 +265,6 @@ SIMD_FORCE_INLINE bool btGreaterEqual (btScalar a, btScalar eps) { return (!((a) <= eps)); } -/*SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } -SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); } -SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); } -SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acosf(x); } -SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); } -SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } -SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } -*/ SIMD_FORCE_INLINE int btIsNegative(btScalar x) { return x < btScalar(0.0) ? 1 : 0; @@ -177,5 +275,151 @@ SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; #define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name +#ifndef btFsel +SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c) +{ + return a >= 0 ? b : c; +} +#endif +#define btFsels(a,b,c) (btScalar)btFsel(a,b,c) + + +SIMD_FORCE_INLINE bool btMachineIsLittleEndian() +{ + long int i = 1; + const char *p = (const char *) &i; + if (p[0] == 1) // Lowest address contains the least significant byte + return true; + else + return false; +} + + + +///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 +///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html +SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) +{ + // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero + // Rely on positive value or'ed with its negative having sign bit on + // and zero value or'ed with its negative (which is still zero) having sign bit off + // Use arithmetic shift right, shifting the sign bit through all 32 bits + unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); + unsigned testEqz = ~testNz; + return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); +} +SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) +{ + unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); + unsigned testEqz = ~testNz; + return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); +} +SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) +{ +#ifdef BT_HAVE_NATIVE_FSEL + return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); +#else + return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; +#endif +} + +template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b) +{ + T tmp = a; + a = b; + b = tmp; +} + + +//PCK: endian swapping functions +SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val) +{ + return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); +} + +SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val) +{ + return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8)); +} + +SIMD_FORCE_INLINE unsigned btSwapEndian(int val) +{ + return btSwapEndian((unsigned)val); +} + +SIMD_FORCE_INLINE unsigned short btSwapEndian(short val) +{ + return btSwapEndian((unsigned short) val); +} + +///btSwapFloat uses using char pointers to swap the endianness +////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values +///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. +///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. +///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. +///so instead of returning a float/double, we return integer/long long integer +SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d) +{ + unsigned int a = 0; + unsigned char *dst = (unsigned char *)&a; + unsigned char *src = (unsigned char *)&d; + + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; + return a; +} + +// unswap using char pointers +SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a) +{ + float d = 0.0f; + unsigned char *src = (unsigned char *)&a; + unsigned char *dst = (unsigned char *)&d; + + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; + + return d; +} + + +// swap using char pointers +SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char* dst) +{ + unsigned char *src = (unsigned char *)&d; + + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; + +} + +// unswap using char pointers +SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) +{ + double d = 0.0; + unsigned char *dst = (unsigned char *)&d; + + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; + + return d; +} + #endif //SIMD___SCALAR_H diff --git a/extern/bullet2/src/LinearMath/btStackAlloc.h b/extern/bullet2/src/LinearMath/btStackAlloc.h index d219b453537..050d44bdfe9 100644 --- a/extern/bullet2/src/LinearMath/btStackAlloc.h +++ b/extern/bullet2/src/LinearMath/btStackAlloc.h @@ -21,6 +21,7 @@ Nov.2006 #define BT_STACK_ALLOC #include "btScalar.h" //for btAssert +#include "btAlignedAllocator.h" struct btBlock { @@ -28,7 +29,7 @@ struct btBlock unsigned char* address; }; -///StackAlloc provides some fast stack-based memory allocator (LIFO last-in first-out) +///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out) class btStackAlloc { public: @@ -39,7 +40,7 @@ public: inline void create(unsigned int size) { destroy(); - data = new unsigned char[size]; + data = (unsigned char*) btAlignedAlloc(size,16); totalsize = size; } inline void destroy() @@ -49,12 +50,20 @@ public: if(usedsize==0) { - if(!ischild) delete[] data; + if(!ischild && data) + btAlignedFree(data); + data = 0; usedsize = 0; } } + + int getAvailableMemory() const + { + return static_cast<int>(totalsize - usedsize); + } + unsigned char* allocate(unsigned int size) { const unsigned int nus(usedsize+size); @@ -68,7 +77,7 @@ public: return(0); } - inline btBlock* beginBlock() + SIMD_FORCE_INLINE btBlock* beginBlock() { btBlock* pb = (btBlock*)allocate(sizeof(btBlock)); pb->previous = current; @@ -76,7 +85,7 @@ public: current = pb; return(pb); } - inline void endBlock(btBlock* block) + SIMD_FORCE_INLINE void endBlock(btBlock* block) { btAssert(block==current); //Raise(L"Unmatched blocks"); diff --git a/extern/bullet2/src/LinearMath/btTransform.h b/extern/bullet2/src/LinearMath/btTransform.h index 2d55fec83a4..a8cdb428100 100644 --- a/extern/bullet2/src/LinearMath/btTransform.h +++ b/extern/bullet2/src/LinearMath/btTransform.h @@ -21,7 +21,8 @@ subject to the following restrictions: #include "btMatrix3x3.h" -///btTransform supports rigid transforms (only translation and rotation, no scaling/shear) +///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 { @@ -92,13 +93,7 @@ public: m_basis.getRotation(q); return q; } - template <typename Scalar2> - void setValue(const Scalar2 *m) - { - m_basis.setValue(m); - m_origin.setValue(&m[12]); - } - + void setFromOpenGLMatrix(const btScalar *m) { @@ -194,8 +189,13 @@ btTransform::operator*(const btTransform& t) const { return btTransform(m_basis * t.m_basis, (*this)(t.m_origin)); -} +} +SIMD_FORCE_INLINE bool operator==(const btTransform& t1, const btTransform& t2) +{ + return ( t1.getBasis() == t2.getBasis() && + t1.getOrigin() == t2.getOrigin() ); +} #endif @@ -204,3 +204,4 @@ btTransform::operator*(const btTransform& t) const + diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h index bc42fd166b6..86ee1da5edf 100644 --- a/extern/bullet2/src/LinearMath/btTransformUtil.h +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -25,7 +25,7 @@ subject to the following restrictions: #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ -inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir) +SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir) { return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(), supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(), @@ -33,7 +33,7 @@ inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& sup } -inline void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q) +SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q) { if (btFabs(n.z()) > SIMDSQRT12) { // choose p in y-z plane @@ -70,7 +70,9 @@ public: predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5)); predictedOrn.normalize(); #else - //exponential map + //Exponential map + //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia + btVector3 axis; btScalar fAngle = angvel.length(); //limit the angular motion @@ -121,6 +123,10 @@ public: dmat.getRotation(dorn); #endif//USE_QUATERNION_DIFF + ///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.); diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h index 74d41ad2a19..96548c6ba60 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -27,6 +27,10 @@ class btVector3 : public btQuadWord { public: SIMD_FORCE_INLINE btVector3() {} + SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) + : btQuadWord(q) + { + } SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) @@ -43,6 +47,7 @@ public: SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v) { + m_x += v.x(); m_y += v.y(); m_z += v.z(); return *this; } @@ -399,4 +404,50 @@ public: }; + +///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization +SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal) +{ + #ifdef BT_USE_DOUBLE_PRECISION + unsigned char* dest = (unsigned char*) &destVal; + unsigned char* src = (unsigned char*) &sourceVal; + dest[0] = src[7]; + dest[1] = src[6]; + dest[2] = src[5]; + dest[3] = src[4]; + dest[4] = src[3]; + dest[5] = src[2]; + dest[6] = src[1]; + dest[7] = src[0]; +#else + unsigned char* dest = (unsigned char*) &destVal; + unsigned char* src = (unsigned char*) &sourceVal; + dest[0] = src[3]; + dest[1] = src[2]; + dest[2] = src[1]; + dest[3] = src[0]; +#endif //BT_USE_DOUBLE_PRECISION +} +///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization +SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec) +{ + for (int i=0;i<4;i++) + { + btSwapScalarEndian(sourceVec[i],destVec[i]); + } + +} + +///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization +SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector) +{ + + btVector3 swappedVec; + for (int i=0;i<4;i++) + { + btSwapScalarEndian(vector[i],swappedVec[i]); + } + vector = swappedVec; +} + #endif //SIMD__VECTOR3_H |