diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.h')
| -rw-r--r-- | thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.h | 1517 |
1 files changed, 789 insertions, 728 deletions
diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.h b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.h index b5a0014580..a316dbf207 100644 --- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.h @@ -26,50 +26,49 @@ subject to the following restrictions: // Compile time configuration // - // Implementation profiles -#define DBVT_IMPL_GENERIC 0 // Generic implementation -#define DBVT_IMPL_SSE 1 // SSE +#define DBVT_IMPL_GENERIC 0 // Generic implementation +#define DBVT_IMPL_SSE 1 // SSE // Template implementation of ICollide #ifdef _WIN32 -#if (defined (_MSC_VER) && _MSC_VER >= 1400) -#define DBVT_USE_TEMPLATE 1 +#if (defined(_MSC_VER) && _MSC_VER >= 1400) +#define DBVT_USE_TEMPLATE 1 #else -#define DBVT_USE_TEMPLATE 0 +#define DBVT_USE_TEMPLATE 0 #endif #else -#define DBVT_USE_TEMPLATE 0 +#define DBVT_USE_TEMPLATE 0 #endif // Use only intrinsics instead of inline asm -#define DBVT_USE_INTRINSIC_SSE 1 +#define DBVT_USE_INTRINSIC_SSE 1 // Using memmov for collideOCL -#define DBVT_USE_MEMMOVE 1 +#define DBVT_USE_MEMMOVE 1 // Enable benchmarking code -#define DBVT_ENABLE_BENCHMARK 0 +#define DBVT_ENABLE_BENCHMARK 0 // Inlining -#define DBVT_INLINE SIMD_FORCE_INLINE +#define DBVT_INLINE SIMD_FORCE_INLINE // Specific methods implementation //SSE gives errors on a MSVC 7.1 -#if defined (BT_USE_SSE) //&& defined (_WIN32) -#define DBVT_SELECT_IMPL DBVT_IMPL_SSE -#define DBVT_MERGE_IMPL DBVT_IMPL_SSE -#define DBVT_INT0_IMPL DBVT_IMPL_SSE +#if defined(BT_USE_SSE) //&& defined (_WIN32) +#define DBVT_SELECT_IMPL DBVT_IMPL_SSE +#define DBVT_MERGE_IMPL DBVT_IMPL_SSE +#define DBVT_INT0_IMPL DBVT_IMPL_SSE #else -#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC -#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC -#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC +#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC +#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC +#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC #endif -#if (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)|| \ - (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)|| \ - (DBVT_INT0_IMPL==DBVT_IMPL_SSE) +#if (DBVT_SELECT_IMPL == DBVT_IMPL_SSE) || \ + (DBVT_MERGE_IMPL == DBVT_IMPL_SSE) || \ + (DBVT_INT0_IMPL == DBVT_IMPL_SSE) #include <emmintrin.h> #endif @@ -78,21 +77,24 @@ subject to the following restrictions: // #if DBVT_USE_TEMPLATE -#define DBVT_VIRTUAL +#define DBVT_VIRTUAL #define DBVT_VIRTUAL_DTOR(a) -#define DBVT_PREFIX template <typename T> -#define DBVT_IPOLICY T& policy -#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker; +#define DBVT_PREFIX template <typename T> +#define DBVT_IPOLICY T& policy +#define DBVT_CHECKTYPE \ + static const ICollide& typechecker = *(T*)1; \ + (void)typechecker; #else -#define DBVT_VIRTUAL_DTOR(a) virtual ~a() {} -#define DBVT_VIRTUAL virtual +#define DBVT_VIRTUAL_DTOR(a) \ + virtual ~a() {} +#define DBVT_VIRTUAL virtual #define DBVT_PREFIX -#define DBVT_IPOLICY ICollide& policy +#define DBVT_IPOLICY ICollide& policy #define DBVT_CHECKTYPE #endif #if DBVT_USE_MEMMOVE -#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) +#if !defined(__CELLOS_LV2__) && !defined(__MWERKS__) #include <memory.h> #endif #include <string.h> @@ -122,194 +124,193 @@ subject to the following restrictions: #error "DBVT_INT0_IMPL undefined" #endif - // // Defaults volumes // -/* btDbvtAabbMm */ -struct btDbvtAabbMm +/* btDbvtAabbMm */ +struct btDbvtAabbMm { - DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } - DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } - DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } - DBVT_INLINE const btVector3& Mins() const { return(mi); } - DBVT_INLINE const btVector3& Maxs() const { return(mx); } - static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); - static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); - static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); - static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); - static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); - DBVT_INLINE void Expand(const btVector3& e); - DBVT_INLINE void SignedExpand(const btVector3& e); - DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; - DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; - DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; - DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - - DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btVector3& b); - - DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - DBVT_INLINE friend int Select( const btDbvtAabbMm& o, - const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - btDbvtAabbMm& r); - DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - - DBVT_INLINE btVector3& tMins() { return(mi); } - DBVT_INLINE btVector3& tMaxs() { return(mx); } - + DBVT_INLINE btVector3 Center() const { return ((mi + mx) / 2); } + DBVT_INLINE btVector3 Lengths() const { return (mx - mi); } + DBVT_INLINE btVector3 Extents() const { return ((mx - mi) / 2); } + DBVT_INLINE const btVector3& Mins() const { return (mi); } + DBVT_INLINE const btVector3& Maxs() const { return (mx); } + static inline btDbvtAabbMm FromCE(const btVector3& c, const btVector3& e); + static inline btDbvtAabbMm FromCR(const btVector3& c, btScalar r); + static inline btDbvtAabbMm FromMM(const btVector3& mi, const btVector3& mx); + static inline btDbvtAabbMm FromPoints(const btVector3* pts, int n); + static inline btDbvtAabbMm FromPoints(const btVector3** ppts, int n); + DBVT_INLINE void Expand(const btVector3& e); + DBVT_INLINE void SignedExpand(const btVector3& e); + DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; + DBVT_INLINE int Classify(const btVector3& n, btScalar o, int s) const; + DBVT_INLINE btScalar ProjectMinimum(const btVector3& v, unsigned signs) const; + DBVT_INLINE friend bool Intersect(const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + + DBVT_INLINE friend bool Intersect(const btDbvtAabbMm& a, + const btVector3& b); + + DBVT_INLINE friend btScalar Proximity(const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + DBVT_INLINE friend int Select(const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + DBVT_INLINE friend void Merge(const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + btDbvtAabbMm& r); + DBVT_INLINE friend bool NotEqual(const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + + DBVT_INLINE btVector3& tMins() { return (mi); } + DBVT_INLINE btVector3& tMaxs() { return (mx); } + private: - DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; + DBVT_INLINE void AddSpan(const btVector3& d, btScalar& smi, btScalar& smx) const; + private: - btVector3 mi,mx; + btVector3 mi, mx; }; -// Types -typedef btDbvtAabbMm btDbvtVolume; +// Types +typedef btDbvtAabbMm btDbvtVolume; -/* btDbvtNode */ -struct btDbvtNode +/* btDbvtNode */ +struct btDbvtNode { - btDbvtVolume volume; - btDbvtNode* parent; - DBVT_INLINE bool isleaf() const { return(childs[1]==0); } - DBVT_INLINE bool isinternal() const { return(!isleaf()); } - union - { - btDbvtNode* childs[2]; - void* data; - int dataAsInt; + btDbvtVolume volume; + btDbvtNode* parent; + DBVT_INLINE bool isleaf() const { return (childs[1] == 0); } + DBVT_INLINE bool isinternal() const { return (!isleaf()); } + union { + btDbvtNode* childs[2]; + void* data; + int dataAsInt; }; }; typedef btAlignedObjectArray<const btDbvtNode*> btNodeStack; - ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). ///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes. ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. -struct btDbvt +struct btDbvt { - /* Stack element */ - struct sStkNN + /* Stack element */ + struct sStkNN { - const btDbvtNode* a; - const btDbvtNode* b; + const btDbvtNode* a; + const btDbvtNode* b; sStkNN() {} - sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} + sStkNN(const btDbvtNode* na, const btDbvtNode* nb) : a(na), b(nb) {} }; - struct sStkNP + struct sStkNP { - const btDbvtNode* node; - int mask; - sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} + const btDbvtNode* node; + int mask; + sStkNP(const btDbvtNode* n, unsigned m) : node(n), mask(m) {} }; - struct sStkNPS + struct sStkNPS { - const btDbvtNode* node; - int mask; - btScalar value; + const btDbvtNode* node; + int mask; + btScalar value; sStkNPS() {} - sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} + sStkNPS(const btDbvtNode* n, unsigned m, btScalar v) : node(n), mask(m), value(v) {} }; - struct sStkCLN + struct sStkCLN { - const btDbvtNode* node; - btDbvtNode* parent; - sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} + const btDbvtNode* node; + btDbvtNode* parent; + sStkCLN(const btDbvtNode* n, btDbvtNode* p) : node(n), parent(p) {} }; // Policies/Interfaces - /* ICollide */ - struct ICollide - { + /* ICollide */ + struct ICollide + { DBVT_VIRTUAL_DTOR(ICollide) - DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} - DBVT_VIRTUAL void Process(const btDbvtNode*) {} - DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); } - DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); } - DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); } + DBVT_VIRTUAL void Process(const btDbvtNode*, const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode* n, btScalar) { Process(n); } + DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return (true); } + DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return (true); } }; - /* IWriter */ - struct IWriter + /* IWriter */ + struct IWriter { virtual ~IWriter() {} - virtual void Prepare(const btDbvtNode* root,int numnodes)=0; - virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0; - virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0; + virtual void Prepare(const btDbvtNode* root, int numnodes) = 0; + virtual void WriteNode(const btDbvtNode*, int index, int parent, int child0, int child1) = 0; + virtual void WriteLeaf(const btDbvtNode*, int index, int parent) = 0; }; - /* IClone */ - struct IClone + /* IClone */ + struct IClone { - virtual ~IClone() {} - virtual void CloneLeaf(btDbvtNode*) {} + virtual ~IClone() {} + virtual void CloneLeaf(btDbvtNode*) {} }; // Constants - enum { - SIMPLE_STACKSIZE = 64, - DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 + enum + { + SIMPLE_STACKSIZE = 64, + DOUBLE_STACKSIZE = SIMPLE_STACKSIZE * 2 }; // Fields - btDbvtNode* m_root; - btDbvtNode* m_free; - int m_lkhd; - int m_leaves; - unsigned m_opath; - - - btAlignedObjectArray<sStkNN> m_stkStack; + btDbvtNode* m_root; + btDbvtNode* m_free; + int m_lkhd; + int m_leaves; + unsigned m_opath; + btAlignedObjectArray<sStkNN> m_stkStack; // Methods btDbvt(); ~btDbvt(); - void clear(); - bool empty() const { return(0==m_root); } - void optimizeBottomUp(); - void optimizeTopDown(int bu_treshold=128); - void optimizeIncremental(int passes); - btDbvtNode* insert(const btDbvtVolume& box,void* data); - void update(btDbvtNode* leaf,int lookahead=-1); - void update(btDbvtNode* leaf,btDbvtVolume& volume); - bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin); - bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity); - bool update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin); - void remove(btDbvtNode* leaf); - void write(IWriter* iwriter) const; - void clone(btDbvt& dest,IClone* iclone=0) const; - static int maxdepth(const btDbvtNode* node); - static int countLeaves(const btDbvtNode* node); - static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves); + void clear(); + bool empty() const { return (0 == m_root); } + void optimizeBottomUp(); + void optimizeTopDown(int bu_treshold = 128); + void optimizeIncremental(int passes); + btDbvtNode* insert(const btDbvtVolume& box, void* data); + void update(btDbvtNode* leaf, int lookahead = -1); + void update(btDbvtNode* leaf, btDbvtVolume& volume); + bool update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity, btScalar margin); + bool update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity); + bool update(btDbvtNode* leaf, btDbvtVolume& volume, btScalar margin); + void remove(btDbvtNode* leaf); + void write(IWriter* iwriter) const; + void clone(btDbvt& dest, IClone* iclone = 0) const; + static int maxdepth(const btDbvtNode* node); + static int countLeaves(const btDbvtNode* node); + static void extractLeaves(const btDbvtNode* node, btAlignedObjectArray<const btDbvtNode*>& leaves); #if DBVT_ENABLE_BENCHMARK - static void benchmark(); + static void benchmark(); #else - static void benchmark(){} + static void benchmark() + { + } #endif // DBVT_IPOLICY must support ICollide policy/interface DBVT_PREFIX - static void enumNodes( const btDbvtNode* root, - DBVT_IPOLICY); + static void enumNodes(const btDbvtNode* root, + DBVT_IPOLICY); DBVT_PREFIX - static void enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY); + static void enumLeaves(const btDbvtNode* root, + DBVT_IPOLICY); DBVT_PREFIX - void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); + void collideTT(const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); DBVT_PREFIX - void collideTTpersistentStack( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); + void collideTTpersistentStack(const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); #if 0 DBVT_PREFIX void collideTT( const btDbvtNode* root0, @@ -325,82 +326,89 @@ struct btDbvt #endif DBVT_PREFIX - void collideTV( const btDbvtNode* root, - const btDbvtVolume& volume, - DBVT_IPOLICY) const; - + void collideTV(const btDbvtNode* root, + const btDbvtVolume& volume, + DBVT_IPOLICY) const; + DBVT_PREFIX - void collideTVNoStackAlloc( const btDbvtNode* root, - const btDbvtVolume& volume, - btNodeStack& stack, - DBVT_IPOLICY) const; - - - - + void collideTVNoStackAlloc(const btDbvtNode* root, + const btDbvtVolume& volume, + btNodeStack& stack, + DBVT_IPOLICY) const; + ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc) ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time DBVT_PREFIX - static void rayTest( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - DBVT_IPOLICY); + static void rayTest(const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + DBVT_IPOLICY); ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections ///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts DBVT_PREFIX - void rayTestInternal( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayDirectionInverse, - unsigned int signs[3], - btScalar lambda_max, - const btVector3& aabbMin, - const btVector3& aabbMax, - btAlignedObjectArray<const btDbvtNode*>& stack, - DBVT_IPOLICY) const; + void rayTestInternal(const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayDirectionInverse, + unsigned int signs[3], + btScalar lambda_max, + const btVector3& aabbMin, + const btVector3& aabbMax, + btAlignedObjectArray<const btDbvtNode*>& stack, + DBVT_IPOLICY) const; DBVT_PREFIX - static void collideKDOP(const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - int count, - DBVT_IPOLICY); + static void collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY); DBVT_PREFIX - static void collideOCL( const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - const btVector3& sortaxis, - int count, - DBVT_IPOLICY, - bool fullsort=true); + static void collideOCL(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fullsort = true); DBVT_PREFIX - static void collideTU( const btDbvtNode* root, - DBVT_IPOLICY); - // Helpers - static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h) + static void collideTU(const btDbvtNode* root, + DBVT_IPOLICY); + // Helpers + static DBVT_INLINE int nearest(const int* i, const btDbvt::sStkNPS* a, btScalar v, int l, int h) { - int m=0; - while(l<h) + int m = 0; + while (l < h) { - m=(l+h)>>1; - if(a[i[m]].value>=v) l=m+1; else h=m; + m = (l + h) >> 1; + if (a[i[m]].value >= v) + l = m + 1; + else + h = m; } - return(h); + return (h); } - static DBVT_INLINE int allocate( btAlignedObjectArray<int>& ifree, - btAlignedObjectArray<sStkNPS>& stock, - const sStkNPS& value) + static DBVT_INLINE int allocate(btAlignedObjectArray<int>& ifree, + btAlignedObjectArray<sStkNPS>& stock, + const sStkNPS& value) { - int i; - if(ifree.size()>0) - { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } + int i; + if (ifree.size() > 0) + { + i = ifree[ifree.size() - 1]; + ifree.pop_back(); + stock[i] = value; + } else - { i=stock.size();stock.push_back(value); } - return(i); + { + i = stock.size(); + stock.push_back(value); + } + return (i); } // private: - btDbvt(const btDbvt&) {} + btDbvt(const btDbvt&) {} }; // @@ -408,227 +416,252 @@ private: // // -inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) +inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c, const btVector3& e) { btDbvtAabbMm box; - box.mi=c-e;box.mx=c+e; - return(box); + box.mi = c - e; + box.mx = c + e; + return (box); } // -inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) +inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c, btScalar r) { - return(FromCE(c,btVector3(r,r,r))); + return (FromCE(c, btVector3(r, r, r))); } // -inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) +inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi, const btVector3& mx) { btDbvtAabbMm box; - box.mi=mi;box.mx=mx; - return(box); + box.mi = mi; + box.mx = mx; + return (box); } // -inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) +inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts, int n) { btDbvtAabbMm box; - box.mi=box.mx=pts[0]; - for(int i=1;i<n;++i) + box.mi = box.mx = pts[0]; + for (int i = 1; i < n; ++i) { box.mi.setMin(pts[i]); box.mx.setMax(pts[i]); } - return(box); + return (box); } // -inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n) +inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts, int n) { btDbvtAabbMm box; - box.mi=box.mx=*ppts[0]; - for(int i=1;i<n;++i) + box.mi = box.mx = *ppts[0]; + for (int i = 1; i < n; ++i) { box.mi.setMin(*ppts[i]); box.mx.setMax(*ppts[i]); } - return(box); + return (box); } // -DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e) +DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e) { - mi-=e;mx+=e; + mi -= e; + mx += e; } // -DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e) +DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e) { - if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); - if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); - if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); + if (e.x() > 0) + mx.setX(mx.x() + e[0]); + else + mi.setX(mi.x() + e[0]); + if (e.y() > 0) + mx.setY(mx.y() + e[1]); + else + mi.setY(mi.y() + e[1]); + if (e.z() > 0) + mx.setZ(mx.z() + e[2]); + else + mi.setZ(mi.z() + e[2]); } // -DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const +DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const { - return( (mi.x()<=a.mi.x())&& - (mi.y()<=a.mi.y())&& - (mi.z()<=a.mi.z())&& - (mx.x()>=a.mx.x())&& - (mx.y()>=a.mx.y())&& - (mx.z()>=a.mx.z())); + return ((mi.x() <= a.mi.x()) && + (mi.y() <= a.mi.y()) && + (mi.z() <= a.mi.z()) && + (mx.x() >= a.mx.x()) && + (mx.y() >= a.mx.y()) && + (mx.z() >= a.mx.z())); } // -DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const +DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n, btScalar o, int s) const { - btVector3 pi,px; - switch(s) + btVector3 pi, px; + switch (s) { - case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z()); - pi=btVector3(mx.x(),mx.y(),mx.z());break; - case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z()); - pi=btVector3(mi.x(),mx.y(),mx.z());break; - case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z()); - pi=btVector3(mx.x(),mi.y(),mx.z());break; - case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z()); - pi=btVector3(mi.x(),mi.y(),mx.z());break; - case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z()); - pi=btVector3(mx.x(),mx.y(),mi.z());break; - case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z()); - pi=btVector3(mi.x(),mx.y(),mi.z());break; - case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z()); - pi=btVector3(mx.x(),mi.y(),mi.z());break; - case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z()); - pi=btVector3(mi.x(),mi.y(),mi.z());break; + case (0 + 0 + 0): + px = btVector3(mi.x(), mi.y(), mi.z()); + pi = btVector3(mx.x(), mx.y(), mx.z()); + break; + case (1 + 0 + 0): + px = btVector3(mx.x(), mi.y(), mi.z()); + pi = btVector3(mi.x(), mx.y(), mx.z()); + break; + case (0 + 2 + 0): + px = btVector3(mi.x(), mx.y(), mi.z()); + pi = btVector3(mx.x(), mi.y(), mx.z()); + break; + case (1 + 2 + 0): + px = btVector3(mx.x(), mx.y(), mi.z()); + pi = btVector3(mi.x(), mi.y(), mx.z()); + break; + case (0 + 0 + 4): + px = btVector3(mi.x(), mi.y(), mx.z()); + pi = btVector3(mx.x(), mx.y(), mi.z()); + break; + case (1 + 0 + 4): + px = btVector3(mx.x(), mi.y(), mx.z()); + pi = btVector3(mi.x(), mx.y(), mi.z()); + break; + case (0 + 2 + 4): + px = btVector3(mi.x(), mx.y(), mx.z()); + pi = btVector3(mx.x(), mi.y(), mi.z()); + break; + case (1 + 2 + 4): + px = btVector3(mx.x(), mx.y(), mx.z()); + pi = btVector3(mi.x(), mi.y(), mi.z()); + break; } - if((btDot(n,px)+o)<0) return(-1); - if((btDot(n,pi)+o)>=0) return(+1); - return(0); + if ((btDot(n, px) + o) < 0) return (-1); + if ((btDot(n, pi) + o) >= 0) return (+1); + return (0); } // -DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const +DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v, unsigned signs) const { - const btVector3* b[]={&mx,&mi}; - const btVector3 p( b[(signs>>0)&1]->x(), - b[(signs>>1)&1]->y(), - b[(signs>>2)&1]->z()); - return(btDot(p,v)); + const btVector3* b[] = {&mx, &mi}; + const btVector3 p(b[(signs >> 0) & 1]->x(), + b[(signs >> 1) & 1]->y(), + b[(signs >> 2) & 1]->z()); + return (btDot(p, v)); } // -DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const +DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d, btScalar& smi, btScalar& smx) const { - for(int i=0;i<3;++i) + for (int i = 0; i < 3; ++i) { - if(d[i]<0) - { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; } + if (d[i] < 0) + { + smi += mx[i] * d[i]; + smx += mi[i] * d[i]; + } else - { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; } + { + smi += mi[i] * d[i]; + smx += mx[i] * d[i]; + } } } // -DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) +DBVT_INLINE bool Intersect(const btDbvtAabbMm& a, + const btDbvtAabbMm& b) { -#if DBVT_INT0_IMPL == DBVT_IMPL_SSE - const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), - _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); -#if defined (_WIN32) - const __int32* pu((const __int32*)&rt); +#if DBVT_INT0_IMPL == DBVT_IMPL_SSE + const __m128 rt(_mm_or_ps(_mm_cmplt_ps(_mm_load_ps(b.mx), _mm_load_ps(a.mi)), + _mm_cmplt_ps(_mm_load_ps(a.mx), _mm_load_ps(b.mi)))); +#if defined(_WIN32) + const __int32* pu((const __int32*)&rt); #else - const int* pu((const int*)&rt); + const int* pu((const int*)&rt); #endif - return((pu[0]|pu[1]|pu[2])==0); + return ((pu[0] | pu[1] | pu[2]) == 0); #else - return( (a.mi.x()<=b.mx.x())&& - (a.mx.x()>=b.mi.x())&& - (a.mi.y()<=b.mx.y())&& - (a.mx.y()>=b.mi.y())&& - (a.mi.z()<=b.mx.z())&& - (a.mx.z()>=b.mi.z())); + return ((a.mi.x() <= b.mx.x()) && + (a.mx.x() >= b.mi.x()) && + (a.mi.y() <= b.mx.y()) && + (a.mx.y() >= b.mi.y()) && + (a.mi.z() <= b.mx.z()) && + (a.mx.z() >= b.mi.z())); #endif } - - // -DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btVector3& b) +DBVT_INLINE bool Intersect(const btDbvtAabbMm& a, + const btVector3& b) { - return( (b.x()>=a.mi.x())&& - (b.y()>=a.mi.y())&& - (b.z()>=a.mi.z())&& - (b.x()<=a.mx.x())&& - (b.y()<=a.mx.y())&& - (b.z()<=a.mx.z())); + return ((b.x() >= a.mi.x()) && + (b.y() >= a.mi.y()) && + (b.z() >= a.mi.z()) && + (b.x() <= a.mx.x()) && + (b.y() <= a.mx.y()) && + (b.z() <= a.mx.z())); } - - - - ////////////////////////////////////// - // -DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) +DBVT_INLINE btScalar Proximity(const btDbvtAabbMm& a, + const btDbvtAabbMm& b) { - const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); - return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); + const btVector3 d = (a.mi + a.mx) - (b.mi + b.mx); + return (btFabs(d.x()) + btFabs(d.y()) + btFabs(d.z())); } - - // -DBVT_INLINE int Select( const btDbvtAabbMm& o, - const btDbvtAabbMm& a, - const btDbvtAabbMm& b) +DBVT_INLINE int Select(const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b) { -#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE - -#if defined (_WIN32) - static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; +#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE + +#if defined(_WIN32) + static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff}; #else - static ATTRIBUTE_ALIGNED16(const unsigned int) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 /*0x7fffffff*/}; + static ATTRIBUTE_ALIGNED16(const unsigned int) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x00000000 /*0x7fffffff*/}; #endif ///@todo: the intrinsic version is 11% slower #if DBVT_USE_INTRINSIC_SSE - union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory + union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory { - __m128 ssereg; - float floats[4]; - int ints[4]; + __m128 ssereg; + float floats[4]; + int ints[4]; }; - __m128 omi(_mm_load_ps(o.mi)); - omi=_mm_add_ps(omi,_mm_load_ps(o.mx)); - __m128 ami(_mm_load_ps(a.mi)); - ami=_mm_add_ps(ami,_mm_load_ps(a.mx)); - ami=_mm_sub_ps(ami,omi); - ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask)); - __m128 bmi(_mm_load_ps(b.mi)); - bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx)); - bmi=_mm_sub_ps(bmi,omi); - bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask)); - __m128 t0(_mm_movehl_ps(ami,ami)); - ami=_mm_add_ps(ami,t0); - ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1)); - __m128 t1(_mm_movehl_ps(bmi,bmi)); - bmi=_mm_add_ps(bmi,t1); - bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); - + __m128 omi(_mm_load_ps(o.mi)); + omi = _mm_add_ps(omi, _mm_load_ps(o.mx)); + __m128 ami(_mm_load_ps(a.mi)); + ami = _mm_add_ps(ami, _mm_load_ps(a.mx)); + ami = _mm_sub_ps(ami, omi); + ami = _mm_and_ps(ami, _mm_load_ps((const float*)mask)); + __m128 bmi(_mm_load_ps(b.mi)); + bmi = _mm_add_ps(bmi, _mm_load_ps(b.mx)); + bmi = _mm_sub_ps(bmi, omi); + bmi = _mm_and_ps(bmi, _mm_load_ps((const float*)mask)); + __m128 t0(_mm_movehl_ps(ami, ami)); + ami = _mm_add_ps(ami, t0); + ami = _mm_add_ss(ami, _mm_shuffle_ps(ami, ami, 1)); + __m128 t1(_mm_movehl_ps(bmi, bmi)); + bmi = _mm_add_ps(bmi, t1); + bmi = _mm_add_ss(bmi, _mm_shuffle_ps(bmi, bmi, 1)); + btSSEUnion tmp; - tmp.ssereg = _mm_cmple_ss(bmi,ami); - return tmp.ints[0]&1; + tmp.ssereg = _mm_cmple_ss(bmi, ami); + return tmp.ints[0] & 1; #else - ATTRIBUTE_ALIGNED16(__int32 r[1]); + ATTRIBUTE_ALIGNED16(__int32 r[1]); __asm { mov eax,o @@ -656,46 +689,52 @@ DBVT_INLINE int Select( const btDbvtAabbMm& o, cmpless xmm2,xmm1 movss r,xmm2 } - return(r[0]&1); + return (r[0] & 1); #endif #else - return(Proximity(o,a)<Proximity(o,b)?0:1); + return (Proximity(o, a) < Proximity(o, b) ? 0 : 1); #endif } // -DBVT_INLINE void Merge( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - btDbvtAabbMm& r) +DBVT_INLINE void Merge(const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + btDbvtAabbMm& r) { -#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE - __m128 ami(_mm_load_ps(a.mi)); - __m128 amx(_mm_load_ps(a.mx)); - __m128 bmi(_mm_load_ps(b.mi)); - __m128 bmx(_mm_load_ps(b.mx)); - ami=_mm_min_ps(ami,bmi); - amx=_mm_max_ps(amx,bmx); - _mm_store_ps(r.mi,ami); - _mm_store_ps(r.mx,amx); +#if DBVT_MERGE_IMPL == DBVT_IMPL_SSE + __m128 ami(_mm_load_ps(a.mi)); + __m128 amx(_mm_load_ps(a.mx)); + __m128 bmi(_mm_load_ps(b.mi)); + __m128 bmx(_mm_load_ps(b.mx)); + ami = _mm_min_ps(ami, bmi); + amx = _mm_max_ps(amx, bmx); + _mm_store_ps(r.mi, ami); + _mm_store_ps(r.mx, amx); #else - for(int i=0;i<3;++i) + for (int i = 0; i < 3; ++i) { - if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i]; - if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; + if (a.mi[i] < b.mi[i]) + r.mi[i] = a.mi[i]; + else + r.mi[i] = b.mi[i]; + if (a.mx[i] > b.mx[i]) + r.mx[i] = a.mx[i]; + else + r.mx[i] = b.mx[i]; } #endif } // -DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) +DBVT_INLINE bool NotEqual(const btDbvtAabbMm& a, + const btDbvtAabbMm& b) { - return( (a.mi.x()!=b.mi.x())|| - (a.mi.y()!=b.mi.y())|| - (a.mi.z()!=b.mi.z())|| - (a.mx.x()!=b.mx.x())|| - (a.mx.y()!=b.mx.y())|| - (a.mx.z()!=b.mx.z())); + return ((a.mi.x() != b.mi.x()) || + (a.mi.y() != b.mi.y()) || + (a.mi.z() != b.mi.z()) || + (a.mx.x() != b.mx.x()) || + (a.mx.y() != b.mx.y()) || + (a.mx.z() != b.mx.z())); } // @@ -704,162 +743,162 @@ DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, // DBVT_PREFIX -inline void btDbvt::enumNodes( const btDbvtNode* root, - DBVT_IPOLICY) +inline void btDbvt::enumNodes(const btDbvtNode* root, + DBVT_IPOLICY) { DBVT_CHECKTYPE - policy.Process(root); - if(root->isinternal()) + policy.Process(root); + if (root->isinternal()) { - enumNodes(root->childs[0],policy); - enumNodes(root->childs[1],policy); + enumNodes(root->childs[0], policy); + enumNodes(root->childs[1], policy); } } // DBVT_PREFIX -inline void btDbvt::enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY) +inline void btDbvt::enumLeaves(const btDbvtNode* root, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root->isinternal()) - { - enumLeaves(root->childs[0],policy); - enumLeaves(root->childs[1],policy); - } - else - { - policy.Process(root); - } + if (root->isinternal()) + { + enumLeaves(root->childs[0], policy); + enumLeaves(root->childs[1], policy); + } + else + { + policy.Process(root); + } } // DBVT_PREFIX -inline void btDbvt::collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY) +inline void btDbvt::collideTT(const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root0&&root1) + if (root0 && root1) + { + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 4; + btAlignedObjectArray<sStkNN> stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0] = sStkNN(root0, root1); + do { - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - btAlignedObjectArray<sStkNN> stkStack; - stkStack.resize(DOUBLE_STACKSIZE); - stkStack[0]=sStkNN(root0,root1); - do { - sStkNN p=stkStack[--depth]; - if(depth>treshold) + sStkNN p = stkStack[--depth]; + if (depth > treshold) + { + stkStack.resize(stkStack.size() * 2); + treshold = stkStack.size() - 4; + } + if (p.a == p.b) + { + if (p.a->isinternal()) { - stkStack.resize(stkStack.size()*2); - treshold=stkStack.size()-4; + stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]); + stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]); + stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]); } - if(p.a==p.b) + } + else if (Intersect(p.a->volume, p.b->volume)) + { + if (p.a->isinternal()) { - if(p.a->isinternal()) + if (p.b->isinternal()) + { + stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]); + stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]); + } + else { - stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); - stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); - stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + stkStack[depth++] = sStkNN(p.a->childs[0], p.b); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b); } } - else if(Intersect(p.a->volume,p.b->volume)) + else { - if(p.a->isinternal()) + if (p.b->isinternal()) { - if(p.b->isinternal()) - { - stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - stkStack[depth++]=sStkNN(p.a->childs[0],p.b); - stkStack[depth++]=sStkNN(p.a->childs[1],p.b); - } + stkStack[depth++] = sStkNN(p.a, p.b->childs[0]); + stkStack[depth++] = sStkNN(p.a, p.b->childs[1]); } else { - if(p.b->isinternal()) - { - stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); - stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); - } - else - { - policy.Process(p.a,p.b); - } + policy.Process(p.a, p.b); } } - } while(depth); - } + } + } while (depth); + } } - - DBVT_PREFIX -inline void btDbvt::collideTTpersistentStack( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY) +inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root0&&root1) + if (root0 && root1) + { + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 4; + + m_stkStack.resize(DOUBLE_STACKSIZE); + m_stkStack[0] = sStkNN(root0, root1); + do { - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - - m_stkStack.resize(DOUBLE_STACKSIZE); - m_stkStack[0]=sStkNN(root0,root1); - do { - sStkNN p=m_stkStack[--depth]; - if(depth>treshold) + sStkNN p = m_stkStack[--depth]; + if (depth > treshold) + { + m_stkStack.resize(m_stkStack.size() * 2); + treshold = m_stkStack.size() - 4; + } + if (p.a == p.b) + { + if (p.a->isinternal()) { - m_stkStack.resize(m_stkStack.size()*2); - treshold=m_stkStack.size()-4; + m_stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]); + m_stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]); + m_stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]); } - if(p.a==p.b) + } + else if (Intersect(p.a->volume, p.b->volume)) + { + if (p.a->isinternal()) { - if(p.a->isinternal()) + if (p.b->isinternal()) { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]); + m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]); + m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]); + m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]); + } + else + { + m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b); + m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b); } } - else if(Intersect(p.a->volume,p.b->volume)) + else { - if(p.a->isinternal()) + if (p.b->isinternal()) { - if(p.b->isinternal()) - { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); - } + m_stkStack[depth++] = sStkNN(p.a, p.b->childs[0]); + m_stkStack[depth++] = sStkNN(p.a, p.b->childs[1]); } else { - if(p.b->isinternal()) - { - m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); - } - else - { - policy.Process(p.a,p.b); - } + policy.Process(p.a, p.b); } } - } while(depth); - } + } + } while (depth); + } } #if 0 @@ -929,33 +968,35 @@ inline void btDbvt::collideTT( const btDbvtNode* root0, const btTransform xform=xform0.inverse()*xform1; collideTT(root0,root1,xform,policy); } -#endif +#endif DBVT_PREFIX -inline void btDbvt::collideTV( const btDbvtNode* root, - const btDbvtVolume& vol, - DBVT_IPOLICY) const +inline void btDbvt::collideTV(const btDbvtNode* root, + const btDbvtVolume& vol, + DBVT_IPOLICY) const { DBVT_CHECKTYPE - if(root) + if (root) { - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); - btAlignedObjectArray<const btDbvtNode*> stack; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume(vol); + btAlignedObjectArray<const btDbvtNode*> stack; stack.resize(0); #ifndef BT_DISABLE_STACK_TEMP_MEMORY - char tempmemory[SIMPLE_STACKSIZE*sizeof(const btDbvtNode*)]; + char tempmemory[SIMPLE_STACKSIZE * sizeof(const btDbvtNode*)]; stack.initializeFromBuffer(tempmemory, 0, SIMPLE_STACKSIZE); #else stack.reserve(SIMPLE_STACKSIZE); -#endif //BT_DISABLE_STACK_TEMP_MEMORY +#endif //BT_DISABLE_STACK_TEMP_MEMORY stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; + do + { + const btDbvtNode* n = stack[stack.size() - 1]; stack.pop_back(); - if(Intersect(n->volume,volume)) + if (Intersect(n->volume, volume)) { - if(n->isinternal()) + if (n->isinternal()) { stack.push_back(n->childs[0]); stack.push_back(n->childs[1]); @@ -965,30 +1006,32 @@ inline void btDbvt::collideTV( const btDbvtNode* root, policy.Process(n); } } - } while(stack.size()>0); + } while (stack.size() > 0); } } // DBVT_PREFIX -inline void btDbvt::collideTVNoStackAlloc( const btDbvtNode* root, - const btDbvtVolume& vol, - btNodeStack& stack, - DBVT_IPOLICY) const +inline void btDbvt::collideTVNoStackAlloc(const btDbvtNode* root, + const btDbvtVolume& vol, + btNodeStack& stack, + DBVT_IPOLICY) const { DBVT_CHECKTYPE - if(root) + if (root) { - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume(vol); stack.resize(0); stack.reserve(SIMPLE_STACKSIZE); stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; + do + { + const btDbvtNode* n = stack[stack.size() - 1]; stack.pop_back(); - if(Intersect(n->volume,volume)) + if (Intersect(n->volume, volume)) { - if(n->isinternal()) + if (n->isinternal()) { stack.push_back(n->childs[0]); stack.push_back(n->childs[1]); @@ -998,328 +1041,346 @@ inline void btDbvt::collideTVNoStackAlloc( const btDbvtNode* root, policy.Process(n); } } - } while(stack.size()>0); + } while (stack.size() > 0); } } - DBVT_PREFIX -inline void btDbvt::rayTestInternal( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayDirectionInverse, - unsigned int signs[3], - btScalar lambda_max, - const btVector3& aabbMin, - const btVector3& aabbMax, - btAlignedObjectArray<const btDbvtNode*>& stack, - DBVT_IPOLICY ) const +inline void btDbvt::rayTestInternal(const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayDirectionInverse, + unsigned int signs[3], + btScalar lambda_max, + const btVector3& aabbMin, + const btVector3& aabbMax, + btAlignedObjectArray<const btDbvtNode*>& stack, + DBVT_IPOLICY) const { - (void) rayTo; + (void)rayTo; DBVT_CHECKTYPE - if(root) + if (root) { btVector3 resultNormal; - int depth=1; - int treshold=DOUBLE_STACKSIZE-2; + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 2; stack.resize(DOUBLE_STACKSIZE); - stack[0]=root; + stack[0] = root; btVector3 bounds[2]; - do + do { - const btDbvtNode* node=stack[--depth]; - bounds[0] = node->volume.Mins()-aabbMax; - bounds[1] = node->volume.Maxs()-aabbMin; - btScalar tmin=1.f,lambda_min=0.f; - unsigned int result1=false; - result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); - if(result1) + const btDbvtNode* node = stack[--depth]; + bounds[0] = node->volume.Mins() - aabbMax; + bounds[1] = node->volume.Maxs() - aabbMin; + btScalar tmin = 1.f, lambda_min = 0.f; + unsigned int result1 = false; + result1 = btRayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max); + if (result1) { - if(node->isinternal()) + if (node->isinternal()) { - if(depth>treshold) + if (depth > treshold) { - stack.resize(stack.size()*2); - treshold=stack.size()-2; + stack.resize(stack.size() * 2); + treshold = stack.size() - 2; } - stack[depth++]=node->childs[0]; - stack[depth++]=node->childs[1]; + stack[depth++] = node->childs[0]; + stack[depth++] = node->childs[1]; } else { policy.Process(node); } } - } while(depth); + } while (depth); } } // DBVT_PREFIX -inline void btDbvt::rayTest( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - DBVT_IPOLICY) +inline void btDbvt::rayTest(const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root) - { - btVector3 rayDir = (rayTo-rayFrom); - rayDir.normalize (); + if (root) + { + btVector3 rayDir = (rayTo - rayFrom); + rayDir.normalize(); - ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT - btVector3 rayDirectionInverse; - rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; - rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; - rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; - unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; + ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT + btVector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; + unsigned int signs[3] = {rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; - btScalar lambda_max = rayDir.dot(rayTo-rayFrom); + btScalar lambda_max = rayDir.dot(rayTo - rayFrom); - btVector3 resultNormal; + btVector3 resultNormal; - btAlignedObjectArray<const btDbvtNode*> stack; + btAlignedObjectArray<const btDbvtNode*> stack; - int depth=1; - int treshold=DOUBLE_STACKSIZE-2; + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 2; - char tempmemory[DOUBLE_STACKSIZE * sizeof(const btDbvtNode*)]; + char tempmemory[DOUBLE_STACKSIZE * sizeof(const btDbvtNode*)]; #ifndef BT_DISABLE_STACK_TEMP_MEMORY - stack.initializeFromBuffer(tempmemory, DOUBLE_STACKSIZE, DOUBLE_STACKSIZE); -#else//BT_DISABLE_STACK_TEMP_MEMORY - stack.resize(DOUBLE_STACKSIZE); -#endif //BT_DISABLE_STACK_TEMP_MEMORY - stack[0]=root; - btVector3 bounds[2]; - do { - const btDbvtNode* node=stack[--depth]; + stack.initializeFromBuffer(tempmemory, DOUBLE_STACKSIZE, DOUBLE_STACKSIZE); +#else //BT_DISABLE_STACK_TEMP_MEMORY + stack.resize(DOUBLE_STACKSIZE); +#endif //BT_DISABLE_STACK_TEMP_MEMORY + stack[0] = root; + btVector3 bounds[2]; + do + { + const btDbvtNode* node = stack[--depth]; + + bounds[0] = node->volume.Mins(); + bounds[1] = node->volume.Maxs(); - bounds[0] = node->volume.Mins(); - bounds[1] = node->volume.Maxs(); - - btScalar tmin=1.f,lambda_min=0.f; - unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + btScalar tmin = 1.f, lambda_min = 0.f; + unsigned int result1 = btRayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max); #ifdef COMPARE_BTRAY_AABB2 - btScalar param=1.f; - bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); - btAssert(result1 == result2); -#endif //TEST_BTRAY_AABB2 + btScalar param = 1.f; + bool result2 = btRayAabb(rayFrom, rayTo, node->volume.Mins(), node->volume.Maxs(), param, resultNormal); + btAssert(result1 == result2); +#endif //TEST_BTRAY_AABB2 - if(result1) + if (result1) + { + if (node->isinternal()) { - if(node->isinternal()) + if (depth > treshold) { - if(depth>treshold) - { - stack.resize(stack.size()*2); - treshold=stack.size()-2; - } - stack[depth++]=node->childs[0]; - stack[depth++]=node->childs[1]; - } - else - { - policy.Process(node); + stack.resize(stack.size() * 2); + treshold = stack.size() - 2; } + stack[depth++] = node->childs[0]; + stack[depth++] = node->childs[1]; } - } while(depth); - - } + else + { + policy.Process(node); + } + } + } while (depth); + } } // DBVT_PREFIX -inline void btDbvt::collideKDOP(const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - int count, - DBVT_IPOLICY) +inline void btDbvt::collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root) + if (root) + { + const int inside = (1 << count) - 1; + btAlignedObjectArray<sStkNP> stack; + int signs[sizeof(unsigned) * 8]; + btAssert(count < int(sizeof(signs) / sizeof(signs[0]))); + for (int i = 0; i < count; ++i) { - const int inside=(1<<count)-1; - btAlignedObjectArray<sStkNP> stack; - int signs[sizeof(unsigned)*8]; - btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); - for(int i=0;i<count;++i) + signs[i] = ((normals[i].x() >= 0) ? 1 : 0) + + ((normals[i].y() >= 0) ? 2 : 0) + + ((normals[i].z() >= 0) ? 4 : 0); + } + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(sStkNP(root, 0)); + do + { + sStkNP se = stack[stack.size() - 1]; + bool out = false; + stack.pop_back(); + for (int i = 0, j = 1; (!out) && (i < count); ++i, j <<= 1) { - signs[i]= ((normals[i].x()>=0)?1:0)+ - ((normals[i].y()>=0)?2:0)+ - ((normals[i].z()>=0)?4:0); - } - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(sStkNP(root,0)); - do { - sStkNP se=stack[stack.size()-1]; - bool out=false; - stack.pop_back(); - for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) + if (0 == (se.mask & j)) { - if(0==(se.mask&j)) + const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]); + switch (side) { - const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); - switch(side) - { - case -1: out=true;break; - case +1: se.mask|=j;break; - } + case -1: + out = true; + break; + case +1: + se.mask |= j; + break; } } - if(!out) + } + if (!out) + { + if ((se.mask != inside) && (se.node->isinternal())) { - if((se.mask!=inside)&&(se.node->isinternal())) - { - stack.push_back(sStkNP(se.node->childs[0],se.mask)); - stack.push_back(sStkNP(se.node->childs[1],se.mask)); - } - else - { - if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); - } + stack.push_back(sStkNP(se.node->childs[0], se.mask)); + stack.push_back(sStkNP(se.node->childs[1], se.mask)); } - } while(stack.size()); - } + else + { + if (policy.AllLeaves(se.node)) enumLeaves(se.node, policy); + } + } + } while (stack.size()); + } } // DBVT_PREFIX -inline void btDbvt::collideOCL( const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - const btVector3& sortaxis, - int count, - DBVT_IPOLICY, - bool fsort) +inline void btDbvt::collideOCL(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fsort) { DBVT_CHECKTYPE - if(root) + if (root) + { + const unsigned srtsgns = (sortaxis[0] >= 0 ? 1 : 0) + + (sortaxis[1] >= 0 ? 2 : 0) + + (sortaxis[2] >= 0 ? 4 : 0); + const int inside = (1 << count) - 1; + btAlignedObjectArray<sStkNPS> stock; + btAlignedObjectArray<int> ifree; + btAlignedObjectArray<int> stack; + int signs[sizeof(unsigned) * 8]; + btAssert(count < int(sizeof(signs) / sizeof(signs[0]))); + for (int i = 0; i < count; ++i) { - const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ - (sortaxis[1]>=0?2:0)+ - (sortaxis[2]>=0?4:0); - const int inside=(1<<count)-1; - btAlignedObjectArray<sStkNPS> stock; - btAlignedObjectArray<int> ifree; - btAlignedObjectArray<int> stack; - int signs[sizeof(unsigned)*8]; - btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); - for(int i=0;i<count;++i) + signs[i] = ((normals[i].x() >= 0) ? 1 : 0) + + ((normals[i].y() >= 0) ? 2 : 0) + + ((normals[i].z() >= 0) ? 4 : 0); + } + stock.reserve(SIMPLE_STACKSIZE); + stack.reserve(SIMPLE_STACKSIZE); + ifree.reserve(SIMPLE_STACKSIZE); + stack.push_back(allocate(ifree, stock, sStkNPS(root, 0, root->volume.ProjectMinimum(sortaxis, srtsgns)))); + do + { + const int id = stack[stack.size() - 1]; + sStkNPS se = stock[id]; + stack.pop_back(); + ifree.push_back(id); + if (se.mask != inside) { - signs[i]= ((normals[i].x()>=0)?1:0)+ - ((normals[i].y()>=0)?2:0)+ - ((normals[i].z()>=0)?4:0); - } - stock.reserve(SIMPLE_STACKSIZE); - stack.reserve(SIMPLE_STACKSIZE); - ifree.reserve(SIMPLE_STACKSIZE); - stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); - do { - const int id=stack[stack.size()-1]; - sStkNPS se=stock[id]; - stack.pop_back();ifree.push_back(id); - if(se.mask!=inside) + bool out = false; + for (int i = 0, j = 1; (!out) && (i < count); ++i, j <<= 1) { - bool out=false; - for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) + if (0 == (se.mask & j)) { - if(0==(se.mask&j)) + const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]); + switch (side) { - const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); - switch(side) - { - case -1: out=true;break; - case +1: se.mask|=j;break; - } + case -1: + out = true; + break; + case +1: + se.mask |= j; + break; } } - if(out) continue; } - if(policy.Descent(se.node)) + if (out) continue; + } + if (policy.Descent(se.node)) + { + if (se.node->isinternal()) { - if(se.node->isinternal()) + const btDbvtNode* pns[] = {se.node->childs[0], se.node->childs[1]}; + sStkNPS nes[] = {sStkNPS(pns[0], se.mask, pns[0]->volume.ProjectMinimum(sortaxis, srtsgns)), + sStkNPS(pns[1], se.mask, pns[1]->volume.ProjectMinimum(sortaxis, srtsgns))}; + const int q = nes[0].value < nes[1].value ? 1 : 0; + int j = stack.size(); + if (fsort && (j > 0)) { - const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; - sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), - sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; - const int q=nes[0].value<nes[1].value?1:0; - int j=stack.size(); - if(fsort&&(j>0)) - { - /* Insert 0 */ - j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); - stack.push_back(0); - - //void * memmove ( void * destination, const void * source, size_t num ); - + /* Insert 0 */ + j = nearest(&stack[0], &stock[0], nes[q].value, 0, stack.size()); + stack.push_back(0); + + //void * memmove ( void * destination, const void * source, size_t num ); + #if DBVT_USE_MEMMOVE - { - int num_items_to_move = stack.size()-1-j; - if(num_items_to_move > 0) - memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move); - } + { + int num_items_to_move = stack.size() - 1 - j; + if (num_items_to_move > 0) + memmove(&stack[j + 1], &stack[j], sizeof(int) * num_items_to_move); + } #else - for(int k=stack.size()-1;k>j;--k) { - stack[k]=stack[k-1]; - } + for (int k = stack.size() - 1; k > j; --k) + { + stack[k] = stack[k - 1]; + } #endif - stack[j]=allocate(ifree,stock,nes[q]); - /* Insert 1 */ - j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); - stack.push_back(0); + stack[j] = allocate(ifree, stock, nes[q]); + /* Insert 1 */ + j = nearest(&stack[0], &stock[0], nes[1 - q].value, j, stack.size()); + stack.push_back(0); #if DBVT_USE_MEMMOVE - { - int num_items_to_move = stack.size()-1-j; - if(num_items_to_move > 0) - memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move); - } -#else - for(int k=stack.size()-1;k>j;--k) { - stack[k]=stack[k-1]; - } -#endif - stack[j]=allocate(ifree,stock,nes[1-q]); + { + int num_items_to_move = stack.size() - 1 - j; + if (num_items_to_move > 0) + memmove(&stack[j + 1], &stack[j], sizeof(int) * num_items_to_move); } - else +#else + for (int k = stack.size() - 1; k > j; --k) { - stack.push_back(allocate(ifree,stock,nes[q])); - stack.push_back(allocate(ifree,stock,nes[1-q])); + stack[k] = stack[k - 1]; } +#endif + stack[j] = allocate(ifree, stock, nes[1 - q]); } else { - policy.Process(se.node,se.value); + stack.push_back(allocate(ifree, stock, nes[q])); + stack.push_back(allocate(ifree, stock, nes[1 - q])); } } - } while(stack.size()); - } + else + { + policy.Process(se.node, se.value); + } + } + } while (stack.size()); + } } // DBVT_PREFIX -inline void btDbvt::collideTU( const btDbvtNode* root, - DBVT_IPOLICY) +inline void btDbvt::collideTU(const btDbvtNode* root, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root) + if (root) + { + btAlignedObjectArray<const btDbvtNode*> stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { - btAlignedObjectArray<const btDbvtNode*> stack; - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; - stack.pop_back(); - if(policy.Descent(n)) + const btDbvtNode* n = stack[stack.size() - 1]; + stack.pop_back(); + if (policy.Descent(n)) + { + if (n->isinternal()) { - if(n->isinternal()) - { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } - else - { policy.Process(n); } + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); } - } while(stack.size()>0); - } + else + { + policy.Process(n); + } + } + } while (stack.size() > 0); + } } // |