/* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ ///btDbvt implementation by Nathanael Presson #include "btDbvt.h" // typedef btAlignedObjectArray tNodeArray; typedef btAlignedObjectArray tConstNodeArray; // struct btDbvtNodeEnumerator : btDbvt::ICollide { tConstNodeArray nodes; void Process(const btDbvtNode* n) { nodes.push_back(n); } }; // static DBVT_INLINE int indexof(const btDbvtNode* node) { return(node->parent->childs[1]==node); } // static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, const btDbvtVolume& b) { #if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE) ATTRIBUTE_ALIGNED16( char locals[sizeof(btDbvtAabbMm)]); btDbvtVolume* ptr = (btDbvtVolume*) locals; btDbvtVolume& res=*ptr; #else btDbvtVolume res; #endif Merge(a,b,res); return(res); } // volume+edge lengths static DBVT_INLINE btScalar size(const btDbvtVolume& a) { const btVector3 edges=a.Lengths(); return( edges.x()*edges.y()*edges.z()+ edges.x()+edges.y()+edges.z()); } // static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth) { if(node->isinternal()) { getmaxdepth(node->childs[0],depth+1,maxdepth); getmaxdepth(node->childs[1],depth+1,maxdepth); } else maxdepth=btMax(maxdepth,depth); } // static DBVT_INLINE void deletenode( btDbvt* pdbvt, btDbvtNode* node) { btAlignedFree(pdbvt->m_free); pdbvt->m_free=node; } // static void recursedeletenode( btDbvt* pdbvt, btDbvtNode* node) { if(!node->isleaf()) { recursedeletenode(pdbvt,node->childs[0]); recursedeletenode(pdbvt,node->childs[1]); } if(node==pdbvt->m_root) pdbvt->m_root=0; deletenode(pdbvt,node); } // static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, btDbvtNode* parent, void* data) { btDbvtNode* node; if(pdbvt->m_free) { node=pdbvt->m_free;pdbvt->m_free=0; } else { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); } node->parent = parent; node->data = data; node->childs[1] = 0; return(node); } // static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, btDbvtNode* parent, const btDbvtVolume& volume, void* data) { btDbvtNode* node=createnode(pdbvt,parent,data); node->volume=volume; return(node); } // static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, btDbvtNode* parent, const btDbvtVolume& volume0, const btDbvtVolume& volume1, void* data) { btDbvtNode* node=createnode(pdbvt,parent,data); Merge(volume0,volume1,node->volume); return(node); } // static void insertleaf( btDbvt* pdbvt, btDbvtNode* root, btDbvtNode* leaf) { if(!pdbvt->m_root) { pdbvt->m_root = leaf; leaf->parent = 0; } else { if(!root->isleaf()) { do { root=root->childs[Select( leaf->volume, root->childs[0]->volume, root->childs[1]->volume)]; } while(!root->isleaf()); } btDbvtNode* prev=root->parent; btDbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0); if(prev) { prev->childs[indexof(root)] = node; node->childs[0] = root;root->parent=node; node->childs[1] = leaf;leaf->parent=node; do { if(!prev->volume.Contain(node->volume)) Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); else break; node=prev; } while(0!=(prev=node->parent)); } else { node->childs[0] = root;root->parent=node; node->childs[1] = leaf;leaf->parent=node; pdbvt->m_root = node; } } } // static btDbvtNode* removeleaf( btDbvt* pdbvt, btDbvtNode* leaf) { if(leaf==pdbvt->m_root) { pdbvt->m_root=0; return(0); } else { btDbvtNode* parent=leaf->parent; btDbvtNode* prev=parent->parent; btDbvtNode* sibling=parent->childs[1-indexof(leaf)]; if(prev) { prev->childs[indexof(parent)]=sibling; sibling->parent=prev; deletenode(pdbvt,parent); while(prev) { const btDbvtVolume pb=prev->volume; Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); if(NotEqual(pb,prev->volume)) { prev=prev->parent; } else break; } return(prev?prev:pdbvt->m_root); } else { pdbvt->m_root=sibling; sibling->parent=0; deletenode(pdbvt,parent); return(pdbvt->m_root); } } } // static void fetchleaves(btDbvt* pdbvt, btDbvtNode* root, tNodeArray& leaves, int depth=-1) { if(root->isinternal()&&depth) { fetchleaves(pdbvt,root->childs[0],leaves,depth-1); fetchleaves(pdbvt,root->childs[1],leaves,depth-1); deletenode(pdbvt,root); } else { leaves.push_back(root); } } // static bool leftOfAxis( const btDbvtNode* node, const btVector3& org, const btVector3& axis) { return btDot(axis, node->volume.Center() - org) <= 0; } // Partitions leaves such that leaves[0, n) are on the // left of axis, and leaves[n, count) are on the right // of axis. returns N. static int split( btDbvtNode** leaves, int count, const btVector3& org, const btVector3& axis) { int begin=0; int end=count; for(;;) { while(begin!=end && leftOfAxis(leaves[begin],org,axis)) { ++begin; } if(begin==end) { break; } while(begin!=end && !leftOfAxis(leaves[end-1],org,axis)) { --end; } if(begin==end) { break; } // swap out of place nodes --end; btDbvtNode* temp=leaves[begin]; leaves[begin]=leaves[end]; leaves[end]=temp; ++begin; } return begin; } // static btDbvtVolume bounds( btDbvtNode** leaves, int count) { #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); btDbvtVolume* ptr = (btDbvtVolume*) locals; btDbvtVolume& volume=*ptr; volume=leaves[0]->volume; #else btDbvtVolume volume=leaves[0]->volume; #endif for(int i=1,ni=count;ivolume,volume); } return(volume); } // static void bottomup( btDbvt* pdbvt, btDbvtNode** leaves, int count) { while(count>1) { btScalar minsize=SIMD_INFINITY; int minidx[2]={-1,-1}; for(int i=0;ivolume,leaves[j]->volume)); if(szvolume,n[1]->volume,0); p->childs[0] = n[0]; p->childs[1] = n[1]; n[0]->parent = p; n[1]->parent = p; leaves[minidx[0]] = p; leaves[minidx[1]] = leaves[count-1]; --count; } } // static btDbvtNode* topdown(btDbvt* pdbvt, btDbvtNode** leaves, int count, int bu_treshold) { static const btVector3 axis[]={btVector3(1,0,0), btVector3(0,1,0), btVector3(0,0,1)}; btAssert(bu_treshold>2); if(count>1) { if(count>bu_treshold) { const btDbvtVolume vol=bounds(leaves,count); const btVector3 org=vol.Center(); int partition; int bestaxis=-1; int bestmidp=count; int splitcount[3][2]={{0,0},{0,0},{0,0}}; int i; for( i=0;ivolume.Center()-org; for(int j=0;j<3;++j) { ++splitcount[j][btDot(x,axis[j])>0?1:0]; } } for( i=0;i<3;++i) { if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) { const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1])); if(midp=0) { partition=split(leaves,count,org,axis[bestaxis]); btAssert(partition!=0 && partition!=count); } else { partition=count/2+1; } btDbvtNode* node=createnode(pdbvt,0,vol,0); node->childs[0]=topdown(pdbvt,&leaves[0],partition,bu_treshold); node->childs[1]=topdown(pdbvt,&leaves[partition],count-partition,bu_treshold); node->childs[0]->parent=node; node->childs[1]->parent=node; return(node); } else { bottomup(pdbvt,leaves,count); return(leaves[0]); } } return(leaves[0]); } // static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r) { btDbvtNode* p=n->parent; btAssert(n->isinternal()); if(p>n) { const int i=indexof(n); const int j=1-i; btDbvtNode* s=p->childs[j]; btDbvtNode* q=p->parent; btAssert(n==p->childs[i]); if(q) q->childs[indexof(p)]=n; else r=n; s->parent=n; p->parent=n; n->parent=q; p->childs[0]=n->childs[0]; p->childs[1]=n->childs[1]; n->childs[0]->parent=p; n->childs[1]->parent=p; n->childs[i]=p; n->childs[j]=s; btSwap(p->volume,n->volume); return(p); } return(n); } #if 0 static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count) { while(n&&(count--)) n=n->parent; return(n); } #endif // // Api // // btDbvt::btDbvt() { m_root = 0; m_free = 0; m_lkhd = -1; m_leaves = 0; m_opath = 0; } // btDbvt::~btDbvt() { clear(); } // void btDbvt::clear() { if(m_root) recursedeletenode(this,m_root); btAlignedFree(m_free); m_free=0; m_lkhd = -1; m_stkStack.clear(); m_opath = 0; } // void btDbvt::optimizeBottomUp() { if(m_root) { tNodeArray leaves; leaves.reserve(m_leaves); fetchleaves(this,m_root,leaves); bottomup(this,&leaves[0],leaves.size()); m_root=leaves[0]; } } // void btDbvt::optimizeTopDown(int bu_treshold) { if(m_root) { tNodeArray leaves; leaves.reserve(m_leaves); fetchleaves(this,m_root,leaves); m_root=topdown(this,&leaves[0],leaves.size(),bu_treshold); } } // void btDbvt::optimizeIncremental(int passes) { if(passes<0) passes=m_leaves; if(m_root&&(passes>0)) { do { btDbvtNode* node=m_root; unsigned bit=0; while(node->isinternal()) { node=sort(node,m_root)->childs[(m_opath>>bit)&1]; bit=(bit+1)&(sizeof(unsigned)*8-1); } update(node); ++m_opath; } while(--passes); } } // btDbvtNode* btDbvt::insert(const btDbvtVolume& volume,void* data) { btDbvtNode* leaf=createnode(this,0,volume,data); insertleaf(this,m_root,leaf); ++m_leaves; return(leaf); } // void btDbvt::update(btDbvtNode* leaf,int lookahead) { btDbvtNode* root=removeleaf(this,leaf); if(root) { if(lookahead>=0) { for(int i=0;(iparent;++i) { root=root->parent; } } else root=m_root; } insertleaf(this,root,leaf); } // void btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume) { btDbvtNode* root=removeleaf(this,leaf); if(root) { if(m_lkhd>=0) { for(int i=0;(iparent;++i) { root=root->parent; } } else root=m_root; } leaf->volume=volume; insertleaf(this,root,leaf); } // bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin) { if(leaf->volume.Contain(volume)) return(false); volume.Expand(btVector3(margin,margin,margin)); volume.SignedExpand(velocity); update(leaf,volume); return(true); } // bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity) { if(leaf->volume.Contain(volume)) return(false); volume.SignedExpand(velocity); update(leaf,volume); return(true); } // bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin) { if(leaf->volume.Contain(volume)) return(false); volume.Expand(btVector3(margin,margin,margin)); update(leaf,volume); return(true); } // void btDbvt::remove(btDbvtNode* leaf) { removeleaf(this,leaf); deletenode(this,leaf); --m_leaves; } // void btDbvt::write(IWriter* iwriter) const { btDbvtNodeEnumerator nodes; nodes.nodes.reserve(m_leaves*2); enumNodes(m_root,nodes); iwriter->Prepare(m_root,nodes.nodes.size()); for(int i=0;iparent) p=nodes.nodes.findLinearSearch(n->parent); if(n->isinternal()) { const int c0=nodes.nodes.findLinearSearch(n->childs[0]); const int c1=nodes.nodes.findLinearSearch(n->childs[1]); iwriter->WriteNode(n,i,p,c0,c1); } else { iwriter->WriteLeaf(n,i,p); } } } // void btDbvt::clone(btDbvt& dest,IClone* iclone) const { dest.clear(); if(m_root!=0) { btAlignedObjectArray stack; stack.reserve(m_leaves); stack.push_back(sStkCLN(m_root,0)); do { const int i=stack.size()-1; const sStkCLN e=stack[i]; btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data); stack.pop_back(); if(e.parent!=0) e.parent->childs[i&1]=n; else dest.m_root=n; if(e.node->isinternal()) { stack.push_back(sStkCLN(e.node->childs[0],n)); stack.push_back(sStkCLN(e.node->childs[1],n)); } else { iclone->CloneLeaf(n); } } while(stack.size()>0); } } // int btDbvt::maxdepth(const btDbvtNode* node) { int depth=0; if(node) getmaxdepth(node,1,depth); return(depth); } // int btDbvt::countLeaves(const btDbvtNode* node) { if(node->isinternal()) return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); else return(1); } // void btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves) { if(node->isinternal()) { extractLeaves(node->childs[0],leaves); extractLeaves(node->childs[1],leaves); } else { leaves.push_back(node); } } // #if DBVT_ENABLE_BENCHMARK #include #include #include "LinearMath/btQuickProf.h" /* q6600,2.4ghz /Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32" /GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch" /Fo"..\..\out\release8\build\libbulletcollision\\" /Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb" /W3 /nologo /c /Wp64 /Zi /errorReport:prompt Benchmarking dbvt... World scale: 100.000000 Extents base: 1.000000 Extents range: 4.000000 Leaves: 8192 sizeof(btDbvtVolume): 32 bytes sizeof(btDbvtNode): 44 bytes [1] btDbvtVolume intersections: 3499 ms (-1%) [2] btDbvtVolume merges: 1934 ms (0%) [3] btDbvt::collideTT: 5485 ms (-21%) [4] btDbvt::collideTT self: 2814 ms (-20%) [5] btDbvt::collideTT xform: 7379 ms (-1%) [6] btDbvt::collideTT xform,self: 7270 ms (-2%) [7] btDbvt::rayTest: 6314 ms (0%),(332143 r/s) [8] insert/remove: 2093 ms (0%),(1001983 ir/s) [9] updates (teleport): 1879 ms (-3%),(1116100 u/s) [10] updates (jitter): 1244 ms (-4%),(1685813 u/s) [11] optimize (incremental): 2514 ms (0%),(1668000 o/s) [12] btDbvtVolume notequal: 3659 ms (0%) [13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s) [14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s) [15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s) [16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s) [17] btDbvtVolume select: 3419 ms (0%) */ struct btDbvtBenchmark { struct NilPolicy : btDbvt::ICollide { NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {} void Process(const btDbvtNode*,const btDbvtNode*) { ++m_pcount; } void Process(const btDbvtNode*) { ++m_pcount; } void Process(const btDbvtNode*,btScalar depth) { ++m_pcount; if(m_checksort) { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); } } int m_pcount; btScalar m_depth; bool m_checksort; }; struct P14 : btDbvt::ICollide { struct Node { const btDbvtNode* leaf; btScalar depth; }; void Process(const btDbvtNode* leaf,btScalar depth) { Node n; n.leaf = leaf; n.depth = depth; } static int sortfnc(const Node& a,const Node& b) { if(a.depthb.depth) return(-1); return(0); } btAlignedObjectArray m_nodes; }; struct P15 : btDbvt::ICollide { struct Node { const btDbvtNode* leaf; btScalar depth; }; void Process(const btDbvtNode* leaf) { Node n; n.leaf = leaf; n.depth = dot(leaf->volume.Center(),m_axis); } static int sortfnc(const Node& a,const Node& b) { if(a.depthb.depth) return(-1); return(0); } btAlignedObjectArray m_nodes; btVector3 m_axis; }; static btScalar RandUnit() { return(rand()/(btScalar)RAND_MAX); } static btVector3 RandVector3() { return(btVector3(RandUnit(),RandUnit(),RandUnit())); } static btVector3 RandVector3(btScalar cs) { return(RandVector3()*cs-btVector3(cs,cs,cs)/2); } static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es) { return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es)); } static btTransform RandTransform(btScalar cs) { btTransform t; t.setOrigin(RandVector3(cs)); t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized()); return(t); } static void RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt) { dbvt.clear(); for(int i=0;i volumes; btAlignedObjectArray results; volumes.resize(cfgLeaves); results.resize(cfgLeaves); for(int i=0;i volumes; btAlignedObjectArray results; volumes.resize(cfgLeaves); results.resize(cfgLeaves); for(int i=0;i transforms; btDbvtBenchmark::NilPolicy policy; transforms.resize(cfgBenchmark5_Iterations); for(int i=0;i transforms; btDbvtBenchmark::NilPolicy policy; transforms.resize(cfgBenchmark6_Iterations); for(int i=0;i rayorg; btAlignedObjectArray raydir; btDbvtBenchmark::NilPolicy policy; rayorg.resize(cfgBenchmark7_Iterations); raydir.resize(cfgBenchmark7_Iterations); for(int i=0;i leaves; btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); dbvt.optimizeTopDown(); dbvt.extractLeaves(dbvt.m_root,leaves); printf("[9] updates (teleport): "); wallclock.reset(); for(int i=0;i(leaves[rand()%cfgLeaves]), btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale)); } } const int time=(int)wallclock.getTimeMilliseconds(); const int up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations; printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time); } if(cfgBenchmark10_Enable) {// Benchmark 10 srand(380843); btDbvt dbvt; btAlignedObjectArray leaves; btAlignedObjectArray vectors; vectors.resize(cfgBenchmark10_Iterations); for(int i=0;i(leaves[rand()%cfgLeaves]); btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d); dbvt.update(l,v); } } const int time=(int)wallclock.getTimeMilliseconds(); const int up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations; printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time); } if(cfgBenchmark11_Enable) {// Benchmark 11 srand(380843); btDbvt dbvt; btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); dbvt.optimizeTopDown(); printf("[11] optimize (incremental): "); wallclock.reset(); for(int i=0;i volumes; btAlignedObjectArray results; volumes.resize(cfgLeaves); results.resize(cfgLeaves); for(int i=0;i vectors; btDbvtBenchmark::NilPolicy policy; vectors.resize(cfgBenchmark13_Iterations); for(int i=0;i vectors; btDbvtBenchmark::P14 policy; vectors.resize(cfgBenchmark14_Iterations); for(int i=0;i vectors; btDbvtBenchmark::P15 policy; vectors.resize(cfgBenchmark15_Iterations); for(int i=0;i batch; btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); dbvt.optimizeTopDown(); batch.reserve(cfgBenchmark16_BatchCount); printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); wallclock.reset(); for(int i=0;i volumes; btAlignedObjectArray results; btAlignedObjectArray indices; volumes.resize(cfgLeaves); results.resize(cfgLeaves); indices.resize(cfgLeaves); for(int i=0;i