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"   240                 virtual void            Prepare(
const btDbvtNode* root,
int numnodes)=0;
   241                 virtual void            WriteNode(
const btDbvtNode*,
int index,
int parent,
int child0,
int child1)=0;
   242                 virtual void            WriteLeaf(
const btDbvtNode*,
int index,
int parent)=0;
   253                 SIMPLE_STACKSIZE        =       64,
   254                 DOUBLE_STACKSIZE        =       SIMPLE_STACKSIZE*2
   273         bool                    empty()
 const { 
return(0==m_root); }
   274         void                    optimizeBottomUp();
   275         void                    optimizeTopDown(
int bu_treshold=128);
   276         void                    optimizeIncremental(
int passes);
   278         void                    update(
btDbvtNode* leaf,
int lookahead=-1);
   284         void                    write(
IWriter* iwriter) 
const;
   287         static int              countLeaves(
const btDbvtNode* node);
   289 #if DBVT_ENABLE_BENCHMARK   290         static void             benchmark();
   296                 static void             enumNodes(      
const btDbvtNode* root,
   299                 static void             enumLeaves(     
const btDbvtNode* root,
   307                 void            collideTTpersistentStack(       
const btDbvtNode* root0,
   342                                                                 unsigned int signs[3],
   349                 static void             collideKDOP(
const btDbvtNode* root,
   355                 static void             collideOCL(     
const btDbvtNode* root,
   363                 static void             collideTU(      
const btDbvtNode* root,
   372                         if(a[i[m]].value>=v) l=m+1; 
else h=m;
   382                 { i=ifree[ifree.
size()-1];ifree.
pop_back();stock[i]=value; }
   400         box.
mi=c-e;box.
mx=c+e;
   422         box.
mi=box.
mx=pts[0];
   435         box.
mi=box.
mx=*ppts[0];
   461         return( (
mi.
x()<=a.
mi.
x())&&
   492         if((
btDot(n,px)+o)<0)           
return(-1);
   493         if((
btDot(n,pi)+o)>=0)  
return(+1);
   502                 b[(signs>>1)&1]->y(),
   503                 b[(signs>>2)&1]->z());
   513                 { smi+=
mx[i]*d[i];smx+=
mi[i]*d[i]; }
   515                 { smi+=
mi[i]*d[i];smx+=
mx[i]*d[i]; }
   523 #if     DBVT_INT0_IMPL == DBVT_IMPL_SSE   524         const __m128    rt(_mm_or_ps(   _mm_cmplt_ps(_mm_load_ps(b.
mx),_mm_load_ps(a.
mi)),
   525                 _mm_cmplt_ps(_mm_load_ps(a.
mx),_mm_load_ps(b.
mi))));
   527         const __int32*  pu((
const __int32*)&rt);
   529     const int*  pu((
const int*)&rt);
   531         return((pu[0]|pu[1]|pu[2])==0);
   533         return( (a.
mi.
x()<=b.
mx.
x())&&
   548         return( (b.
x()>=a.
mi.
x())&&
   578 #if     DBVT_SELECT_IMPL == DBVT_IMPL_SSE   581         static ATTRIBUTE_ALIGNED16(
const unsigned __int32)      mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
   583     static ATTRIBUTE_ALIGNED16(
const unsigned int)      mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 };
   585 #if DBVT_USE_INTRINSIC_SSE   595         __m128  omi(_mm_load_ps(o.
mi));
   596         omi=_mm_add_ps(omi,_mm_load_ps(o.
mx));
   597         __m128  ami(_mm_load_ps(a.
mi));
   598         ami=_mm_add_ps(ami,_mm_load_ps(a.
mx));
   599         ami=_mm_sub_ps(ami,omi);
   600         ami=_mm_and_ps(ami,_mm_load_ps((
const float*)mask));
   601         __m128  bmi(_mm_load_ps(b.
mi));
   602         bmi=_mm_add_ps(bmi,_mm_load_ps(b.
mx));
   603         bmi=_mm_sub_ps(bmi,omi);
   604         bmi=_mm_and_ps(bmi,_mm_load_ps((
const float*)mask));
   605         __m128  t0(_mm_movehl_ps(ami,ami));
   606         ami=_mm_add_ps(ami,t0);
   607         ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
   608         __m128 t1(_mm_movehl_ps(bmi,bmi));
   609         bmi=_mm_add_ps(bmi,t1);
   610         bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
   613         tmp.ssereg = _mm_cmple_ss(bmi,ami);
   614         return tmp.ints[0]&1;
   657 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE   658         __m128  ami(_mm_load_ps(a.
mi));
   659         __m128  amx(_mm_load_ps(a.
mx));
   660         __m128  bmi(_mm_load_ps(b.
mi));
   661         __m128  bmx(_mm_load_ps(b.
mx));
   662         ami=_mm_min_ps(ami,bmi);
   663         amx=_mm_max_ps(amx,bmx);
   664         _mm_store_ps(r.
mi,ami);
   665         _mm_store_ps(r.
mx,amx);
   669                 if(a.
mi[i]<b.
mi[i]) r.
mi[i]=a.
mi[i]; 
else r.
mi[i]=b.
mi[i];
   670                 if(a.
mx[i]>b.
mx[i]) r.
mx[i]=a.
mx[i]; 
else r.
mx[i]=b.
mx[i];
   679         return( (a.
mi.
x()!=b.
mi.
x())||
   697                 policy.Process(root);
   700                 enumNodes(root->
childs[0],policy);
   701                 enumNodes(root->
childs[1],policy);
   713                         enumLeaves(root->
childs[0],policy);
   714                         enumLeaves(root->
childs[1],policy);
   718                         policy.Process(root);
   732                         int                                                             treshold=DOUBLE_STACKSIZE-4;
   734                         stkStack.
resize(DOUBLE_STACKSIZE);
   735                         stkStack[0]=
sStkNN(root0,root1);
   737                                 sStkNN  p=stkStack[--depth];
   741                                         treshold=stkStack.
size()-4;
   778                                                         policy.Process(p.
a,p.
b);
   797                         int                                                             treshold=DOUBLE_STACKSIZE-4;
   799                         m_stkStack.
resize(DOUBLE_STACKSIZE);
   800                         m_stkStack[0]=
sStkNN(root0,root1);
   802                                 sStkNN  p=m_stkStack[--depth];
   806                                         treshold=m_stkStack.
size()-4;
   843                                                         policy.Process(p.
a,p.
b);
   863                         int                                                             treshold=DOUBLE_STACKSIZE-4;
   865                         stkStack.
resize(DOUBLE_STACKSIZE);
   866                         stkStack[0]=
sStkNN(root0,root1);
   868                                 sStkNN  p=stkStack[--depth];
   874                                                 treshold=stkStack.
size()-4;
   900                                                         policy.Process(p.
a,p.
b);
   916         collideTT(root0,root1,xform,policy);
   932                         stack.
reserve(SIMPLE_STACKSIZE);
   949                         } 
