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());
1202 for(
int k=stack.
size()-1;k>j;--k)
1204 stack[k]=stack[k-1];
1207 stack[j]=allocate(ifree,stock,nes[q]);
1209 j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.
size());
1214 for(
int k=stack.
size()-1;k>j;--k) stack[k]=stack[k-1];
1216 stack[j]=allocate(ifree,stock,nes[1-q]);
1220 stack.
push_back(allocate(ifree,stock,nes[q]));
1221 stack.
push_back(allocate(ifree,stock,nes[1-q]));
1229 }
while(stack.
size());
1242 stack.
reserve(SIMPLE_STACKSIZE);
1247 if(policy.Descent(n))
1252 { policy.Process(n); }
1254 }
while(stack.
size()>0);
1262 #undef DBVT_USE_MEMMOVE 1263 #undef DBVT_USE_TEMPLATE 1264 #undef DBVT_VIRTUAL_DTOR 1268 #undef DBVT_CHECKTYPE 1269 #undef DBVT_IMPL_GENERIC 1270 #undef DBVT_IMPL_SSE 1271 #undef DBVT_USE_INTRINSIC_SSE 1272 #undef DBVT_SELECT_IMPL 1273 #undef DBVT_MERGE_IMPL 1274 #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.