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                 stack.
reserve(SIMPLE_STACKSIZE);
   962                 } 
while(stack.
size()>0);
   978                 stack.
reserve(SIMPLE_STACKSIZE);
   995                 } 
while(stack.
size()>0);
  1005                                                                 unsigned int signs[3],
  1019                 int                                                             treshold=DOUBLE_STACKSIZE-2;
  1020                 stack.
resize(DOUBLE_STACKSIZE);
  1029                         unsigned int result1=
false;
  1030                         result1 = 
btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
  1038                                                 treshold=stack.
size()-2;
  1040                                         stack[depth++]=node->
childs[0];
  1041                                         stack[depth++]=node->
childs[1];
  1045                                         policy.Process(node);
  1070                         unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
  1079                         int                                                             treshold=DOUBLE_STACKSIZE-2;
  1081                         stack.
resize(DOUBLE_STACKSIZE);
  1091                                 unsigned int result1 = 
btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
  1093 #ifdef COMPARE_BTRAY_AABB2  1097 #endif //TEST_BTRAY_AABB2  1106                                                         treshold=stack.
size()-2;
  1108                                                 stack[depth++]=node->
childs[0];
  1109                                                 stack[depth++]=node->
childs[1];
  1113                                                 policy.Process(node);
  1132                         const int                                               inside=(1<<count)-1;
  1134                         int                                                             signs[
sizeof(unsigned)*8];
  1135                         btAssert(count<
int (
sizeof(signs)/
sizeof(signs[0])));
  1136                         for(
int i=0;i<count;++i)
  1138                                 signs[i]=       ((normals[i].
x()>=0)?1:0)+
  1139                                         ((normals[i].
y()>=0)?2:0)+
  1140                                         ((normals[i].
z()>=0)?4:0);
  1142                         stack.
reserve(SIMPLE_STACKSIZE);
  1148                                 for(
int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
  1155                                                 case    -1:     out=
true;
break;
  1156                                                 case    +1:     se.
mask|=j;
break;
  1169                                                 if(policy.AllLeaves(se.
node)) enumLeaves(se.
node,policy);
  1172                         } 
while(stack.
size());
  1189                         const unsigned                                  srtsgns=(sortaxis[0]>=0?1:0)+
  1190                                 (sortaxis[1]>=0?2:0)+
  1191                                 (sortaxis[2]>=0?4:0);
  1192                         const int                                               inside=(1<<count)-1;
  1196                         int                                                             signs[
sizeof(unsigned)*8];
  1197                         btAssert(count<
int (
sizeof(signs)/
sizeof(signs[0])));
  1198                         for(
int i=0;i<count;++i)
  1200                                 signs[i]=       ((normals[i].
x()>=0)?1:0)+
  1201                                         ((normals[i].
y()>=0)?2:0)+
  1202                                         ((normals[i].
z()>=0)?4:0);
  1204                         stock.
reserve(SIMPLE_STACKSIZE);
  1205                         stack.
reserve(SIMPLE_STACKSIZE);
  1206                         ifree.
reserve(SIMPLE_STACKSIZE);
  1209                                 const int       id=stack[stack.
size()-1];
  1215                                         for(
int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
  1222                                                         case    -1:     out=
true;
break;
  1223                                                         case    +1:     se.
mask|=j;
break;
  1229                                 if(policy.Descent(se.
node))
  1241                                                         j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.
size());
  1246 #if DBVT_USE_MEMMOVE  1248                      int num_items_to_move = stack.
size()-1-j;
  1249                      if(num_items_to_move > 0)
  1250                         memmove(&stack[j+1],&stack[j],
sizeof(
int)*num_items_to_move);
  1253                      for(
int k=stack.
size()-1;k>j;--k) {
  1254                                                                 stack[k]=stack[k-1];
  1257                                                         stack[j]=allocate(ifree,stock,nes[q]);
  1259                                                         j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.
size());
  1261 #if DBVT_USE_MEMMOVE  1263                      int num_items_to_move = stack.
size()-1-j;
  1264                      if(num_items_to_move > 0)
  1265                         memmove(&stack[j+1],&stack[j],
sizeof(
int)*num_items_to_move);
  1268                      for(
int k=stack.
size()-1;k>j;--k) {
  1269                         stack[k]=stack[k-1];
  1272                                                         stack[j]=allocate(ifree,stock,nes[1-q]);
  1276                                                         stack.
push_back(allocate(ifree,stock,nes[q]));
  1277                                                         stack.
push_back(allocate(ifree,stock,nes[1-q]));
  1285                         } 
while(stack.
size());
  1298                         stack.
reserve(SIMPLE_STACKSIZE);
  1303                                 if(policy.Descent(n))
  1308                                         { policy.Process(n); }
  1310                         } 
while(stack.
size()>0);
  1318 #undef DBVT_USE_MEMMOVE  1319 #undef DBVT_USE_TEMPLATE  1320 #undef DBVT_VIRTUAL_DTOR  1324 #undef DBVT_CHECKTYPE  1325 #undef DBVT_IMPL_GENERIC  1326 #undef DBVT_IMPL_SSE  1327 #undef DBVT_USE_INTRINSIC_SSE  1328 #undef DBVT_SELECT_IMPL  1329 #undef DBVT_MERGE_IMPL  1330 #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 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
static DBVT_INLINE int nearest(const int *i, const btDbvt::sStkNPS *a, btScalar v, int l, int h)
btAlignedObjectArray< const btDbvtNode * > btNodeStack
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 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. 
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 setX(btScalar _x)
Set the x value. 
static btDbvtAabbMm FromCE(const btVector3 &c, const btVector3 &e)
DBVT_VIRTUAL bool Descent(const btDbvtNode *)
const btScalar & x() const
Return the x value. 
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. 
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
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
DBVT_INLINE int Classify(const btVector3 &n, btScalar o, int s) 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_INLINE bool isinternal() const
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)
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
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()
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)