while(stack.
size()>0);
   958                                                                 unsigned int signs[3],
   971                 int                                                             treshold=DOUBLE_STACKSIZE-2;
   973                 stack.
resize(DOUBLE_STACKSIZE);
   982                         unsigned int result1=
false;
   983                         result1 = 
btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
   991                                                 treshold=stack.
size()-2;
   993                                         stack[depth++]=node->
childs[0];
   994                                         stack[depth++]=node->
childs[1];
   998                                         policy.Process(node);
  1023                         unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
  1032                         int                                                             treshold=DOUBLE_STACKSIZE-2;
  1034                         stack.
resize(DOUBLE_STACKSIZE);
  1044                                 unsigned int result1 = 
btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
  1046 #ifdef COMPARE_BTRAY_AABB2  1050 #endif //TEST_BTRAY_AABB2  1059                                                         treshold=stack.
size()-2;
  1061                                                 stack[depth++]=node->
childs[0];
  1062                                                 stack[depth++]=node->
childs[1];
  1066                                                 policy.Process(node);
  1085                         const int                                               inside=(1<<count)-1;
  1087                         int                                                             signs[
sizeof(unsigned)*8];
  1088                         btAssert(count<
int (
sizeof(signs)/
sizeof(signs[0])));
  1089                         for(
int i=0;i<count;++i)
  1091                                 signs[i]=       ((normals[i].
x()>=0)?1:0)+
  1092                                         ((normals[i].
y()>=0)?2:0)+
  1093                                         ((normals[i].
z()>=0)?4:0);
  1095                         stack.
