17 #ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H 18 #define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H 31 #define DBVT_IMPL_GENERIC 0 // Generic implementation 32 #define DBVT_IMPL_SSE 1 // SSE 36 #if (defined (_MSC_VER) && _MSC_VER >= 1400) 37 #define DBVT_USE_TEMPLATE 1 39 #define DBVT_USE_TEMPLATE 0 42 #define DBVT_USE_TEMPLATE 0 46 #define DBVT_USE_INTRINSIC_SSE 1 49 #define DBVT_USE_MEMMOVE 1 52 #define DBVT_ENABLE_BENCHMARK 0 55 #define DBVT_INLINE SIMD_FORCE_INLINE 60 #if defined (BT_USE_SSE) //&& defined (_WIN32) 61 #define DBVT_SELECT_IMPL DBVT_IMPL_SSE 62 #define DBVT_MERGE_IMPL DBVT_IMPL_SSE 63 #define DBVT_INT0_IMPL DBVT_IMPL_SSE 65 #define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC 66 #define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC 67 #define DBVT_INT0_IMPL DBVT_IMPL_GENERIC 70 #if (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)|| \ 71 (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)|| \ 72 (DBVT_INT0_IMPL==DBVT_IMPL_SSE) 73 #include <emmintrin.h> 82 #define DBVT_VIRTUAL_DTOR(a) 83 #define DBVT_PREFIX template <typename T> 84 #define DBVT_IPOLICY T& policy 85 #define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker; 87 #define DBVT_VIRTUAL_DTOR(a) virtual ~a() {} 88 #define DBVT_VIRTUAL virtual 90 #define DBVT_IPOLICY ICollide& policy 91 #define DBVT_CHECKTYPE 95 #if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) 101 #ifndef DBVT_USE_TEMPLATE 102 #error "DBVT_USE_TEMPLATE undefined" 105 #ifndef DBVT_USE_MEMMOVE 106 #error "DBVT_USE_MEMMOVE undefined" 109 #ifndef DBVT_ENABLE_BENCHMARK 110 #error "DBVT_ENABLE_BENCHMARK undefined" 113 #ifndef DBVT_SELECT_IMPL 114 #error "DBVT_SELECT_IMPL undefined" 117 #ifndef DBVT_MERGE_IMPL 118 #error "DBVT_MERGE_IMPL undefined" 121 #ifndef DBVT_INT0_IMPL 122 #error "DBVT_INT0_IMPL undefined" 244 virtual void Prepare(
const btDbvtNode* root,
int numnodes)=0;
245 virtual void WriteNode(
const btDbvtNode*,
int index,
int parent,
int child0,
int child1)=0;
246 virtual void WriteLeaf(
const btDbvtNode*,
int index,
int parent)=0;
257 SIMPLE_STACKSIZE = 64,
258 DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2
276 bool empty()
const {
return(0==m_root); }
277 void optimizeBottomUp();
278 void optimizeTopDown(
int bu_treshold=128);
279 void optimizeIncremental(
int passes);
281 void update(
btDbvtNode* leaf,
int lookahead=-1);
287 void write(
IWriter* iwriter)
const;
290 static int countLeaves(
const btDbvtNode* node);
292 #if DBVT_ENABLE_BENCHMARK 293 static void benchmark();
299 static void enumNodes(
const btDbvtNode* root,
302 static void enumLeaves(
const btDbvtNode* root,
310 void collideTTpersistentStack(
const btDbvtNode* root0,
333 void collideTVNoStackAlloc(
const btDbvtNode* root,
355 unsigned int signs[3],
363 static void collideKDOP(
const btDbvtNode* root,
369 static void collideOCL(
const btDbvtNode* root,
377 static void collideTU(
const btDbvtNode* root,
386 if(a[i[m]].value>=v) l=m+1;
else h=m;
396 { i=ifree[ifree.
size()-1];ifree.
pop_back();stock[i]=value; }
414 box.
mi=c-e;box.
mx=c+e;
436 box.
mi=box.
mx=pts[0];
449 box.
mi=box.
mx=*ppts[0];
475 return( (
mi.
x()<=a.
mi.
x())&&
506 if((
btDot(n,px)+o)<0)
return(-1);
507 if((
btDot(n,pi)+o)>=0)
return(+1);
516 b[(signs>>1)&1]->y(),
517 b[(signs>>2)&1]->z());
527 { smi+=
mx[i]*d[i];smx+=
mi[i]*d[i]; }
529 { smi+=
mi[i]*d[i];smx+=
mx[i]*d[i]; }
537 #if DBVT_INT0_IMPL == DBVT_IMPL_SSE 538 const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.
mx),_mm_load_ps(a.
mi)),
539 _mm_cmplt_ps(_mm_load_ps(a.
mx),_mm_load_ps(b.
mi))));
541 const __int32* pu((
const __int32*)&rt);
543 const int* pu((
const int*)&rt);
545 return((pu[0]|pu[1]|pu[2])==0);
547 return( (a.
mi.
x()<=b.
mx.
x())&&
562 return( (b.
x()>=a.
mi.
x())&&
592 #if DBVT_SELECT_IMPL == DBVT_IMPL_SSE 595 static ATTRIBUTE_ALIGNED16(
const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
597 static ATTRIBUTE_ALIGNED16(
const unsigned int) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 };
599 #if DBVT_USE_INTRINSIC_SSE 609 __m128 omi(_mm_load_ps(o.
mi));
610 omi=_mm_add_ps(omi,_mm_load_ps(o.
mx));
611 __m128 ami(_mm_load_ps(a.
mi));
612 ami=_mm_add_ps(ami,_mm_load_ps(a.
mx));
613 ami=_mm_sub_ps(ami,omi);
614 ami=_mm_and_ps(ami,_mm_load_ps((
const float*)mask));
615 __m128 bmi(_mm_load_ps(b.
mi));
616 bmi=_mm_add_ps(bmi,_mm_load_ps(b.
mx));
617 bmi=_mm_sub_ps(bmi,omi);
618 bmi=_mm_and_ps(bmi,_mm_load_ps((
const float*)mask));
619 __m128 t0(_mm_movehl_ps(ami,ami));
620 ami=_mm_add_ps(ami,t0);
621 ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
622 __m128 t1(_mm_movehl_ps(bmi,bmi));
623 bmi=_mm_add_ps(bmi,t1);
624 bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
627 tmp.ssereg = _mm_cmple_ss(bmi,ami);
628 return tmp.ints[0]&1;
671 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE 672 __m128 ami(_mm_load_ps(a.
mi));
673 __m128 amx(_mm_load_ps(a.
mx));
674 __m128 bmi(_mm_load_ps(b.
mi));
675 __m128 bmx(_mm_load_ps(b.
mx));
676 ami=_mm_min_ps(ami,bmi);
677 amx=_mm_max_ps(amx,bmx);
678 _mm_store_ps(r.
mi,ami);
679 _mm_store_ps(r.
mx,amx);
683 if(a.
mi[i]<b.
mi[i]) r.
mi[i]=a.
mi[i];
else r.
mi[i]=b.
mi[i];
684 if(a.
mx[i]>b.
mx[i]) r.
mx[i]=a.
mx[i];
else r.
mx[i]=b.
mx[i];
693 return( (a.
mi.
x()!=b.
mi.
x())||
711 policy.Process(root);
714 enumNodes(root->
childs[0],policy);
715 enumNodes(root->
childs[1],policy);
727 enumLeaves(root->
childs[0],policy);
728 enumLeaves(root->
childs[1],policy);
732 policy.Process(root);
746 int treshold=DOUBLE_STACKSIZE-4;
748 stkStack.
resize(DOUBLE_STACKSIZE);
749 stkStack[0]=
sStkNN(root0,root1);
751 sStkNN p=stkStack[--depth];
755 treshold=stkStack.
size()-4;
792 policy.Process(p.
a,p.
b);
811 int treshold=DOUBLE_STACKSIZE-4;
813 m_stkStack.
resize(DOUBLE_STACKSIZE);
814 m_stkStack[0]=
sStkNN(root0,root1);
816 sStkNN p=m_stkStack[--depth];
820 treshold=m_stkStack.
size()-4;
857 policy.Process(p.
a,p.
b);
877 int treshold=DOUBLE_STACKSIZE-4;
879 stkStack.
resize(DOUBLE_STACKSIZE);
880 stkStack[0]=
sStkNN(root0,root1);
882 sStkNN p=stkStack[--depth];
888 treshold=stkStack.
size()-4;
914 policy.Process(p.
a,p.
b);
930 collideTT(root0,root1,xform,policy);
945 #ifndef BT_DISABLE_STACK_TEMP_MEMORY 946 char tempmemory[SIMPLE_STACKSIZE*
sizeof(
const btDbvtNode*)];
949 stack.
reserve(SIMPLE_STACKSIZE);
950 #endif //BT_DISABLE_STACK_TEMP_MEMORY 968 }
while(stack.
size()>0);
984 stack.
reserve(SIMPLE_STACKSIZE);
1001 }
while(stack.
size()>0);
1011 unsigned int signs[3],
1025 int treshold=DOUBLE_STACKSIZE-2;
1026 stack.
resize(DOUBLE_STACKSIZE);
1035 unsigned int result1=
false;
1036 result1 =
btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
1044 treshold=stack.
size()-2;
1046 stack[depth++]=node->
childs[0];
1047 stack[depth++]=node->
childs[1];
1051 policy.Process(node);
1076 unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
1085 int treshold=DOUBLE_STACKSIZE-2;
1087 char tempmemory[DOUBLE_STACKSIZE *
sizeof(
const btDbvtNode*)];
1088 #ifndef BT_DISABLE_STACK_TEMP_MEMORY 1090 #else//BT_DISABLE_STACK_TEMP_MEMORY 1091 stack.
resize(DOUBLE_STACKSIZE);
1092 #endif //BT_DISABLE_STACK_TEMP_MEMORY 1102 unsigned int result1 =
btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
1104 #ifdef COMPARE_BTRAY_AABB2 1108 #endif //TEST_BTRAY_AABB2 1117 treshold=stack.
size()-2;
1119 stack[depth++]=node->
childs[0];
1120 stack[depth++]=node->
childs[1];
1124 policy.Process(node);
1143 const int inside=(1<<count)-1;
1145 int signs[
sizeof(unsigned)*8];
1146 btAssert(count<
int (
sizeof(signs)/
sizeof(signs[0])));
1147 for(
int i=0;i<count;++i)
1149 signs[i]= ((normals[i].
x()>=0)?1:0)+
1150 ((normals[i].
y()>=0)?2:0)+
1151 ((normals[i].
z()>=0)?4:0);
1153 stack.
reserve(SIMPLE_STACKSIZE);
1159 for(
int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
1166 case -1: out=
true;
break;
1167 case +1: se.
mask|=j;
break;
1180 if(policy.AllLeaves(se.
node)) enumLeaves(se.
node,policy);
1183 }
while(stack.
size());
1200 const unsigned srtsgns=(sortaxis[0]>=0?1:0)+
1201 (sortaxis[1]>=0?2:0)+
1202 (sortaxis[2]>=0?4:0);
1203 const int inside=(1<<count)-1;
1207 int signs[
sizeof(unsigned)*8];
1208 btAssert(count<
int (
sizeof(signs)/
sizeof(signs[0])));
1209 for(
int i=0;i<count;++i)
1211 signs[i]= ((normals[i].
x()>=0)?1:0)+
1212 ((normals[i].
y()>=0)?2:0)+
1213 ((normals[i].
z()>=0)?4:0);
1215 stock.
reserve(SIMPLE_STACKSIZE);
1216 stack.
reserve(SIMPLE_STACKSIZE);
1217 ifree.
reserve(SIMPLE_STACKSIZE);
1220 const int id=stack[stack.
size()-1];
1226 for(
int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
1233 case -1: out=
true;
break;
1234 case +1: se.
mask|=j;
break;
1240 if(policy.Descent(se.
node))
1252 j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.
size());
1257 #if DBVT_USE_MEMMOVE 1259 int num_items_to_move = stack.
size()-1-j;
1260 if(num_items_to_move > 0)
1261 memmove(&stack[j+1],&stack[j],
sizeof(
int)*num_items_to_move);
1264 for(
int k=stack.
size()-1;k>j;--k) {
1265 stack[k]=stack[k-1];
1268 stack[j]=allocate(ifree,stock,nes[q]);
1270 j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.
size());
1272 #if DBVT_USE_MEMMOVE 1274 int num_items_to_move = stack.
size()-1-j;
1275 if(num_items_to_move > 0)
1276 memmove(&stack[j+1],&stack[j],
sizeof(
int)*num_items_to_move);
1279 for(
int k=stack.
size()-1;k>j;--k) {
1280 stack[k]=stack[k-1];
1283 stack[j]=allocate(ifree,stock,nes[1-q]);
1287 stack.
push_back(allocate(ifree,stock,nes[q]));
1288 stack.
push_back(allocate(ifree,stock,nes[1-q]));
1296 }
while(stack.
size());
1309 stack.
reserve(SIMPLE_STACKSIZE);
1314 if(policy.Descent(n))
1319 { policy.Process(n); }
1321 }
while(stack.
size()>0);
1329 #undef DBVT_USE_MEMMOVE 1330 #undef DBVT_USE_TEMPLATE 1331 #undef DBVT_VIRTUAL_DTOR 1335 #undef DBVT_CHECKTYPE 1336 #undef DBVT_IMPL_GENERIC 1337 #undef DBVT_IMPL_SSE 1338 #undef DBVT_USE_INTRINSIC_SSE 1339 #undef DBVT_SELECT_IMPL 1340 #undef DBVT_MERGE_IMPL 1341 #undef DBVT_INT0_IMPL
static DBVT_PREFIX void collideKDOP(const btDbvtNode *root, const btVector3 *normals, const btScalar *offsets, int count, DBVT_IPOLICY)
void push_back(const T &_Val)
DBVT_INLINE int Classify(const btVector3 &n, btScalar o, int s) const
DBVT_INLINE bool isleaf() const
static DBVT_PREFIX void collideOCL(const btDbvtNode *root, const btVector3 *normals, const btScalar *offsets, const btVector3 &sortaxis, int count, DBVT_IPOLICY, bool fullsort=true)
sStkCLN(const btDbvtNode *n, btDbvtNode *p)
DBVT_VIRTUAL void Process(const btDbvtNode *)
void setZ(btScalar _z)
Set the z value.
static DBVT_PREFIX void collideTU(const btDbvtNode *root, DBVT_IPOLICY)
static btDbvtAabbMm FromPoints(const btVector3 *pts, int n)
The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes ...
static DBVT_INLINE int nearest(const int *i, const btDbvt::sStkNPS *a, btScalar v, int l, int h)
btAlignedObjectArray< const btDbvtNode * > btNodeStack
static DBVT_PREFIX void rayTest(const btDbvtNode *root, const btVector3 &rayFrom, const btVector3 &rayTo, DBVT_IPOLICY)
rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thre...
DBVT_INLINE friend void Merge(const btDbvtAabbMm &a, const btDbvtAabbMm &b, btDbvtAabbMm &r)
static DBVT_INLINE int allocate(btAlignedObjectArray< int > &ifree, btAlignedObjectArray< sStkNPS > &stock, const sStkNPS &value)
DBVT_INLINE bool isinternal() const
btScalar dot(const btVector3 &v) const
Return the dot product.
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
const btScalar & x() const
Return the x value.
void setX(btScalar _x)
Set the x value.
int size() const
return the number of elements in the array
static btDbvtAabbMm FromCE(const btVector3 &c, const btVector3 &e)
DBVT_VIRTUAL bool Descent(const btDbvtNode *)
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
DBVT_INLINE btVector3 Lengths() const
sStkNN(const btDbvtNode *na, const btDbvtNode *nb)
void setY(btScalar _y)
Set the y value.
void initializeFromBuffer(void *buffer, int size, int capacity)
DBVT_PREFIX void collideTVNoStackAlloc(const btDbvtNode *root, const btDbvtVolume &volume, btNodeStack &stack, DBVT_IPOLICY) const
sStkNP(const btDbvtNode *n, unsigned m)
DBVT_INLINE btScalar ProjectMinimum(const btVector3 &v, unsigned signs) const
DBVT_INLINE btVector3 Center() const
DBVT_INLINE void Expand(const btVector3 &e)
static btDbvtAabbMm FromCR(const btVector3 &c, btScalar r)
#define DBVT_VIRTUAL_DTOR(a)
btAlignedObjectArray< sStkNN > m_stkStack
const btScalar & y() const
Return the y value.
DBVT_INLINE void AddSpan(const btVector3 &d, btScalar &smi, btScalar &smx) const
DBVT_INLINE btVector3 Extents() const
btVector3 can be used to represent 3D points and vectors.
#define ATTRIBUTE_ALIGNED16(a)
DBVT_PREFIX void collideTTpersistentStack(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
DBVT_INLINE const btVector3 & Maxs() const
static void clear(T &value)
void resize(int newsize, const T &fillData=T())
DBVT_INLINE friend int Select(const btDbvtAabbMm &o, const btDbvtAabbMm &a, const btDbvtAabbMm &b)
DBVT_VIRTUAL void Process(const btDbvtNode *n, btScalar)
DBVT_INLINE friend btScalar Proximity(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
static btDbvtVolume bounds(btDbvtNode **leaves, int count)
static DBVT_PREFIX void enumLeaves(const btDbvtNode *root, DBVT_IPOLICY)
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
DBVT_INLINE btVector3 & tMaxs()
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
DBVT_INLINE bool Contain(const btDbvtAabbMm &a) const
static DBVT_PREFIX void enumNodes(const btDbvtNode *root, DBVT_IPOLICY)
DBVT_INLINE const btVector3 & Mins() const
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
rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory ...
DBVT_INLINE void SignedExpand(const btVector3 &e)
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)
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
bool btRayAabb(const btVector3 &rayFrom, const btVector3 &rayTo, const btVector3 &aabbMin, const btVector3 &aabbMax, btScalar ¶m, btVector3 &normal)
DBVT_VIRTUAL bool AllLeaves(const btDbvtNode *)
btDbvtAabbMm btDbvtVolume
DBVT_INLINE friend bool Intersect(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
virtual void CloneLeaf(btDbvtNode *)
DBVT_INLINE btVector3 & tMins()
sStkNPS(const btDbvtNode *n, unsigned m, btScalar v)
DBVT_INLINE friend bool NotEqual(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
btScalar btFabs(btScalar x)
const btScalar & z() const
Return the z value.