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" 287 void write(IWriter* iwriter)
const;
292 #if DBVT_ENABLE_BENCHMARK 355 unsigned int signs[3],
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);
732 policy.Process(root);
749 stkStack[0]=
sStkNN(root0,root1);
751 sStkNN p=stkStack[--depth];
755 treshold=stkStack.
size()-4;
792 policy.Process(p.
a,p.
b);
857 policy.Process(p.
a,p.
b);
880 stkStack[0]=sStkNN(root0,root1);
882 sStkNN p=stkStack[--depth];
883 if(
Intersect(p.a->volume,p.b->volume,xform))
888 treshold=stkStack.
size()-4;
890 if(p.a->isinternal())
892 if(p.b->isinternal())
894 stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
895 stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
896 stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
897 stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
901 stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
902 stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
907 if(p.b->isinternal())
909 stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
910 stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
914 policy.Process(p.a,p.b);
945 #ifndef BT_DISABLE_STACK_TEMP_MEMORY 950 #endif //BT_DISABLE_STACK_TEMP_MEMORY 968 }
while(stack.
size()>0);
1001 }
while(stack.
size()>0);
1011 unsigned int signs[3],
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};
1088 #ifndef BT_DISABLE_STACK_TEMP_MEMORY 1090 #else//BT_DISABLE_STACK_TEMP_MEMORY 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);
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;
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);
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]);
1296 }
while(stack.
size());
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
DBVT_VIRTUAL void Process(const btDbvtNode *, const btDbvtNode *)
static DBVT_PREFIX void collideKDOP(const btDbvtNode *root, const btVector3 *normals, const btScalar *offsets, int count, DBVT_IPOLICY)
DBVT_INLINE void Merge(const btDbvtAabbMm &a, const btDbvtAabbMm &b, btDbvtAabbMm &r)
void push_back(const T &_Val)
DBVT_INLINE const btVector3 & Mins() 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 ...
DBVT_INLINE btVector3 Center() const
btDbvtNode * insert(const btDbvtVolume &box, void *data)
static DBVT_INLINE int nearest(const int *i, const btDbvt::sStkNPS *a, btScalar v, int l, int h)
btAlignedObjectArray< const btDbvtNode * > btNodeStack
virtual void WriteNode(const btDbvtNode *, int index, int parent, int child0, int child1)=0
DBVT_INLINE btVector3 Lengths() const
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)
DBVT_INLINE int Select(const btDbvtAabbMm &o, const btDbvtAabbMm &a, const btDbvtAabbMm &b)
DBVT_INLINE void AddSpan(const btVector3 &d, btScalar &smi, btScalar &smx) const
static DBVT_INLINE int allocate(btAlignedObjectArray< int > &ifree, btAlignedObjectArray< sStkNPS > &stock, const sStkNPS &value)
DBVT_INLINE btVector3 Extents() const
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
void write(IWriter *iwriter) 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 const btVector3 & Maxs() const
void update(btDbvtNode *leaf, int lookahead=-1)
void setX(btScalar _x)
Set the x value.
static int countLeaves(const btDbvtNode *node)
void optimizeIncremental(int passes)
static btDbvtAabbMm FromCE(const btVector3 &c, const btVector3 &e)
DBVT_VIRTUAL bool Descent(const btDbvtNode *)
const btScalar & x() const
Return the x value.
static int maxdepth(const btDbvtNode *node)
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
btScalar dot(const btVector3 &v) const
Return the dot product.
sStkNN(const btDbvtNode *na, const btDbvtNode *nb)
void setY(btScalar _y)
Set the y value.
const btScalar & y() const
Return the y value.
const btScalar & z() const
Return the z value.
void initializeFromBuffer(void *buffer, int size, int capacity)
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
sStkNP(const btDbvtNode *n, unsigned m)
DBVT_INLINE void Expand(const btVector3 &e)
static btDbvtAabbMm FromCR(const btVector3 &c, btScalar r)
#define DBVT_VIRTUAL_DTOR(a)
btAlignedObjectArray< sStkNN > m_stkStack
void optimizeTopDown(int bu_treshold=128)
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)
int size() const
return the number of elements in the array
DBVT_INLINE bool isleaf() const
virtual void Prepare(const btDbvtNode *root, int numnodes)=0
DBVT_INLINE int Classify(const btVector3 &n, btScalar o, int s) const
void resize(int newsize, const T &fillData=T())
DBVT_INLINE friend int Select(const btDbvtAabbMm &o, const btDbvtAabbMm &a, const btDbvtAabbMm &b)
DBVT_INLINE btScalar Proximity(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
DBVT_INLINE bool isinternal() const
virtual void WriteLeaf(const btDbvtNode *, int index, int parent)=0
DBVT_INLINE btScalar ProjectMinimum(const btVector3 &v, unsigned signs) const
DBVT_VIRTUAL void Process(const btDbvtNode *n, btScalar)
DBVT_INLINE bool Contain(const btDbvtAabbMm &a) const
DBVT_INLINE friend btScalar Proximity(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
DBVT_INLINE bool Intersect(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_PREFIX void collideTVNoStackAlloc(const btDbvtNode *root, const btDbvtVolume &volume, btNodeStack &stack, DBVT_IPOLICY) const
void clone(btDbvt &dest, IClone *iclone=0) const
static DBVT_PREFIX void enumNodes(const btDbvtNode *root, DBVT_IPOLICY)
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)
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()
static void extractLeaves(const btDbvtNode *node, btAlignedObjectArray< const btDbvtNode *> &leaves)
sStkNPS(const btDbvtNode *n, unsigned m, btScalar v)
DBVT_INLINE bool NotEqual(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
DBVT_INLINE friend bool NotEqual(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
btScalar btFabs(btScalar x)