reserve(SIMPLE_STACKSIZE);
  1101                                 for(
int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
  1108                                                 case    -1:     out=
true;
break;
  1109                                                 case    +1:     se.
mask|=j;
break;
  1122                                                 if(policy.AllLeaves(se.
node)) enumLeaves(se.
node,policy);
  1125                         } 
while(stack.
size());
  1142                         const unsigned                                  srtsgns=(sortaxis[0]>=0?1:0)+
  1143                                 (sortaxis[1]>=0?2:0)+
  1144                                 (sortaxis[2]>=0?4:0);
  1145                         const int                                               inside=(1<<count)-1;
  1149                         int                                                             signs[
sizeof(unsigned)*8];
  1150                         btAssert(count<
int (
sizeof(signs)/
sizeof(signs[0])));
  1151                         for(
int i=0;i<count;++i)
  1153                                 signs[i]=       ((normals[i].
x()>=0)?1:0)+
  1154                                         ((normals[i].
y()>=0)?2:0)+
  1155                                         ((normals[i].
z()>=0)?4:0);
  1157                         stock.
reserve(SIMPLE_STACKSIZE);
  1158                         stack.
reserve(SIMPLE_STACKSIZE);
  1159                         ifree.
reserve(SIMPLE_STACKSIZE);
  1162                                 const int       id=stack[stack.
size()-1];
  1168                                         for(
int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
  1175                                                         case    -1:     out=
true;
break;
  1176                                                         case    +1:     se.
mask|=j;
break;
  1182                                 if(policy.Descent(se.
node))
  1194                                                         j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.
size());
  1199 #if DBVT_USE_MEMMOVE  1201                      int num_items_to_move = stack.
size()-1-j;
  1202                      if(num_items_to_move > 0)
  1203                         memmove(&stack[j+1],&stack[j],
sizeof(
int)*num_items_to_move);
  1206                      for(
int k=stack.
size()-1;k>j;--k) {
  1207                                                                 stack[k]=stack[k-1];
  1210                                                         stack[j]=allocate(ifree,stock,nes[q]);
  1212                                                         j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.
size());
  1214 #if DBVT_USE_MEMMOVE  1216                      int num_items_to_move = stack.
size()-1-j;
  1217                      if(num_items_to_move > 0)
  1218                         memmove(&stack[j+1],&stack[j],
sizeof(
int)*num_items_to_move);
  1221                      for(
int k=stack.
size()-1;k>j;--k) {
  1222                         stack[k]=stack[k-1];
  1225                                                         stack[j]=allocate(ifree,stock,nes[1-q]);
  1229                                                         stack.
push_back(allocate(ifree,stock,nes[q]));
  1230                                                         stack.
push_back(allocate(ifree,stock,nes[1-q]));
  1238                         } 
while(stack.
size());
  1251                         stack.
reserve(SIMPLE_STACKSIZE);
  1256                                 if(policy.Descent(n))
  1261                                         { policy.Process(n); }
  1263                         } 
while(stack.
size()>0);
  1271 #undef DBVT_USE_MEMMOVE  1272 #undef DBVT_USE_TEMPLATE  1273 #undef DBVT_VIRTUAL_DTOR  1277 #undef DBVT_CHECKTYPE  1278 #undef DBVT_IMPL_GENERIC  1279 #undef DBVT_IMPL_SSE  1280 #undef DBVT_USE_INTRINSIC_SSE  1281 #undef DBVT_SELECT_IMPL  1282 #undef DBVT_MERGE_IMPL  1283 #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)
btAlignedObjectArray< const btDbvtNode * > m_rayTestStack
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)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
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_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, DBVT_IPOLICY) const 
rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory ...
static DBVT_INLINE int nearest(const int *i, const btDbvt::sStkNPS *a, btScalar v, int l, int h)
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. 
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 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_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)
static btDbvtVolume bounds(const tNodeArray &leaves)
DBVT_INLINE friend bool NotEqual(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
btScalar btFabs(btScalar x)
const btScalar & z() const 
Return the z value.