diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision/BroadPhaseCollision')
9 files changed, 4887 insertions, 0 deletions
diff --git a/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h new file mode 100644 index 0000000000..1bc56cf80a --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h @@ -0,0 +1,40 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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. +*/ + +#ifndef B3_BROADPHASE_CALLBACK_H +#define B3_BROADPHASE_CALLBACK_H + +#include "Bullet3Common/b3Vector3.h" +struct b3BroadphaseProxy; + + +struct b3BroadphaseAabbCallback +{ + virtual ~b3BroadphaseAabbCallback() {} + virtual bool process(const b3BroadphaseProxy* proxy) = 0; +}; + + +struct b3BroadphaseRayCallback : public b3BroadphaseAabbCallback +{ + ///added some cached data to accelerate ray-AABB tests + b3Vector3 m_rayDirectionInverse; + unsigned int m_signs[3]; + b3Scalar m_lambda_max; + + virtual ~b3BroadphaseRayCallback() {} +}; + +#endif //B3_BROADPHASE_CALLBACK_H diff --git a/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp new file mode 100644 index 0000000000..0f04efe331 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp @@ -0,0 +1,1325 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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. +*/ +///b3DynamicBvh implementation by Nathanael Presson + +#include "b3DynamicBvh.h" + +// +typedef b3AlignedObjectArray<b3DbvtNode*> b3NodeArray; +typedef b3AlignedObjectArray<const b3DbvtNode*> b3ConstNodeArray; + +// +struct b3DbvtNodeEnumerator : b3DynamicBvh::ICollide +{ + b3ConstNodeArray nodes; + void Process(const b3DbvtNode* n) { nodes.push_back(n); } +}; + +// +static B3_DBVT_INLINE int b3IndexOf(const b3DbvtNode* node) +{ + return(node->parent->childs[1]==node); +} + +// +static B3_DBVT_INLINE b3DbvtVolume b3Merge( const b3DbvtVolume& a, + const b3DbvtVolume& b) +{ +#if (B3_DBVT_MERGE_IMPL==B3_DBVT_IMPL_SSE) + B3_ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtAabbMm)]); + b3DbvtVolume& res=*(b3DbvtVolume*)locals; +#else + b3DbvtVolume res; +#endif + b3Merge(a,b,res); + return(res); +} + +// volume+edge lengths +static B3_DBVT_INLINE b3Scalar b3Size(const b3DbvtVolume& a) +{ + const b3Vector3 edges=a.Lengths(); + return( edges.x*edges.y*edges.z+ + edges.x+edges.y+edges.z); +} + +// +static void b3GetMaxDepth(const b3DbvtNode* node,int depth,int& maxdepth) +{ + if(node->isinternal()) + { + b3GetMaxDepth(node->childs[0],depth+1,maxdepth); + b3GetMaxDepth(node->childs[1],depth+1,maxdepth); + } else maxdepth=b3Max(maxdepth,depth); +} + +// +static B3_DBVT_INLINE void b3DeleteNode( b3DynamicBvh* pdbvt, + b3DbvtNode* node) +{ + b3AlignedFree(pdbvt->m_free); + pdbvt->m_free=node; +} + +// +static void b3RecurseDeleteNode( b3DynamicBvh* pdbvt, + b3DbvtNode* node) +{ + if(!node->isleaf()) + { + b3RecurseDeleteNode(pdbvt,node->childs[0]); + b3RecurseDeleteNode(pdbvt,node->childs[1]); + } + if(node==pdbvt->m_root) pdbvt->m_root=0; + b3DeleteNode(pdbvt,node); +} + +// +static B3_DBVT_INLINE b3DbvtNode* b3CreateNode( b3DynamicBvh* pdbvt, + b3DbvtNode* parent, + void* data) +{ + b3DbvtNode* node; + if(pdbvt->m_free) + { node=pdbvt->m_free;pdbvt->m_free=0; } + else + { node=new(b3AlignedAlloc(sizeof(b3DbvtNode),16)) b3DbvtNode(); } + node->parent = parent; + node->data = data; + node->childs[1] = 0; + return(node); +} + +// +static B3_DBVT_INLINE b3DbvtNode* b3CreateNode( b3DynamicBvh* pdbvt, + b3DbvtNode* parent, + const b3DbvtVolume& volume, + void* data) +{ + b3DbvtNode* node=b3CreateNode(pdbvt,parent,data); + node->volume=volume; + return(node); +} + +// +static B3_DBVT_INLINE b3DbvtNode* b3CreateNode( b3DynamicBvh* pdbvt, + b3DbvtNode* parent, + const b3DbvtVolume& volume0, + const b3DbvtVolume& volume1, + void* data) +{ + b3DbvtNode* node=b3CreateNode(pdbvt,parent,data); + b3Merge(volume0,volume1,node->volume); + return(node); +} + +// +static void b3InsertLeaf( b3DynamicBvh* pdbvt, + b3DbvtNode* root, + b3DbvtNode* leaf) +{ + if(!pdbvt->m_root) + { + pdbvt->m_root = leaf; + leaf->parent = 0; + } + else + { + if(!root->isleaf()) + { + do { + root=root->childs[b3Select( leaf->volume, + root->childs[0]->volume, + root->childs[1]->volume)]; + } while(!root->isleaf()); + } + b3DbvtNode* prev=root->parent; + b3DbvtNode* node=b3CreateNode(pdbvt,prev,leaf->volume,root->volume,0); + if(prev) + { + prev->childs[b3IndexOf(root)] = node; + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + do { + if(!prev->volume.Contain(node->volume)) + b3Merge(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 b3DbvtNode* b3RemoveLeaf( b3DynamicBvh* pdbvt, + b3DbvtNode* leaf) +{ + if(leaf==pdbvt->m_root) + { + pdbvt->m_root=0; + return(0); + } + else + { + b3DbvtNode* parent=leaf->parent; + b3DbvtNode* prev=parent->parent; + b3DbvtNode* sibling=parent->childs[1-b3IndexOf(leaf)]; + if(prev) + { + prev->childs[b3IndexOf(parent)]=sibling; + sibling->parent=prev; + b3DeleteNode(pdbvt,parent); + while(prev) + { + const b3DbvtVolume pb=prev->volume; + b3Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + if(b3NotEqual(pb,prev->volume)) + { + prev=prev->parent; + } else break; + } + return(prev?prev:pdbvt->m_root); + } + else + { + pdbvt->m_root=sibling; + sibling->parent=0; + b3DeleteNode(pdbvt,parent); + return(pdbvt->m_root); + } + } +} + +// +static void b3FetchLeaves(b3DynamicBvh* pdbvt, + b3DbvtNode* root, + b3NodeArray& leaves, + int depth=-1) +{ + if(root->isinternal()&&depth) + { + b3FetchLeaves(pdbvt,root->childs[0],leaves,depth-1); + b3FetchLeaves(pdbvt,root->childs[1],leaves,depth-1); + b3DeleteNode(pdbvt,root); + } + else + { + leaves.push_back(root); + } +} + +static bool b3LeftOfAxis( const b3DbvtNode* node, + const b3Vector3& org, + const b3Vector3& axis) +{ + return b3Dot(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 b3Split( b3DbvtNode** leaves, + int count, + const b3Vector3& org, + const b3Vector3& axis) +{ + int begin=0; + int end=count; + for(;;) + { + while(begin!=end && b3LeftOfAxis(leaves[begin],org,axis)) + { + ++begin; + } + + if(begin==end) + { + break; + } + + while(begin!=end && !b3LeftOfAxis(leaves[end-1],org,axis)) + { + --end; + } + + if(begin==end) + { + break; + } + + // swap out of place nodes + --end; + b3DbvtNode* temp=leaves[begin]; + leaves[begin]=leaves[end]; + leaves[end]=temp; + ++begin; + } + + return begin; +} + +// +static b3DbvtVolume b3Bounds( b3DbvtNode** leaves, + int count) +{ +#if B3_DBVT_MERGE_IMPL==B3_DBVT_IMPL_SSE + B3_ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtVolume)]); + b3DbvtVolume& volume=*(b3DbvtVolume*)locals; + volume=leaves[0]->volume; +#else + b3DbvtVolume volume=leaves[0]->volume; +#endif + for(int i=1,ni=count;i<ni;++i) + { + b3Merge(volume,leaves[i]->volume,volume); + } + return(volume); +} + +// +static void b3BottomUp( b3DynamicBvh* pdbvt, + b3DbvtNode** leaves, + int count) +{ + while(count>1) + { + b3Scalar minsize=B3_INFINITY; + int minidx[2]={-1,-1}; + for(int i=0;i<count;++i) + { + for(int j=i+1;j<count;++j) + { + const b3Scalar sz=b3Size(b3Merge(leaves[i]->volume,leaves[j]->volume)); + if(sz<minsize) + { + minsize = sz; + minidx[0] = i; + minidx[1] = j; + } + } + } + b3DbvtNode* n[] = {leaves[minidx[0]],leaves[minidx[1]]}; + b3DbvtNode* p = b3CreateNode(pdbvt,0,n[0]->volume,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 b3DbvtNode* b3TopDown(b3DynamicBvh* pdbvt, + b3DbvtNode** leaves, + int count, + int bu_treshold) +{ + static const b3Vector3 axis[]={b3MakeVector3(1,0,0), + b3MakeVector3(0,1,0), + b3MakeVector3(0,0,1)}; + b3Assert(bu_treshold>1); + if(count>1) + { + if(count>bu_treshold) + { + const b3DbvtVolume vol=b3Bounds(leaves,count); + const b3Vector3 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;i<count;++i) + { + const b3Vector3 x=leaves[i]->volume.Center()-org; + for(int j=0;j<3;++j) + { + ++splitcount[j][b3Dot(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)b3Fabs(b3Scalar(splitcount[i][0]-splitcount[i][1])); + if(midp<bestmidp) + { + bestaxis=i; + bestmidp=midp; + } + } + } + if(bestaxis>=0) + { + partition=b3Split(leaves,count,org,axis[bestaxis]); + b3Assert(partition!=0 && partition!=count); + } + else + { + partition=count/2+1; + } + b3DbvtNode* node=b3CreateNode(pdbvt,0,vol,0); + node->childs[0]=b3TopDown(pdbvt,&leaves[0],partition,bu_treshold); + node->childs[1]=b3TopDown(pdbvt,&leaves[partition],count-partition,bu_treshold); + node->childs[0]->parent=node; + node->childs[1]->parent=node; + return(node); + } + else + { + b3BottomUp(pdbvt,leaves,count); + return(leaves[0]); + } + } + return(leaves[0]); +} + +// +static B3_DBVT_INLINE b3DbvtNode* b3Sort(b3DbvtNode* n,b3DbvtNode*& r) +{ + b3DbvtNode* p=n->parent; + b3Assert(n->isinternal()); + if(p>n) + { + const int i=b3IndexOf(n); + const int j=1-i; + b3DbvtNode* s=p->childs[j]; + b3DbvtNode* q=p->parent; + b3Assert(n==p->childs[i]); + if(q) q->childs[b3IndexOf(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; + b3Swap(p->volume,n->volume); + return(p); + } + return(n); +} + +#if 0 +static B3_DBVT_INLINE b3DbvtNode* walkup(b3DbvtNode* n,int count) +{ + while(n&&(count--)) n=n->parent; + return(n); +} +#endif + +// +// Api +// + +// +b3DynamicBvh::b3DynamicBvh() +{ + m_root = 0; + m_free = 0; + m_lkhd = -1; + m_leaves = 0; + m_opath = 0; +} + +// +b3DynamicBvh::~b3DynamicBvh() +{ + clear(); +} + +// +void b3DynamicBvh::clear() +{ + if(m_root) + b3RecurseDeleteNode(this,m_root); + b3AlignedFree(m_free); + m_free=0; + m_lkhd = -1; + m_stkStack.clear(); + m_opath = 0; + +} + +// +void b3DynamicBvh::optimizeBottomUp() +{ + if(m_root) + { + b3NodeArray leaves; + leaves.reserve(m_leaves); + b3FetchLeaves(this,m_root,leaves); + b3BottomUp(this,&leaves[0],leaves.size()); + m_root=leaves[0]; + } +} + +// +void b3DynamicBvh::optimizeTopDown(int bu_treshold) +{ + if(m_root) + { + b3NodeArray leaves; + leaves.reserve(m_leaves); + b3FetchLeaves(this,m_root,leaves); + m_root=b3TopDown(this,&leaves[0],leaves.size(),bu_treshold); + } +} + +// +void b3DynamicBvh::optimizeIncremental(int passes) +{ + if(passes<0) passes=m_leaves; + if(m_root&&(passes>0)) + { + do { + b3DbvtNode* node=m_root; + unsigned bit=0; + while(node->isinternal()) + { + node=b3Sort(node,m_root)->childs[(m_opath>>bit)&1]; + bit=(bit+1)&(sizeof(unsigned)*8-1); + } + update(node); + ++m_opath; + } while(--passes); + } +} + +// +b3DbvtNode* b3DynamicBvh::insert(const b3DbvtVolume& volume,void* data) +{ + b3DbvtNode* leaf=b3CreateNode(this,0,volume,data); + b3InsertLeaf(this,m_root,leaf); + ++m_leaves; + return(leaf); +} + +// +void b3DynamicBvh::update(b3DbvtNode* leaf,int lookahead) +{ + b3DbvtNode* root=b3RemoveLeaf(this,leaf); + if(root) + { + if(lookahead>=0) + { + for(int i=0;(i<lookahead)&&root->parent;++i) + { + root=root->parent; + } + } else root=m_root; + } + b3InsertLeaf(this,root,leaf); +} + +// +void b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume) +{ + b3DbvtNode* root=b3RemoveLeaf(this,leaf); + if(root) + { + if(m_lkhd>=0) + { + for(int i=0;(i<m_lkhd)&&root->parent;++i) + { + root=root->parent; + } + } else root=m_root; + } + leaf->volume=volume; + b3InsertLeaf(this,root,leaf); +} + +// +bool b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity,b3Scalar margin) +{ + if(leaf->volume.Contain(volume)) return(false); + volume.Expand(b3MakeVector3(margin,margin,margin)); + volume.SignedExpand(velocity); + update(leaf,volume); + return(true); +} + +// +bool b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity) +{ + if(leaf->volume.Contain(volume)) return(false); + volume.SignedExpand(velocity); + update(leaf,volume); + return(true); +} + +// +bool b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume,b3Scalar margin) +{ + if(leaf->volume.Contain(volume)) return(false); + volume.Expand(b3MakeVector3(margin,margin,margin)); + update(leaf,volume); + return(true); +} + +// +void b3DynamicBvh::remove(b3DbvtNode* leaf) +{ + b3RemoveLeaf(this,leaf); + b3DeleteNode(this,leaf); + --m_leaves; +} + +// +void b3DynamicBvh::write(IWriter* iwriter) const +{ + b3DbvtNodeEnumerator nodes; + nodes.nodes.reserve(m_leaves*2); + enumNodes(m_root,nodes); + iwriter->Prepare(m_root,nodes.nodes.size()); + for(int i=0;i<nodes.nodes.size();++i) + { + const b3DbvtNode* n=nodes.nodes[i]; + int p=-1; + if(n->parent) 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 b3DynamicBvh::clone(b3DynamicBvh& dest,IClone* iclone) const +{ + dest.clear(); + if(m_root!=0) + { + b3AlignedObjectArray<sStkCLN> stack; + stack.reserve(m_leaves); + stack.push_back(sStkCLN(m_root,0)); + do { + const int i=stack.size()-1; + const sStkCLN e=stack[i]; + b3DbvtNode* n=b3CreateNode(&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 b3DynamicBvh::maxdepth(const b3DbvtNode* node) +{ + int depth=0; + if(node) b3GetMaxDepth(node,1,depth); + return(depth); +} + +// +int b3DynamicBvh::countLeaves(const b3DbvtNode* node) +{ + if(node->isinternal()) + return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); + else + return(1); +} + +// +void b3DynamicBvh::extractLeaves(const b3DbvtNode* node,b3AlignedObjectArray<const b3DbvtNode*>& leaves) +{ + if(node->isinternal()) + { + extractLeaves(node->childs[0],leaves); + extractLeaves(node->childs[1],leaves); + } + else + { + leaves.push_back(node); + } +} + +// +#if B3_DBVT_ENABLE_BENCHMARK + +#include <stdio.h> +#include <stdlib.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(b3DbvtVolume): 32 bytes +sizeof(b3DbvtNode): 44 bytes +[1] b3DbvtVolume intersections: 3499 ms (-1%) +[2] b3DbvtVolume merges: 1934 ms (0%) +[3] b3DynamicBvh::collideTT: 5485 ms (-21%) +[4] b3DynamicBvh::collideTT self: 2814 ms (-20%) +[5] b3DynamicBvh::collideTT xform: 7379 ms (-1%) +[6] b3DynamicBvh::collideTT xform,self: 7270 ms (-2%) +[7] b3DynamicBvh::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] b3DbvtVolume 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] b3DbvtVolume select: 3419 ms (0%) +*/ + +struct b3DbvtBenchmark +{ + struct NilPolicy : b3DynamicBvh::ICollide + { + NilPolicy() : m_pcount(0),m_depth(-B3_INFINITY),m_checksort(true) {} + void Process(const b3DbvtNode*,const b3DbvtNode*) { ++m_pcount; } + void Process(const b3DbvtNode*) { ++m_pcount; } + void Process(const b3DbvtNode*,b3Scalar 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; + b3Scalar m_depth; + bool m_checksort; + }; + struct P14 : b3DynamicBvh::ICollide + { + struct Node + { + const b3DbvtNode* leaf; + b3Scalar depth; + }; + void Process(const b3DbvtNode* leaf,b3Scalar depth) + { + Node n; + n.leaf = leaf; + n.depth = depth; + } + static int sortfnc(const Node& a,const Node& b) + { + if(a.depth<b.depth) return(+1); + if(a.depth>b.depth) return(-1); + return(0); + } + b3AlignedObjectArray<Node> m_nodes; + }; + struct P15 : b3DynamicBvh::ICollide + { + struct Node + { + const b3DbvtNode* leaf; + b3Scalar depth; + }; + void Process(const b3DbvtNode* 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.depth<b.depth) return(+1); + if(a.depth>b.depth) return(-1); + return(0); + } + b3AlignedObjectArray<Node> m_nodes; + b3Vector3 m_axis; + }; + static b3Scalar RandUnit() + { + return(rand()/(b3Scalar)RAND_MAX); + } + static b3Vector3 RandVector3() + { + return(b3Vector3(RandUnit(),RandUnit(),RandUnit())); + } + static b3Vector3 RandVector3(b3Scalar cs) + { + return(RandVector3()*cs-b3Vector3(cs,cs,cs)/2); + } + static b3DbvtVolume RandVolume(b3Scalar cs,b3Scalar eb,b3Scalar es) + { + return(b3DbvtVolume::FromCE(RandVector3(cs),b3Vector3(eb,eb,eb)+RandVector3()*es)); + } + static b3Transform RandTransform(b3Scalar cs) + { + b3Transform t; + t.setOrigin(RandVector3(cs)); + t.setRotation(b3Quaternion(RandUnit()*B3_PI*2,RandUnit()*B3_PI*2,RandUnit()*B3_PI*2).normalized()); + return(t); + } + static void RandTree(b3Scalar cs,b3Scalar eb,b3Scalar es,int leaves,b3DynamicBvh& dbvt) + { + dbvt.clear(); + for(int i=0;i<leaves;++i) + { + dbvt.insert(RandVolume(cs,eb,es),0); + } + } +}; + +void b3DynamicBvh::benchmark() +{ + static const b3Scalar cfgVolumeCenterScale = 100; + static const b3Scalar cfgVolumeExentsBase = 1; + static const b3Scalar cfgVolumeExentsScale = 4; + static const int cfgLeaves = 8192; + static const bool cfgEnable = true; + + //[1] b3DbvtVolume intersections + bool cfgBenchmark1_Enable = cfgEnable; + static const int cfgBenchmark1_Iterations = 8; + static const int cfgBenchmark1_Reference = 3499; + //[2] b3DbvtVolume merges + bool cfgBenchmark2_Enable = cfgEnable; + static const int cfgBenchmark2_Iterations = 4; + static const int cfgBenchmark2_Reference = 1945; + //[3] b3DynamicBvh::collideTT + bool cfgBenchmark3_Enable = cfgEnable; + static const int cfgBenchmark3_Iterations = 512; + static const int cfgBenchmark3_Reference = 5485; + //[4] b3DynamicBvh::collideTT self + bool cfgBenchmark4_Enable = cfgEnable; + static const int cfgBenchmark4_Iterations = 512; + static const int cfgBenchmark4_Reference = 2814; + //[5] b3DynamicBvh::collideTT xform + bool cfgBenchmark5_Enable = cfgEnable; + static const int cfgBenchmark5_Iterations = 512; + static const b3Scalar cfgBenchmark5_OffsetScale = 2; + static const int cfgBenchmark5_Reference = 7379; + //[6] b3DynamicBvh::collideTT xform,self + bool cfgBenchmark6_Enable = cfgEnable; + static const int cfgBenchmark6_Iterations = 512; + static const b3Scalar cfgBenchmark6_OffsetScale = 2; + static const int cfgBenchmark6_Reference = 7270; + //[7] b3DynamicBvh::rayTest + bool cfgBenchmark7_Enable = cfgEnable; + static const int cfgBenchmark7_Passes = 32; + static const int cfgBenchmark7_Iterations = 65536; + static const int cfgBenchmark7_Reference = 6307; + //[8] insert/remove + bool cfgBenchmark8_Enable = cfgEnable; + static const int cfgBenchmark8_Passes = 32; + static const int cfgBenchmark8_Iterations = 65536; + static const int cfgBenchmark8_Reference = 2105; + //[9] updates (teleport) + bool cfgBenchmark9_Enable = cfgEnable; + static const int cfgBenchmark9_Passes = 32; + static const int cfgBenchmark9_Iterations = 65536; + static const int cfgBenchmark9_Reference = 1879; + //[10] updates (jitter) + bool cfgBenchmark10_Enable = cfgEnable; + static const b3Scalar cfgBenchmark10_Scale = cfgVolumeCenterScale/10000; + static const int cfgBenchmark10_Passes = 32; + static const int cfgBenchmark10_Iterations = 65536; + static const int cfgBenchmark10_Reference = 1244; + //[11] optimize (incremental) + bool cfgBenchmark11_Enable = cfgEnable; + static const int cfgBenchmark11_Passes = 64; + static const int cfgBenchmark11_Iterations = 65536; + static const int cfgBenchmark11_Reference = 2510; + //[12] b3DbvtVolume notequal + bool cfgBenchmark12_Enable = cfgEnable; + static const int cfgBenchmark12_Iterations = 32; + static const int cfgBenchmark12_Reference = 3677; + //[13] culling(OCL+fullsort) + bool cfgBenchmark13_Enable = cfgEnable; + static const int cfgBenchmark13_Iterations = 1024; + static const int cfgBenchmark13_Reference = 2231; + //[14] culling(OCL+qsort) + bool cfgBenchmark14_Enable = cfgEnable; + static const int cfgBenchmark14_Iterations = 8192; + static const int cfgBenchmark14_Reference = 3500; + //[15] culling(KDOP+qsort) + bool cfgBenchmark15_Enable = cfgEnable; + static const int cfgBenchmark15_Iterations = 8192; + static const int cfgBenchmark15_Reference = 1151; + //[16] insert/remove batch + bool cfgBenchmark16_Enable = cfgEnable; + static const int cfgBenchmark16_BatchCount = 256; + static const int cfgBenchmark16_Passes = 16384; + static const int cfgBenchmark16_Reference = 5138; + //[17] select + bool cfgBenchmark17_Enable = cfgEnable; + static const int cfgBenchmark17_Iterations = 4; + static const int cfgBenchmark17_Reference = 3390; + + b3Clock wallclock; + printf("Benchmarking dbvt...\r\n"); + printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale); + printf("\tExtents base: %f\r\n",cfgVolumeExentsBase); + printf("\tExtents range: %f\r\n",cfgVolumeExentsScale); + printf("\tLeaves: %u\r\n",cfgLeaves); + printf("\tsizeof(b3DbvtVolume): %u bytes\r\n",sizeof(b3DbvtVolume)); + printf("\tsizeof(b3DbvtNode): %u bytes\r\n",sizeof(b3DbvtNode)); + if(cfgBenchmark1_Enable) + {// Benchmark 1 + srand(380843); + b3AlignedObjectArray<b3DbvtVolume> volumes; + b3AlignedObjectArray<bool> results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i<cfgLeaves;++i) + { + volumes[i]=b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + } + printf("[1] b3DbvtVolume intersections: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark1_Iterations;++i) + { + for(int j=0;j<cfgLeaves;++j) + { + for(int k=0;k<cfgLeaves;++k) + { + results[k]=Intersect(volumes[j],volumes[k]); + } + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark1_Reference)*100/time); + } + if(cfgBenchmark2_Enable) + {// Benchmark 2 + srand(380843); + b3AlignedObjectArray<b3DbvtVolume> volumes; + b3AlignedObjectArray<b3DbvtVolume> results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i<cfgLeaves;++i) + { + volumes[i]=b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + } + printf("[2] b3DbvtVolume merges: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark2_Iterations;++i) + { + for(int j=0;j<cfgLeaves;++j) + { + for(int k=0;k<cfgLeaves;++k) + { + Merge(volumes[j],volumes[k],results[k]); + } + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark2_Reference)*100/time); + } + if(cfgBenchmark3_Enable) + {// Benchmark 3 + srand(380843); + b3DynamicBvh dbvt[2]; + b3DbvtBenchmark::NilPolicy policy; + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]); + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]); + dbvt[0].optimizeTopDown(); + dbvt[1].optimizeTopDown(); + printf("[3] b3DynamicBvh::collideTT: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark3_Iterations;++i) + { + b3DynamicBvh::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy); + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark3_Reference)*100/time); + } + if(cfgBenchmark4_Enable) + {// Benchmark 4 + srand(380843); + b3DynamicBvh dbvt; + b3DbvtBenchmark::NilPolicy policy; + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[4] b3DynamicBvh::collideTT self: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark4_Iterations;++i) + { + b3DynamicBvh::collideTT(dbvt.m_root,dbvt.m_root,policy); + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark4_Reference)*100/time); + } + if(cfgBenchmark5_Enable) + {// Benchmark 5 + srand(380843); + b3DynamicBvh dbvt[2]; + b3AlignedObjectArray<b3Transform> transforms; + b3DbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark5_Iterations); + for(int i=0;i<transforms.size();++i) + { + transforms[i]=b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale); + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]); + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]); + dbvt[0].optimizeTopDown(); + dbvt[1].optimizeTopDown(); + printf("[5] b3DynamicBvh::collideTT xform: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark5_Iterations;++i) + { + b3DynamicBvh::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy); + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark5_Reference)*100/time); + } + if(cfgBenchmark6_Enable) + {// Benchmark 6 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray<b3Transform> transforms; + b3DbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark6_Iterations); + for(int i=0;i<transforms.size();++i) + { + transforms[i]=b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale); + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[6] b3DynamicBvh::collideTT xform,self: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark6_Iterations;++i) + { + b3DynamicBvh::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy); + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark6_Reference)*100/time); + } + if(cfgBenchmark7_Enable) + {// Benchmark 7 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray<b3Vector3> rayorg; + b3AlignedObjectArray<b3Vector3> raydir; + b3DbvtBenchmark::NilPolicy policy; + rayorg.resize(cfgBenchmark7_Iterations); + raydir.resize(cfgBenchmark7_Iterations); + for(int i=0;i<rayorg.size();++i) + { + rayorg[i]=b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale*2); + raydir[i]=b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale*2); + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[7] b3DynamicBvh::rayTest: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark7_Passes;++i) + { + for(int j=0;j<cfgBenchmark7_Iterations;++j) + { + b3DynamicBvh::rayTest(dbvt.m_root,rayorg[j],rayorg[j]+raydir[j],policy); + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + unsigned rays=cfgBenchmark7_Passes*cfgBenchmark7_Iterations; + printf("%u ms (%i%%),(%u r/s)\r\n",time,(time-cfgBenchmark7_Reference)*100/time,(rays*1000)/time); + } + if(cfgBenchmark8_Enable) + {// Benchmark 8 + srand(380843); + b3DynamicBvh dbvt; + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[8] insert/remove: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark8_Passes;++i) + { + for(int j=0;j<cfgBenchmark8_Iterations;++j) + { + dbvt.remove(dbvt.insert(b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0)); + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int ir=cfgBenchmark8_Passes*cfgBenchmark8_Iterations; + printf("%u ms (%i%%),(%u ir/s)\r\n",time,(time-cfgBenchmark8_Reference)*100/time,ir*1000/time); + } + if(cfgBenchmark9_Enable) + {// Benchmark 9 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray<const b3DbvtNode*> leaves; + b3DbvtBenchmark::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<cfgBenchmark9_Passes;++i) + { + for(int j=0;j<cfgBenchmark9_Iterations;++j) + { + dbvt.update(const_cast<b3DbvtNode*>(leaves[rand()%cfgLeaves]), + b3DbvtBenchmark::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); + b3DynamicBvh dbvt; + b3AlignedObjectArray<const b3DbvtNode*> leaves; + b3AlignedObjectArray<b3Vector3> vectors; + vectors.resize(cfgBenchmark10_Iterations); + for(int i=0;i<vectors.size();++i) + { + vectors[i]=(b3DbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1))*cfgBenchmark10_Scale; + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + dbvt.extractLeaves(dbvt.m_root,leaves); + printf("[10] updates (jitter): "); + wallclock.reset(); + + for(int i=0;i<cfgBenchmark10_Passes;++i) + { + for(int j=0;j<cfgBenchmark10_Iterations;++j) + { + const b3Vector3& d=vectors[j]; + b3DbvtNode* l=const_cast<b3DbvtNode*>(leaves[rand()%cfgLeaves]); + b3DbvtVolume v=b3DbvtVolume::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); + b3DynamicBvh dbvt; + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[11] optimize (incremental): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark11_Passes;++i) + { + dbvt.optimizeIncremental(cfgBenchmark11_Iterations); + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int op=cfgBenchmark11_Passes*cfgBenchmark11_Iterations; + printf("%u ms (%i%%),(%u o/s)\r\n",time,(time-cfgBenchmark11_Reference)*100/time,op/time*1000); + } + if(cfgBenchmark12_Enable) + {// Benchmark 12 + srand(380843); + b3AlignedObjectArray<b3DbvtVolume> volumes; + b3AlignedObjectArray<bool> results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i<cfgLeaves;++i) + { + volumes[i]=b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + } + printf("[12] b3DbvtVolume notequal: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark12_Iterations;++i) + { + for(int j=0;j<cfgLeaves;++j) + { + for(int k=0;k<cfgLeaves;++k) + { + results[k]=NotEqual(volumes[j],volumes[k]); + } + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark12_Reference)*100/time); + } + if(cfgBenchmark13_Enable) + {// Benchmark 13 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray<b3Vector3> vectors; + b3DbvtBenchmark::NilPolicy policy; + vectors.resize(cfgBenchmark13_Iterations); + for(int i=0;i<vectors.size();++i) + { + vectors[i]=(b3DbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1)).normalized(); + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[13] culling(OCL+fullsort): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark13_Iterations;++i) + { + static const b3Scalar offset=0; + policy.m_depth=-B3_INFINITY; + dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy); + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int t=cfgBenchmark13_Iterations; + printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark13_Reference)*100/time,(t*1000)/time); + } + if(cfgBenchmark14_Enable) + {// Benchmark 14 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray<b3Vector3> vectors; + b3DbvtBenchmark::P14 policy; + vectors.resize(cfgBenchmark14_Iterations); + for(int i=0;i<vectors.size();++i) + { + vectors[i]=(b3DbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1)).normalized(); + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + policy.m_nodes.reserve(cfgLeaves); + printf("[14] culling(OCL+qsort): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark14_Iterations;++i) + { + static const b3Scalar offset=0; + policy.m_nodes.resize(0); + dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false); + policy.m_nodes.quickSort(b3DbvtBenchmark::P14::sortfnc); + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int t=cfgBenchmark14_Iterations; + printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark14_Reference)*100/time,(t*1000)/time); + } + if(cfgBenchmark15_Enable) + {// Benchmark 15 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray<b3Vector3> vectors; + b3DbvtBenchmark::P15 policy; + vectors.resize(cfgBenchmark15_Iterations); + for(int i=0;i<vectors.size();++i) + { + vectors[i]=(b3DbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1)).normalized(); + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + policy.m_nodes.reserve(cfgLeaves); + printf("[15] culling(KDOP+qsort): "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark15_Iterations;++i) + { + static const b3Scalar offset=0; + policy.m_nodes.resize(0); + policy.m_axis=vectors[i]; + dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy); + policy.m_nodes.quickSort(b3DbvtBenchmark::P15::sortfnc); + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int t=cfgBenchmark15_Iterations; + printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark15_Reference)*100/time,(t*1000)/time); + } + if(cfgBenchmark16_Enable) + {// Benchmark 16 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray<b3DbvtNode*> batch; + b3DbvtBenchmark::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<cfgBenchmark16_Passes;++i) + { + for(int j=0;j<cfgBenchmark16_BatchCount;++j) + { + batch.push_back(dbvt.insert(b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0)); + } + for(int j=0;j<cfgBenchmark16_BatchCount;++j) + { + dbvt.remove(batch[j]); + } + batch.resize(0); + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int ir=cfgBenchmark16_Passes*cfgBenchmark16_BatchCount; + printf("%u ms (%i%%),(%u bir/s)\r\n",time,(time-cfgBenchmark16_Reference)*100/time,int(ir*1000.0/time)); + } + if(cfgBenchmark17_Enable) + {// Benchmark 17 + srand(380843); + b3AlignedObjectArray<b3DbvtVolume> volumes; + b3AlignedObjectArray<int> results; + b3AlignedObjectArray<int> indices; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + indices.resize(cfgLeaves); + for(int i=0;i<cfgLeaves;++i) + { + indices[i]=i; + volumes[i]=b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale); + } + for(int i=0;i<cfgLeaves;++i) + { + b3Swap(indices[i],indices[rand()%cfgLeaves]); + } + printf("[17] b3DbvtVolume select: "); + wallclock.reset(); + for(int i=0;i<cfgBenchmark17_Iterations;++i) + { + for(int j=0;j<cfgLeaves;++j) + { + for(int k=0;k<cfgLeaves;++k) + { + const int idx=indices[k]; + results[idx]=Select(volumes[idx],volumes[j],volumes[k]); + } + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time); + } + printf("\r\n\r\n"); +} +#endif diff --git a/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h new file mode 100644 index 0000000000..c004b9130f --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h @@ -0,0 +1,1269 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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. +*/ +///b3DynamicBvh implementation by Nathanael Presson + +#ifndef B3_DYNAMIC_BOUNDING_VOLUME_TREE_H +#define B3_DYNAMIC_BOUNDING_VOLUME_TREE_H + +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Transform.h" +#include "Bullet3Geometry/b3AabbUtil.h" + +// +// Compile time configuration +// + + +// Implementation profiles +#define B3_DBVT_IMPL_GENERIC 0 // Generic implementation +#define B3_DBVT_IMPL_SSE 1 // SSE + +// Template implementation of ICollide +#ifdef _WIN32 +#if (defined (_MSC_VER) && _MSC_VER >= 1400) +#define B3_DBVT_USE_TEMPLATE 1 +#else +#define B3_DBVT_USE_TEMPLATE 0 +#endif +#else +#define B3_DBVT_USE_TEMPLATE 0 +#endif + +// Use only intrinsics instead of inline asm +#define B3_DBVT_USE_INTRINSIC_SSE 1 + +// Using memmov for collideOCL +#define B3_DBVT_USE_MEMMOVE 1 + +// Enable benchmarking code +#define B3_DBVT_ENABLE_BENCHMARK 0 + +// Inlining +#define B3_DBVT_INLINE B3_FORCE_INLINE + +// Specific methods implementation + +//SSE gives errors on a MSVC 7.1 +#if defined (B3_USE_SSE) //&& defined (_WIN32) +#define B3_DBVT_SELECT_IMPL B3_DBVT_IMPL_SSE +#define B3_DBVT_MERGE_IMPL B3_DBVT_IMPL_SSE +#define B3_DBVT_INT0_IMPL B3_DBVT_IMPL_SSE +#else +#define B3_DBVT_SELECT_IMPL B3_DBVT_IMPL_GENERIC +#define B3_DBVT_MERGE_IMPL B3_DBVT_IMPL_GENERIC +#define B3_DBVT_INT0_IMPL B3_DBVT_IMPL_GENERIC +#endif + +#if (B3_DBVT_SELECT_IMPL==B3_DBVT_IMPL_SSE)|| \ + (B3_DBVT_MERGE_IMPL==B3_DBVT_IMPL_SSE)|| \ + (B3_DBVT_INT0_IMPL==B3_DBVT_IMPL_SSE) +#include <emmintrin.h> +#endif + +// +// Auto config and checks +// + +#if B3_DBVT_USE_TEMPLATE +#define B3_DBVT_VIRTUAL +#define B3_DBVT_VIRTUAL_DTOR(a) +#define B3_DBVT_PREFIX template <typename T> +#define B3_DBVT_IPOLICY T& policy +#define B3_DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker; +#else +#define B3_DBVT_VIRTUAL_DTOR(a) virtual ~a() {} +#define B3_DBVT_VIRTUAL virtual +#define B3_DBVT_PREFIX +#define B3_DBVT_IPOLICY ICollide& policy +#define B3_DBVT_CHECKTYPE +#endif + +#if B3_DBVT_USE_MEMMOVE +#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) +#include <memory.h> +#endif +#include <string.h> +#endif + +#ifndef B3_DBVT_USE_TEMPLATE +#error "B3_DBVT_USE_TEMPLATE undefined" +#endif + +#ifndef B3_DBVT_USE_MEMMOVE +#error "B3_DBVT_USE_MEMMOVE undefined" +#endif + +#ifndef B3_DBVT_ENABLE_BENCHMARK +#error "B3_DBVT_ENABLE_BENCHMARK undefined" +#endif + +#ifndef B3_DBVT_SELECT_IMPL +#error "B3_DBVT_SELECT_IMPL undefined" +#endif + +#ifndef B3_DBVT_MERGE_IMPL +#error "B3_DBVT_MERGE_IMPL undefined" +#endif + +#ifndef B3_DBVT_INT0_IMPL +#error "B3_DBVT_INT0_IMPL undefined" +#endif + +// +// Defaults volumes +// + +/* b3DbvtAabbMm */ +struct b3DbvtAabbMm +{ + B3_DBVT_INLINE b3Vector3 Center() const { return((mi+mx)/2); } + B3_DBVT_INLINE b3Vector3 Lengths() const { return(mx-mi); } + B3_DBVT_INLINE b3Vector3 Extents() const { return((mx-mi)/2); } + B3_DBVT_INLINE const b3Vector3& Mins() const { return(mi); } + B3_DBVT_INLINE const b3Vector3& Maxs() const { return(mx); } + static inline b3DbvtAabbMm FromCE(const b3Vector3& c,const b3Vector3& e); + static inline b3DbvtAabbMm FromCR(const b3Vector3& c,b3Scalar r); + static inline b3DbvtAabbMm FromMM(const b3Vector3& mi,const b3Vector3& mx); + static inline b3DbvtAabbMm FromPoints(const b3Vector3* pts,int n); + static inline b3DbvtAabbMm FromPoints(const b3Vector3** ppts,int n); + B3_DBVT_INLINE void Expand(const b3Vector3& e); + B3_DBVT_INLINE void SignedExpand(const b3Vector3& e); + B3_DBVT_INLINE bool Contain(const b3DbvtAabbMm& a) const; + B3_DBVT_INLINE int Classify(const b3Vector3& n,b3Scalar o,int s) const; + B3_DBVT_INLINE b3Scalar ProjectMinimum(const b3Vector3& v,unsigned signs) const; + B3_DBVT_INLINE friend bool b3Intersect( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b); + + B3_DBVT_INLINE friend bool b3Intersect( const b3DbvtAabbMm& a, + const b3Vector3& b); + + B3_DBVT_INLINE friend b3Scalar b3Proximity( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b); + B3_DBVT_INLINE friend int b3Select( const b3DbvtAabbMm& o, + const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b); + B3_DBVT_INLINE friend void b3Merge( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b, + b3DbvtAabbMm& r); + B3_DBVT_INLINE friend bool b3NotEqual( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b); + + B3_DBVT_INLINE b3Vector3& tMins() { return(mi); } + B3_DBVT_INLINE b3Vector3& tMaxs() { return(mx); } + +private: + B3_DBVT_INLINE void AddSpan(const b3Vector3& d,b3Scalar& smi,b3Scalar& smx) const; +private: + b3Vector3 mi,mx; +}; + +// Types +typedef b3DbvtAabbMm b3DbvtVolume; + +/* b3DbvtNode */ +struct b3DbvtNode +{ + b3DbvtVolume volume; + b3DbvtNode* parent; + B3_DBVT_INLINE bool isleaf() const { return(childs[1]==0); } + B3_DBVT_INLINE bool isinternal() const { return(!isleaf()); } + union + { + b3DbvtNode* childs[2]; + void* data; + int dataAsInt; + }; +}; + +///The b3DynamicBvh class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). +///This b3DynamicBvh is used for soft body collision detection and for the b3DynamicBvhBroadphase. It has a fast insert, remove and update of nodes. +///Unlike the b3QuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. +struct b3DynamicBvh +{ + /* Stack element */ + struct sStkNN + { + const b3DbvtNode* a; + const b3DbvtNode* b; + sStkNN() {} + sStkNN(const b3DbvtNode* na,const b3DbvtNode* nb) : a(na),b(nb) {} + }; + struct sStkNP + { + const b3DbvtNode* node; + int mask; + sStkNP(const b3DbvtNode* n,unsigned m) : node(n),mask(m) {} + }; + struct sStkNPS + { + const b3DbvtNode* node; + int mask; + b3Scalar value; + sStkNPS() {} + sStkNPS(const b3DbvtNode* n,unsigned m,b3Scalar v) : node(n),mask(m),value(v) {} + }; + struct sStkCLN + { + const b3DbvtNode* node; + b3DbvtNode* parent; + sStkCLN(const b3DbvtNode* n,b3DbvtNode* p) : node(n),parent(p) {} + }; + // Policies/Interfaces + + /* ICollide */ + struct ICollide + { + B3_DBVT_VIRTUAL_DTOR(ICollide) + B3_DBVT_VIRTUAL void Process(const b3DbvtNode*,const b3DbvtNode*) {} + B3_DBVT_VIRTUAL void Process(const b3DbvtNode*) {} + B3_DBVT_VIRTUAL void Process(const b3DbvtNode* n,b3Scalar) { Process(n); } + B3_DBVT_VIRTUAL bool Descent(const b3DbvtNode*) { return(true); } + B3_DBVT_VIRTUAL bool AllLeaves(const b3DbvtNode*) { return(true); } + }; + /* IWriter */ + struct IWriter + { + virtual ~IWriter() {} + virtual void Prepare(const b3DbvtNode* root,int numnodes)=0; + virtual void WriteNode(const b3DbvtNode*,int index,int parent,int child0,int child1)=0; + virtual void WriteLeaf(const b3DbvtNode*,int index,int parent)=0; + }; + /* IClone */ + struct IClone + { + virtual ~IClone() {} + virtual void CloneLeaf(b3DbvtNode*) {} + }; + + // Constants + enum { + B3_SIMPLE_STACKSIZE = 64, + B3_DOUBLE_STACKSIZE = B3_SIMPLE_STACKSIZE*2 + }; + + // Fields + b3DbvtNode* m_root; + b3DbvtNode* m_free; + int m_lkhd; + int m_leaves; + unsigned m_opath; + + + b3AlignedObjectArray<sStkNN> m_stkStack; + mutable b3AlignedObjectArray<const b3DbvtNode*> m_rayTestStack; + + + // Methods + b3DynamicBvh(); + ~b3DynamicBvh(); + void clear(); + bool empty() const { return(0==m_root); } + void optimizeBottomUp(); + void optimizeTopDown(int bu_treshold=128); + void optimizeIncremental(int passes); + b3DbvtNode* insert(const b3DbvtVolume& box,void* data); + void update(b3DbvtNode* leaf,int lookahead=-1); + void update(b3DbvtNode* leaf,b3DbvtVolume& volume); + bool update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity,b3Scalar margin); + bool update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity); + bool update(b3DbvtNode* leaf,b3DbvtVolume& volume,b3Scalar margin); + void remove(b3DbvtNode* leaf); + void write(IWriter* iwriter) const; + void clone(b3DynamicBvh& dest,IClone* iclone=0) const; + static int maxdepth(const b3DbvtNode* node); + static int countLeaves(const b3DbvtNode* node); + static void extractLeaves(const b3DbvtNode* node,b3AlignedObjectArray<const b3DbvtNode*>& leaves); +#if B3_DBVT_ENABLE_BENCHMARK + static void benchmark(); +#else + static void benchmark(){} +#endif + // B3_DBVT_IPOLICY must support ICollide policy/interface + B3_DBVT_PREFIX + static void enumNodes( const b3DbvtNode* root, + B3_DBVT_IPOLICY); + B3_DBVT_PREFIX + static void enumLeaves( const b3DbvtNode* root, + B3_DBVT_IPOLICY); + B3_DBVT_PREFIX + void collideTT( const b3DbvtNode* root0, + const b3DbvtNode* root1, + B3_DBVT_IPOLICY); + + B3_DBVT_PREFIX + void collideTTpersistentStack( const b3DbvtNode* root0, + const b3DbvtNode* root1, + B3_DBVT_IPOLICY); +#if 0 + B3_DBVT_PREFIX + void collideTT( const b3DbvtNode* root0, + const b3DbvtNode* root1, + const b3Transform& xform, + B3_DBVT_IPOLICY); + B3_DBVT_PREFIX + void collideTT( const b3DbvtNode* root0, + const b3Transform& xform0, + const b3DbvtNode* root1, + const b3Transform& xform1, + B3_DBVT_IPOLICY); +#endif + + B3_DBVT_PREFIX + void collideTV( const b3DbvtNode* root, + const b3DbvtVolume& volume, + B3_DBVT_IPOLICY) const; + ///rayTest is a re-entrant ray test, and can be called in parallel as long as the b3AlignedAlloc is thread-safe (uses locking etc) + ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time + B3_DBVT_PREFIX + static void rayTest( const b3DbvtNode* root, + const b3Vector3& rayFrom, + const b3Vector3& rayTo, + B3_DBVT_IPOLICY); + ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections + ///rayTestInternal is used by b3DynamicBvhBroadphase to accelerate world ray casts + B3_DBVT_PREFIX + void rayTestInternal( const b3DbvtNode* root, + const b3Vector3& rayFrom, + const b3Vector3& rayTo, + const b3Vector3& rayDirectionInverse, + unsigned int signs[3], + b3Scalar lambda_max, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + B3_DBVT_IPOLICY) const; + + B3_DBVT_PREFIX + static void collideKDOP(const b3DbvtNode* root, + const b3Vector3* normals, + const b3Scalar* offsets, + int count, + B3_DBVT_IPOLICY); + B3_DBVT_PREFIX + static void collideOCL( const b3DbvtNode* root, + const b3Vector3* normals, + const b3Scalar* offsets, + const b3Vector3& sortaxis, + int count, + B3_DBVT_IPOLICY, + bool fullsort=true); + B3_DBVT_PREFIX + static void collideTU( const b3DbvtNode* root, + B3_DBVT_IPOLICY); + // Helpers + static B3_DBVT_INLINE int nearest(const int* i,const b3DynamicBvh::sStkNPS* a,b3Scalar v,int l,int h) + { + int m=0; + while(l<h) + { + m=(l+h)>>1; + if(a[i[m]].value>=v) l=m+1; else h=m; + } + return(h); + } + static B3_DBVT_INLINE int allocate( b3AlignedObjectArray<int>& ifree, + b3AlignedObjectArray<sStkNPS>& stock, + const sStkNPS& value) + { + int i; + if(ifree.size()>0) + { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } + else + { i=stock.size();stock.push_back(value); } + return(i); + } + // +private: + b3DynamicBvh(const b3DynamicBvh&) {} +}; + +// +// Inline's +// + +// +inline b3DbvtAabbMm b3DbvtAabbMm::FromCE(const b3Vector3& c,const b3Vector3& e) +{ + b3DbvtAabbMm box; + box.mi=c-e;box.mx=c+e; + return(box); +} + +// +inline b3DbvtAabbMm b3DbvtAabbMm::FromCR(const b3Vector3& c,b3Scalar r) +{ + return(FromCE(c,b3MakeVector3(r,r,r))); +} + +// +inline b3DbvtAabbMm b3DbvtAabbMm::FromMM(const b3Vector3& mi,const b3Vector3& mx) +{ + b3DbvtAabbMm box; + box.mi=mi;box.mx=mx; + return(box); +} + +// +inline b3DbvtAabbMm b3DbvtAabbMm::FromPoints(const b3Vector3* pts,int n) +{ + b3DbvtAabbMm box; + box.mi=box.mx=pts[0]; + for(int i=1;i<n;++i) + { + box.mi.setMin(pts[i]); + box.mx.setMax(pts[i]); + } + return(box); +} + +// +inline b3DbvtAabbMm b3DbvtAabbMm::FromPoints(const b3Vector3** ppts,int n) +{ + b3DbvtAabbMm box; + box.mi=box.mx=*ppts[0]; + for(int i=1;i<n;++i) + { + box.mi.setMin(*ppts[i]); + box.mx.setMax(*ppts[i]); + } + return(box); +} + +// +B3_DBVT_INLINE void b3DbvtAabbMm::Expand(const b3Vector3& e) +{ + mi-=e;mx+=e; +} + +// +B3_DBVT_INLINE void b3DbvtAabbMm::SignedExpand(const b3Vector3& e) +{ + if(e.x>0) mx.setX(mx.x+e[0]); else mi.setX(mi.x+e[0]); + if(e.y>0) mx.setY(mx.y+e[1]); else mi.setY(mi.y+e[1]); + if(e.z>0) mx.setZ(mx.z+e[2]); else mi.setZ(mi.z+e[2]); +} + +// +B3_DBVT_INLINE bool b3DbvtAabbMm::Contain(const b3DbvtAabbMm& a) const +{ + return( (mi.x<=a.mi.x)&& + (mi.y<=a.mi.y)&& + (mi.z<=a.mi.z)&& + (mx.x>=a.mx.x)&& + (mx.y>=a.mx.y)&& + (mx.z>=a.mx.z)); +} + +// +B3_DBVT_INLINE int b3DbvtAabbMm::Classify(const b3Vector3& n,b3Scalar o,int s) const +{ + b3Vector3 pi,px; + switch(s) + { + case (0+0+0): px=b3MakeVector3(mi.x,mi.y,mi.z); + pi=b3MakeVector3(mx.x,mx.y,mx.z);break; + case (1+0+0): px=b3MakeVector3(mx.x,mi.y,mi.z); + pi=b3MakeVector3(mi.x,mx.y,mx.z);break; + case (0+2+0): px=b3MakeVector3(mi.x,mx.y,mi.z); + pi=b3MakeVector3(mx.x,mi.y,mx.z);break; + case (1+2+0): px=b3MakeVector3(mx.x,mx.y,mi.z); + pi=b3MakeVector3(mi.x,mi.y,mx.z);break; + case (0+0+4): px=b3MakeVector3(mi.x,mi.y,mx.z); + pi=b3MakeVector3(mx.x,mx.y,mi.z);break; + case (1+0+4): px=b3MakeVector3(mx.x,mi.y,mx.z); + pi=b3MakeVector3(mi.x,mx.y,mi.z);break; + case (0+2+4): px=b3MakeVector3(mi.x,mx.y,mx.z); + pi=b3MakeVector3(mx.x,mi.y,mi.z);break; + case (1+2+4): px=b3MakeVector3(mx.x,mx.y,mx.z); + pi=b3MakeVector3(mi.x,mi.y,mi.z);break; + } + if((b3Dot(n,px)+o)<0) return(-1); + if((b3Dot(n,pi)+o)>=0) return(+1); + return(0); +} + +// +B3_DBVT_INLINE b3Scalar b3DbvtAabbMm::ProjectMinimum(const b3Vector3& v,unsigned signs) const +{ + const b3Vector3* b[]={&mx,&mi}; + const b3Vector3 p = b3MakeVector3( b[(signs>>0)&1]->x, + b[(signs>>1)&1]->y, + b[(signs>>2)&1]->z); + return(b3Dot(p,v)); +} + +// +B3_DBVT_INLINE void b3DbvtAabbMm::AddSpan(const b3Vector3& d,b3Scalar& smi,b3Scalar& smx) const +{ + for(int i=0;i<3;++i) + { + if(d[i]<0) + { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; } + else + { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; } + } +} + +// +B3_DBVT_INLINE bool b3Intersect( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b) +{ +#if B3_DBVT_INT0_IMPL == B3_DBVT_IMPL_SSE + const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), + _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); +#if defined (_WIN32) + const __int32* pu((const __int32*)&rt); +#else + const int* pu((const int*)&rt); +#endif + return((pu[0]|pu[1]|pu[2])==0); +#else + return( (a.mi.x<=b.mx.x)&& + (a.mx.x>=b.mi.x)&& + (a.mi.y<=b.mx.y)&& + (a.mx.y>=b.mi.y)&& + (a.mi.z<=b.mx.z)&& + (a.mx.z>=b.mi.z)); +#endif +} + + + +// +B3_DBVT_INLINE bool b3Intersect( const b3DbvtAabbMm& a, + const b3Vector3& b) +{ + return( (b.x>=a.mi.x)&& + (b.y>=a.mi.y)&& + (b.z>=a.mi.z)&& + (b.x<=a.mx.x)&& + (b.y<=a.mx.y)&& + (b.z<=a.mx.z)); +} + + + + + +////////////////////////////////////// + + +// +B3_DBVT_INLINE b3Scalar b3Proximity( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b) +{ + const b3Vector3 d=(a.mi+a.mx)-(b.mi+b.mx); + return(b3Fabs(d.x)+b3Fabs(d.y)+b3Fabs(d.z)); +} + + + +// +B3_DBVT_INLINE int b3Select( const b3DbvtAabbMm& o, + const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b) +{ +#if B3_DBVT_SELECT_IMPL == B3_DBVT_IMPL_SSE + +#if defined (_WIN32) + static B3_ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; +#else + static B3_ATTRIBUTE_ALIGNED16(const unsigned int) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 /*0x7fffffff*/}; +#endif + ///@todo: the intrinsic version is 11% slower +#if B3_DBVT_USE_INTRINSIC_SSE + + union b3SSEUnion ///NOTE: if we use more intrinsics, move b3SSEUnion into the LinearMath directory + { + __m128 ssereg; + float floats[4]; + int ints[4]; + }; + + __m128 omi(_mm_load_ps(o.mi)); + omi=_mm_add_ps(omi,_mm_load_ps(o.mx)); + __m128 ami(_mm_load_ps(a.mi)); + ami=_mm_add_ps(ami,_mm_load_ps(a.mx)); + ami=_mm_sub_ps(ami,omi); + ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask)); + __m128 bmi(_mm_load_ps(b.mi)); + bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx)); + bmi=_mm_sub_ps(bmi,omi); + bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask)); + __m128 t0(_mm_movehl_ps(ami,ami)); + ami=_mm_add_ps(ami,t0); + ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1)); + __m128 t1(_mm_movehl_ps(bmi,bmi)); + bmi=_mm_add_ps(bmi,t1); + bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); + + b3SSEUnion tmp; + tmp.ssereg = _mm_cmple_ss(bmi,ami); + return tmp.ints[0]&1; + +#else + B3_ATTRIBUTE_ALIGNED16(__int32 r[1]); + __asm + { + mov eax,o + mov ecx,a + mov edx,b + movaps xmm0,[eax] + movaps xmm5,mask + addps xmm0,[eax+16] + movaps xmm1,[ecx] + movaps xmm2,[edx] + addps xmm1,[ecx+16] + addps xmm2,[edx+16] + subps xmm1,xmm0 + subps xmm2,xmm0 + andps xmm1,xmm5 + andps xmm2,xmm5 + movhlps xmm3,xmm1 + movhlps xmm4,xmm2 + addps xmm1,xmm3 + addps xmm2,xmm4 + pshufd xmm3,xmm1,1 + pshufd xmm4,xmm2,1 + addss xmm1,xmm3 + addss xmm2,xmm4 + cmpless xmm2,xmm1 + movss r,xmm2 + } + return(r[0]&1); +#endif +#else + return(b3Proximity(o,a)<b3Proximity(o,b)?0:1); +#endif +} + +// +B3_DBVT_INLINE void b3Merge( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b, + b3DbvtAabbMm& r) +{ +#if B3_DBVT_MERGE_IMPL==B3_DBVT_IMPL_SSE + __m128 ami(_mm_load_ps(a.mi)); + __m128 amx(_mm_load_ps(a.mx)); + __m128 bmi(_mm_load_ps(b.mi)); + __m128 bmx(_mm_load_ps(b.mx)); + ami=_mm_min_ps(ami,bmi); + amx=_mm_max_ps(amx,bmx); + _mm_store_ps(r.mi,ami); + _mm_store_ps(r.mx,amx); +#else + for(int i=0;i<3;++i) + { + if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i]; + if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; + } +#endif +} + +// +B3_DBVT_INLINE bool b3NotEqual( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b) +{ + return( (a.mi.x!=b.mi.x)|| + (a.mi.y!=b.mi.y)|| + (a.mi.z!=b.mi.z)|| + (a.mx.x!=b.mx.x)|| + (a.mx.y!=b.mx.y)|| + (a.mx.z!=b.mx.z)); +} + +// +// Inline's +// + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::enumNodes( const b3DbvtNode* root, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + policy.Process(root); + if(root->isinternal()) + { + enumNodes(root->childs[0],policy); + enumNodes(root->childs[1],policy); + } +} + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::enumLeaves( const b3DbvtNode* root, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root->isinternal()) + { + enumLeaves(root->childs[0],policy); + enumLeaves(root->childs[1],policy); + } + else + { + policy.Process(root); + } +} + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideTT( const b3DbvtNode* root0, + const b3DbvtNode* root1, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=B3_DOUBLE_STACKSIZE-4; + b3AlignedObjectArray<sStkNN> stkStack; + stkStack.resize(B3_DOUBLE_STACKSIZE); + stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=stkStack[--depth]; + if(depth>treshold) + { + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a==p.b) + { + if(p.a->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + } + } + else if(b3Intersect(p.a->volume,p.b->volume)) + { + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} + + + +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideTTpersistentStack( const b3DbvtNode* root0, + const b3DbvtNode* root1, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=B3_DOUBLE_STACKSIZE-4; + + m_stkStack.resize(B3_DOUBLE_STACKSIZE); + m_stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=m_stkStack[--depth]; + if(depth>treshold) + { + m_stkStack.resize(m_stkStack.size()*2); + treshold=m_stkStack.size()-4; + } + if(p.a==p.b) + { + if(p.a->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + } + } + else if(b3Intersect(p.a->volume,p.b->volume)) + { + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} + +#if 0 +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideTT( const b3DbvtNode* root0, + const b3DbvtNode* root1, + const b3Transform& xform, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=B3_DOUBLE_STACKSIZE-4; + b3AlignedObjectArray<sStkNN> stkStack; + stkStack.resize(B3_DOUBLE_STACKSIZE); + stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=stkStack[--depth]; + if(b3Intersect(p.a->volume,p.b->volume,xform)) + { + if(depth>treshold) + { + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideTT( const b3DbvtNode* root0, + const b3Transform& xform0, + const b3DbvtNode* root1, + const b3Transform& xform1, + B3_DBVT_IPOLICY) +{ + const b3Transform xform=xform0.inverse()*xform1; + collideTT(root0,root1,xform,policy); +} +#endif + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideTV( const b3DbvtNode* root, + const b3DbvtVolume& vol, + B3_DBVT_IPOLICY) const +{ + B3_DBVT_CHECKTYPE + if(root) + { + B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) volume(vol); + b3AlignedObjectArray<const b3DbvtNode*> stack; + stack.resize(0); + stack.reserve(B3_SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const b3DbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(b3Intersect(n->volume,volume)) + { + if(n->isinternal()) + { + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); + } + else + { + policy.Process(n); + } + } + } while(stack.size()>0); + } +} + +B3_DBVT_PREFIX +inline void b3DynamicBvh::rayTestInternal( const b3DbvtNode* root, + const b3Vector3& rayFrom, + const b3Vector3& rayTo, + const b3Vector3& rayDirectionInverse, + unsigned int signs[3], + b3Scalar lambda_max, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + B3_DBVT_IPOLICY) const +{ + (void) rayTo; + B3_DBVT_CHECKTYPE + if(root) + { + int depth=1; + int treshold=B3_DOUBLE_STACKSIZE-2; + b3AlignedObjectArray<const b3DbvtNode*>& stack = m_rayTestStack; + stack.resize(B3_DOUBLE_STACKSIZE); + stack[0]=root; + b3Vector3 bounds[2]; + do + { + const b3DbvtNode* node=stack[--depth]; + bounds[0] = node->volume.Mins()-aabbMax; + bounds[1] = node->volume.Maxs()-aabbMin; + b3Scalar tmin=1.f,lambda_min=0.f; + unsigned int result1=false; + result1 = b3RayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + if(result1) + { + if(node->isinternal()) + { + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-2; + } + stack[depth++]=node->childs[0]; + stack[depth++]=node->childs[1]; + } + else + { + policy.Process(node); + } + } + } while(depth); + } +} + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::rayTest( const b3DbvtNode* root, + const b3Vector3& rayFrom, + const b3Vector3& rayTo, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root) + { + b3Vector3 rayDir = (rayTo-rayFrom); + rayDir.normalize (); + + ///what about division by zero? --> just set rayDirection[i] to INF/B3_LARGE_FLOAT + b3Vector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[2]; + unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; + + b3Scalar lambda_max = rayDir.dot(rayTo-rayFrom); +#ifdef COMPARE_BTRAY_AABB2 + b3Vector3 resultNormal; +#endif//COMPARE_BTRAY_AABB2 + + b3AlignedObjectArray<const b3DbvtNode*> stack; + + int depth=1; + int treshold=B3_DOUBLE_STACKSIZE-2; + + stack.resize(B3_DOUBLE_STACKSIZE); + stack[0]=root; + b3Vector3 bounds[2]; + do { + const b3DbvtNode* node=stack[--depth]; + + bounds[0] = node->volume.Mins(); + bounds[1] = node->volume.Maxs(); + + b3Scalar tmin=1.f,lambda_min=0.f; + unsigned int result1 = b3RayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + +#ifdef COMPARE_BTRAY_AABB2 + b3Scalar param=1.f; + bool result2 = b3RayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); + b3Assert(result1 == result2); +#endif //TEST_BTRAY_AABB2 + + if(result1) + { + if(node->isinternal()) + { + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-2; + } + stack[depth++]=node->childs[0]; + stack[depth++]=node->childs[1]; + } + else + { + policy.Process(node); + } + } + } while(depth); + + } +} + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideKDOP(const b3DbvtNode* root, + const b3Vector3* normals, + const b3Scalar* offsets, + int count, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root) + { + const int inside=(1<<count)-1; + b3AlignedObjectArray<sStkNP> stack; + int signs[sizeof(unsigned)*8]; + b3Assert(count<int (sizeof(signs)/sizeof(signs[0]))); + for(int i=0;i<count;++i) + { + signs[i]= ((normals[i].x>=0)?1:0)+ + ((normals[i].y>=0)?2:0)+ + ((normals[i].z>=0)?4:0); + } + stack.reserve(B3_SIMPLE_STACKSIZE); + stack.push_back(sStkNP(root,0)); + do { + sStkNP se=stack[stack.size()-1]; + bool out=false; + stack.pop_back(); + for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) + { + if(0==(se.mask&j)) + { + const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } + } + } + if(!out) + { + if((se.mask!=inside)&&(se.node->isinternal())) + { + stack.push_back(sStkNP(se.node->childs[0],se.mask)); + stack.push_back(sStkNP(se.node->childs[1],se.mask)); + } + else + { + if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); + } + } + } while(stack.size()); + } +} + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideOCL( const b3DbvtNode* root, + const b3Vector3* normals, + const b3Scalar* offsets, + const b3Vector3& sortaxis, + int count, + B3_DBVT_IPOLICY, + bool fsort) +{ + B3_DBVT_CHECKTYPE + if(root) + { + const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ + (sortaxis[1]>=0?2:0)+ + (sortaxis[2]>=0?4:0); + const int inside=(1<<count)-1; + b3AlignedObjectArray<sStkNPS> stock; + b3AlignedObjectArray<int> ifree; + b3AlignedObjectArray<int> stack; + int signs[sizeof(unsigned)*8]; + b3Assert(count<int (sizeof(signs)/sizeof(signs[0]))); + for(int i=0;i<count;++i) + { + signs[i]= ((normals[i].x>=0)?1:0)+ + ((normals[i].y>=0)?2:0)+ + ((normals[i].z>=0)?4:0); + } + stock.reserve(B3_SIMPLE_STACKSIZE); + stack.reserve(B3_SIMPLE_STACKSIZE); + ifree.reserve(B3_SIMPLE_STACKSIZE); + stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); + do { + const int id=stack[stack.size()-1]; + sStkNPS se=stock[id]; + stack.pop_back();ifree.push_back(id); + if(se.mask!=inside) + { + bool out=false; + for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) + { + if(0==(se.mask&j)) + { + const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } + } + } + if(out) continue; + } + if(policy.Descent(se.node)) + { + if(se.node->isinternal()) + { + const b3DbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; + sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), + sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; + const int q=nes[0].value<nes[1].value?1:0; + int j=stack.size(); + if(fsort&&(j>0)) + { + /* Insert 0 */ + j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); + stack.push_back(0); +#if B3_DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); +#else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; +#endif + stack[j]=allocate(ifree,stock,nes[q]); + /* Insert 1 */ + j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); + stack.push_back(0); +#if B3_DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); +#else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; +#endif + stack[j]=allocate(ifree,stock,nes[1-q]); + } + else + { + stack.push_back(allocate(ifree,stock,nes[q])); + stack.push_back(allocate(ifree,stock,nes[1-q])); + } + } + else + { + policy.Process(se.node,se.value); + } + } + } while(stack.size()); + } +} + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideTU( const b3DbvtNode* root, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root) + { + b3AlignedObjectArray<const b3DbvtNode*> stack; + stack.reserve(B3_SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const b3DbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(policy.Descent(n)) + { + if(n->isinternal()) + { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } + else + { policy.Process(n); } + } + } while(stack.size()>0); + } +} + +// +// PP Cleanup +// + +#undef B3_DBVT_USE_MEMMOVE +#undef B3_DBVT_USE_TEMPLATE +#undef B3_DBVT_VIRTUAL_DTOR +#undef B3_DBVT_VIRTUAL +#undef B3_DBVT_PREFIX +#undef B3_DBVT_IPOLICY +#undef B3_DBVT_CHECKTYPE +#undef B3_DBVT_IMPL_GENERIC +#undef B3_DBVT_IMPL_SSE +#undef B3_DBVT_USE_INTRINSIC_SSE +#undef B3_DBVT_SELECT_IMPL +#undef B3_DBVT_MERGE_IMPL +#undef B3_DBVT_INT0_IMPL + +#endif diff --git a/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp new file mode 100644 index 0000000000..bc150955b8 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp @@ -0,0 +1,804 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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. +*/ + +///b3DynamicBvhBroadphase implementation by Nathanael Presson + +#include "b3DynamicBvhBroadphase.h" +#include "b3OverlappingPair.h" + +// +// Profiling +// + +#if B3_DBVT_BP_PROFILE||B3_DBVT_BP_ENABLE_BENCHMARK +#include <stdio.h> +#endif + +#if B3_DBVT_BP_PROFILE +struct b3ProfileScope +{ + __forceinline b3ProfileScope(b3Clock& clock,unsigned long& value) : + m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) + { + } + __forceinline ~b3ProfileScope() + { + (*m_value)+=m_clock->getTimeMicroseconds()-m_base; + } + b3Clock* m_clock; + unsigned long* m_value; + unsigned long m_base; +}; +#define b3SPC(_value_) b3ProfileScope spc_scope(m_clock,_value_) +#else +#define b3SPC(_value_) +#endif + +// +// Helpers +// + +// +template <typename T> +static inline void b3ListAppend(T* item,T*& list) +{ + item->links[0]=0; + item->links[1]=list; + if(list) list->links[0]=item; + list=item; +} + +// +template <typename T> +static inline void b3ListRemove(T* item,T*& list) +{ + if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; + if(item->links[1]) item->links[1]->links[0]=item->links[0]; +} + +// +template <typename T> +static inline int b3ListCount(T* root) +{ + int n=0; + while(root) { ++n;root=root->links[1]; } + return(n); +} + +// +template <typename T> +static inline void b3Clear(T& value) +{ + static const struct ZeroDummy : T {} zerodummy; + value=zerodummy; +} + +// +// Colliders +// + +/* Tree collider */ +struct b3DbvtTreeCollider : b3DynamicBvh::ICollide +{ + b3DynamicBvhBroadphase* pbp; + b3DbvtProxy* proxy; + b3DbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {} + void Process(const b3DbvtNode* na,const b3DbvtNode* nb) + { + if(na!=nb) + { + b3DbvtProxy* pa=(b3DbvtProxy*)na->data; + b3DbvtProxy* pb=(b3DbvtProxy*)nb->data; +#if B3_DBVT_BP_SORTPAIRS + if(pa->m_uniqueId>pb->m_uniqueId) + b3Swap(pa,pb); +#endif + pbp->m_paircache->addOverlappingPair(pa->getUid(),pb->getUid()); + ++pbp->m_newpairs; + } + } + void Process(const b3DbvtNode* n) + { + Process(n,proxy->leaf); + } +}; + +// +// b3DynamicBvhBroadphase +// + +// +b3DynamicBvhBroadphase::b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache) +{ + m_deferedcollide = false; + m_needcleanup = true; + m_releasepaircache = (paircache!=0)?false:true; + m_prediction = 0; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + m_paircache = paircache? paircache : new(b3AlignedAlloc(sizeof(b3HashedOverlappingPairCache),16)) b3HashedOverlappingPairCache(); + + m_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } +#if B3_DBVT_BP_PROFILE + b3Clear(m_profiling); +#endif + m_proxies.resize(proxyCapacity); +} + +// +b3DynamicBvhBroadphase::~b3DynamicBvhBroadphase() +{ + if(m_releasepaircache) + { + m_paircache->~b3OverlappingPairCache(); + b3AlignedFree(m_paircache); + } +} + +// +b3BroadphaseProxy* b3DynamicBvhBroadphase::createProxy( const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + int objectId, + void* userPtr, + int collisionFilterGroup, + int collisionFilterMask) +{ + b3DbvtProxy* mem = &m_proxies[objectId]; + b3DbvtProxy* proxy=new(mem) b3DbvtProxy( aabbMin,aabbMax,userPtr, + collisionFilterGroup, + collisionFilterMask); + + b3DbvtAabbMm aabb = b3DbvtVolume::FromMM(aabbMin,aabbMax); + + //bproxy->aabb = b3DbvtVolume::FromMM(aabbMin,aabbMax); + proxy->stage = m_stageCurrent; + proxy->m_uniqueId = objectId; + proxy->leaf = m_sets[0].insert(aabb,proxy); + b3ListAppend(proxy,m_stageRoots[m_stageCurrent]); + if(!m_deferedcollide) + { + b3DbvtTreeCollider collider(this); + collider.proxy=proxy; + m_sets[0].collideTV(m_sets[0].m_root,aabb,collider); + m_sets[1].collideTV(m_sets[1].m_root,aabb,collider); + } + return(proxy); +} + +// +void b3DynamicBvhBroadphase::destroyProxy( b3BroadphaseProxy* absproxy, + b3Dispatcher* dispatcher) +{ + b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy; + if(proxy->stage==STAGECOUNT) + m_sets[1].remove(proxy->leaf); + else + m_sets[0].remove(proxy->leaf); + b3ListRemove(proxy,m_stageRoots[proxy->stage]); + m_paircache->removeOverlappingPairsContainingProxy(proxy->getUid(),dispatcher); + + m_needcleanup=true; +} + +void b3DynamicBvhBroadphase::getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const +{ + const b3DbvtProxy* proxy=&m_proxies[objectId]; + aabbMin = proxy->m_aabbMin; + aabbMax = proxy->m_aabbMax; +} +/* +void b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const +{ + b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy; + aabbMin = proxy->m_aabbMin; + aabbMax = proxy->m_aabbMax; +} +*/ + + +struct BroadphaseRayTester : b3DynamicBvh::ICollide +{ + b3BroadphaseRayCallback& m_rayCallback; + BroadphaseRayTester(b3BroadphaseRayCallback& orgCallback) + :m_rayCallback(orgCallback) + { + } + void Process(const b3DbvtNode* leaf) + { + b3DbvtProxy* proxy=(b3DbvtProxy*)leaf->data; + m_rayCallback.process(proxy); + } +}; + +void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback,const b3Vector3& aabbMin,const b3Vector3& aabbMax) +{ + BroadphaseRayTester callback(rayCallback); + + m_sets[0].rayTestInternal( m_sets[0].m_root, + rayFrom, + rayTo, + rayCallback.m_rayDirectionInverse, + rayCallback.m_signs, + rayCallback.m_lambda_max, + aabbMin, + aabbMax, + callback); + + m_sets[1].rayTestInternal( m_sets[1].m_root, + rayFrom, + rayTo, + rayCallback.m_rayDirectionInverse, + rayCallback.m_signs, + rayCallback.m_lambda_max, + aabbMin, + aabbMax, + callback); + +} + + +struct BroadphaseAabbTester : b3DynamicBvh::ICollide +{ + b3BroadphaseAabbCallback& m_aabbCallback; + BroadphaseAabbTester(b3BroadphaseAabbCallback& orgCallback) + :m_aabbCallback(orgCallback) + { + } + void Process(const b3DbvtNode* leaf) + { + b3DbvtProxy* proxy=(b3DbvtProxy*)leaf->data; + m_aabbCallback.process(proxy); + } +}; + +void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3BroadphaseAabbCallback& aabbCallback) +{ + BroadphaseAabbTester callback(aabbCallback); + + const B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds=b3DbvtVolume::FromMM(aabbMin,aabbMax); + //process all children, that overlap with the given AABB bounds + m_sets[0].collideTV(m_sets[0].m_root,bounds,callback); + m_sets[1].collideTV(m_sets[1].m_root,bounds,callback); + +} + + + +// +void b3DynamicBvhBroadphase::setAabb(int objectId, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + b3Dispatcher* /*dispatcher*/) +{ + b3DbvtProxy* proxy=&m_proxies[objectId]; +// b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy; + B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax); +#if B3_DBVT_BP_PREVENTFALSEUPDATE + if(b3NotEqual(aabb,proxy->leaf->volume)) +#endif + { + bool docollide=false; + if(proxy->stage==STAGECOUNT) + {/* fixed -> dynamic set */ + m_sets[1].remove(proxy->leaf); + proxy->leaf=m_sets[0].insert(aabb,proxy); + docollide=true; + } + else + {/* dynamic set */ + ++m_updates_call; + if(b3Intersect(proxy->leaf->volume,aabb)) + {/* Moving */ + + const b3Vector3 delta=aabbMin-proxy->m_aabbMin; + b3Vector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction); + if(delta[0]<0) velocity[0]=-velocity[0]; + if(delta[1]<0) velocity[1]=-velocity[1]; + if(delta[2]<0) velocity[2]=-velocity[2]; + if ( +#ifdef B3_DBVT_BP_MARGIN + m_sets[0].update(proxy->leaf,aabb,velocity,B3_DBVT_BP_MARGIN) +#else + m_sets[0].update(proxy->leaf,aabb,velocity) +#endif + ) + { + ++m_updates_done; + docollide=true; + } + } + else + {/* Teleporting */ + m_sets[0].update(proxy->leaf,aabb); + ++m_updates_done; + docollide=true; + } + } + b3ListRemove(proxy,m_stageRoots[proxy->stage]); + proxy->m_aabbMin = aabbMin; + proxy->m_aabbMax = aabbMax; + proxy->stage = m_stageCurrent; + b3ListAppend(proxy,m_stageRoots[m_stageCurrent]); + if(docollide) + { + m_needcleanup=true; + if(!m_deferedcollide) + { + b3DbvtTreeCollider collider(this); + m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); + } + } + } +} + + +// +void b3DynamicBvhBroadphase::setAabbForceUpdate( b3BroadphaseProxy* absproxy, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + b3Dispatcher* /*dispatcher*/) +{ + b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy; + B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax); + bool docollide=false; + if(proxy->stage==STAGECOUNT) + {/* fixed -> dynamic set */ + m_sets[1].remove(proxy->leaf); + proxy->leaf=m_sets[0].insert(aabb,proxy); + docollide=true; + } + else + {/* dynamic set */ + ++m_updates_call; + /* Teleporting */ + m_sets[0].update(proxy->leaf,aabb); + ++m_updates_done; + docollide=true; + } + b3ListRemove(proxy,m_stageRoots[proxy->stage]); + proxy->m_aabbMin = aabbMin; + proxy->m_aabbMax = aabbMax; + proxy->stage = m_stageCurrent; + b3ListAppend(proxy,m_stageRoots[m_stageCurrent]); + if(docollide) + { + m_needcleanup=true; + if(!m_deferedcollide) + { + b3DbvtTreeCollider collider(this); + m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); + } + } +} + +// +void b3DynamicBvhBroadphase::calculateOverlappingPairs(b3Dispatcher* dispatcher) +{ + collide(dispatcher); +#if B3_DBVT_BP_PROFILE + if(0==(m_pid%B3_DBVT_BP_PROFILING_RATE)) + { + printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs()); + unsigned int total=m_profiling.m_total; + if(total<=0) total=1; + printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/B3_DBVT_BP_PROFILING_RATE); + printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/B3_DBVT_BP_PROFILING_RATE); + printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/B3_DBVT_BP_PROFILING_RATE); + printf("total: %uus\r\n",total/B3_DBVT_BP_PROFILING_RATE); + const unsigned long sum=m_profiling.m_ddcollide+ + m_profiling.m_fdcollide+ + m_profiling.m_cleanup; + printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/B3_DBVT_BP_PROFILING_RATE); + printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*B3_DBVT_BP_PROFILING_RATE)); + b3Clear(m_profiling); + m_clock.reset(); + } +#endif + + performDeferredRemoval(dispatcher); + +} + +void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher) +{ + + if (m_paircache->hasDeferredRemoval()) + { + + b3BroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray(); + + //perform a sort, to find duplicates and to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(b3BroadphasePairSortPredicate()); + + int invalidPair = 0; + + + int i; + + b3BroadphasePair previousPair = b3MakeBroadphasePair(-1,-1); + + + + for (i=0;i<overlappingPairArray.size();i++) + { + + b3BroadphasePair& pair = overlappingPairArray[i]; + + bool isDuplicate = (pair == previousPair); + + previousPair = pair; + + bool needsRemoval = false; + + if (!isDuplicate) + { + //important to perform AABB check that is consistent with the broadphase + b3DbvtProxy* pa=&m_proxies[pair.x]; + b3DbvtProxy* pb=&m_proxies[pair.y]; + bool hasOverlap = b3Intersect(pa->leaf->volume,pb->leaf->volume); + + if (hasOverlap) + { + needsRemoval = false; + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + } + + if (needsRemoval) + { + m_paircache->cleanOverlappingPair(pair,dispatcher); + + pair.x = -1; + pair.y = -1; + invalidPair++; + } + + } + + //perform a sort, to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(b3BroadphasePairSortPredicate()); + overlappingPairArray.resize(overlappingPairArray.size() - invalidPair); + } +} + +// +void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher) +{ + /*printf("---------------------------------------------------------\n"); + printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves); + printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves); + printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs()); + { + int i; + for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++) + { + printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(), + getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid()); + } + printf("\n"); + } +*/ + + + + b3SPC(m_profiling.m_total); + /* optimize */ + m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); + if(m_fixedleft) + { + const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; + m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); + m_fixedleft=b3Max<int>(0,m_fixedleft-count); + } + /* dynamic -> fixed set */ + m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; + b3DbvtProxy* current=m_stageRoots[m_stageCurrent]; + if(current) + { + b3DbvtTreeCollider collider(this); + do { + b3DbvtProxy* next=current->links[1]; + b3ListRemove(current,m_stageRoots[current->stage]); + b3ListAppend(current,m_stageRoots[STAGECOUNT]); +#if B3_DBVT_BP_ACCURATESLEEPING + m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); + collider.proxy=current; + b3DynamicBvh::collideTV(m_sets[0].m_root,current->aabb,collider); + b3DynamicBvh::collideTV(m_sets[1].m_root,current->aabb,collider); +#endif + m_sets[0].remove(current->leaf); + B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) curAabb=b3DbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); + current->leaf = m_sets[1].insert(curAabb,current); + current->stage = STAGECOUNT; + current = next; + } while(current); + m_fixedleft=m_sets[1].m_leaves; + m_needcleanup=true; + } + /* collide dynamics */ + { + b3DbvtTreeCollider collider(this); + if(m_deferedcollide) + { + b3SPC(m_profiling.m_fdcollide); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider); + } + if(m_deferedcollide) + { + b3SPC(m_profiling.m_ddcollide); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider); + } + } + /* clean up */ + if(m_needcleanup) + { + b3SPC(m_profiling.m_cleanup); + b3BroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); + if(pairs.size()>0) + { + + int ni=b3Min(pairs.size(),b3Max<int>(m_newpairs,(pairs.size()*m_cupdates)/100)); + for(int i=0;i<ni;++i) + { + b3BroadphasePair& p=pairs[(m_cid+i)%pairs.size()]; + b3DbvtProxy* pa=&m_proxies[p.x]; + b3DbvtProxy* pb=&m_proxies[p.y]; + if(!b3Intersect(pa->leaf->volume,pb->leaf->volume)) + { +#if B3_DBVT_BP_SORTPAIRS + if(pa->m_uniqueId>pb->m_uniqueId) + b3Swap(pa,pb); +#endif + m_paircache->removeOverlappingPair(pa->getUid(),pb->getUid(),dispatcher); + --ni;--i; + } + } + if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; + } + } + ++m_pid; + m_newpairs=1; + m_needcleanup=false; + if(m_updates_call>0) + { m_updates_ratio=m_updates_done/(b3Scalar)m_updates_call; } + else + { m_updates_ratio=0; } + m_updates_done/=2; + m_updates_call/=2; +} + +// +void b3DynamicBvhBroadphase::optimize() +{ + m_sets[0].optimizeTopDown(); + m_sets[1].optimizeTopDown(); +} + +// +b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() +{ + return(m_paircache); +} + +// +const b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() const +{ + return(m_paircache); +} + +// +void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const +{ + + B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds; + + if(!m_sets[0].empty()) + if(!m_sets[1].empty()) b3Merge( m_sets[0].m_root->volume, + m_sets[1].m_root->volume,bounds); + else + bounds=m_sets[0].m_root->volume; + else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; + else + bounds=b3DbvtVolume::FromCR(b3MakeVector3(0,0,0),0); + aabbMin=bounds.Mins(); + aabbMax=bounds.Maxs(); +} + +void b3DynamicBvhBroadphase::resetPool(b3Dispatcher* dispatcher) +{ + + int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves; + if (!totalObjects) + { + //reset internal dynamic tree data structures + m_sets[0].clear(); + m_sets[1].clear(); + + m_deferedcollide = false; + m_needcleanup = true; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + + m_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } + } +} + +// +void b3DynamicBvhBroadphase::printStats() +{} + +// +#if B3_DBVT_BP_ENABLE_BENCHMARK + +struct b3BroadphaseBenchmark +{ + struct Experiment + { + const char* name; + int object_count; + int update_count; + int spawn_count; + int iterations; + b3Scalar speed; + b3Scalar amplitude; + }; + struct Object + { + b3Vector3 center; + b3Vector3 extents; + b3BroadphaseProxy* proxy; + b3Scalar time; + void update(b3Scalar speed,b3Scalar amplitude,b3BroadphaseInterface* pbi) + { + time += speed; + center[0] = b3Cos(time*(b3Scalar)2.17)*amplitude+ + b3Sin(time)*amplitude/2; + center[1] = b3Cos(time*(b3Scalar)1.38)*amplitude+ + b3Sin(time)*amplitude; + center[2] = b3Sin(time*(b3Scalar)0.777)*amplitude; + pbi->setAabb(proxy,center-extents,center+extents,0); + } + }; + static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } + static b3Scalar UnitRand() { return(UnsignedRand(16384)/(b3Scalar)16384); } + static void OutputTime(const char* name,b3Clock& c,unsigned count=0) + { + const unsigned long us=c.getTimeMicroseconds(); + const unsigned long ms=(us+500)/1000; + const b3Scalar sec=us/(b3Scalar)(1000*1000); + if(count>0) + printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec); + else + printf("%s : %u us (%u ms)\r\n",name,us,ms); + } +}; + +void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface* pbi) +{ + static const b3BroadphaseBenchmark::Experiment experiments[]= + { + {"1024o.10%",1024,10,0,8192,(b3Scalar)0.005,(b3Scalar)100}, + /*{"4096o.10%",4096,10,0,8192,(b3Scalar)0.005,(b3Scalar)100}, + {"8192o.10%",8192,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},*/ + }; + static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); + b3AlignedObjectArray<b3BroadphaseBenchmark::Object*> objects; + b3Clock wallclock; + /* Begin */ + for(int iexp=0;iexp<nexperiments;++iexp) + { + const b3BroadphaseBenchmark::Experiment& experiment=experiments[iexp]; + const int object_count=experiment.object_count; + const int update_count=(object_count*experiment.update_count)/100; + const int spawn_count=(object_count*experiment.spawn_count)/100; + const b3Scalar speed=experiment.speed; + const b3Scalar amplitude=experiment.amplitude; + printf("Experiment #%u '%s':\r\n",iexp,experiment.name); + printf("\tObjects: %u\r\n",object_count); + printf("\tUpdate: %u\r\n",update_count); + printf("\tSpawn: %u\r\n",spawn_count); + printf("\tSpeed: %f\r\n",speed); + printf("\tAmplitude: %f\r\n",amplitude); + srand(180673); + /* Create objects */ + wallclock.reset(); + objects.reserve(object_count); + for(int i=0;i<object_count;++i) + { + b3BroadphaseBenchmark::Object* po=new b3BroadphaseBenchmark::Object(); + po->center[0]=b3BroadphaseBenchmark::UnitRand()*50; + po->center[1]=b3BroadphaseBenchmark::UnitRand()*50; + po->center[2]=b3BroadphaseBenchmark::UnitRand()*50; + po->extents[0]=b3BroadphaseBenchmark::UnitRand()*2+2; + po->extents[1]=b3BroadphaseBenchmark::UnitRand()*2+2; + po->extents[2]=b3BroadphaseBenchmark::UnitRand()*2+2; + po->time=b3BroadphaseBenchmark::UnitRand()*2000; + po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); + objects.push_back(po); + } + b3BroadphaseBenchmark::OutputTime("\tInitialization",wallclock); + /* First update */ + wallclock.reset(); + for(int i=0;i<objects.size();++i) + { + objects[i]->update(speed,amplitude,pbi); + } + b3BroadphaseBenchmark::OutputTime("\tFirst update",wallclock); + /* Updates */ + wallclock.reset(); + for(int i=0;i<experiment.iterations;++i) + { + for(int j=0;j<update_count;++j) + { + objects[j]->update(speed,amplitude,pbi); + } + pbi->calculateOverlappingPairs(0); + } + b3BroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); + /* Clean up */ + wallclock.reset(); + for(int i=0;i<objects.size();++i) + { + pbi->destroyProxy(objects[i]->proxy,0); + delete objects[i]; + } + objects.resize(0); + b3BroadphaseBenchmark::OutputTime("\tRelease",wallclock); + } + +} +#else +/*void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface*) +{} +*/ +#endif + +#if B3_DBVT_BP_PROFILE +#undef b3SPC +#endif + diff --git a/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h new file mode 100644 index 0000000000..7ac085d90c --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h @@ -0,0 +1,206 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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. +*/ + +///b3DynamicBvhBroadphase implementation by Nathanael Presson +#ifndef B3_DBVT_BROADPHASE_H +#define B3_DBVT_BROADPHASE_H + +#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h" +#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +#include "b3BroadphaseCallback.h" + +// +// Compile time config +// + +#define B3_DBVT_BP_PROFILE 0 +//#define B3_DBVT_BP_SORTPAIRS 1 +#define B3_DBVT_BP_PREVENTFALSEUPDATE 0 +#define B3_DBVT_BP_ACCURATESLEEPING 0 +#define B3_DBVT_BP_ENABLE_BENCHMARK 0 +#define B3_DBVT_BP_MARGIN (b3Scalar)0.05 + +#if B3_DBVT_BP_PROFILE +#define B3_DBVT_BP_PROFILING_RATE 256 + +#endif + + + + +B3_ATTRIBUTE_ALIGNED16(struct) b3BroadphaseProxy +{ + +B3_DECLARE_ALIGNED_ALLOCATOR(); + + ///optional filtering to cull potential collisions + enum CollisionFilterGroups + { + DefaultFilter = 1, + StaticFilter = 2, + KinematicFilter = 4, + DebrisFilter = 8, + SensorTrigger = 16, + CharacterFilter = 32, + AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger + }; + + //Usually the client b3CollisionObject or Rigidbody class + void* m_clientObject; + int m_collisionFilterGroup; + int m_collisionFilterMask; + int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. + + b3Vector3 m_aabbMin; + b3Vector3 m_aabbMax; + + B3_FORCE_INLINE int getUid() const + { + return m_uniqueId; + } + + //used for memory pools + b3BroadphaseProxy() :m_clientObject(0) + { + } + + b3BroadphaseProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask) + :m_clientObject(userPtr), + m_collisionFilterGroup(collisionFilterGroup), + m_collisionFilterMask(collisionFilterMask), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax) + { + } +}; + + + + + +// +// b3DbvtProxy +// +struct b3DbvtProxy : b3BroadphaseProxy +{ + /* Fields */ + //b3DbvtAabbMm aabb; + b3DbvtNode* leaf; + b3DbvtProxy* links[2]; + int stage; + /* ctor */ + + explicit b3DbvtProxy() {} + b3DbvtProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask) : + b3BroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) + { + links[0]=links[1]=0; + } +}; + +typedef b3AlignedObjectArray<b3DbvtProxy*> b3DbvtProxyArray; + +///The b3DynamicBvhBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see b3DynamicBvh). +///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other. +///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases b3AxisSweep3 and b332BitAxisSweep3. +struct b3DynamicBvhBroadphase +{ + /* Config */ + enum { + DYNAMIC_SET = 0, /* Dynamic set index */ + FIXED_SET = 1, /* Fixed set index */ + STAGECOUNT = 2 /* Number of stages */ + }; + /* Fields */ + b3DynamicBvh m_sets[2]; // Dbvt sets + b3DbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list + + b3AlignedObjectArray<b3DbvtProxy> m_proxies; + b3OverlappingPairCache* m_paircache; // Pair cache + b3Scalar m_prediction; // Velocity prediction + int m_stageCurrent; // Current stage + int m_fupdates; // % of fixed updates per frame + int m_dupdates; // % of dynamic updates per frame + int m_cupdates; // % of cleanup updates per frame + int m_newpairs; // Number of pairs created + int m_fixedleft; // Fixed optimization left + unsigned m_updates_call; // Number of updates call + unsigned m_updates_done; // Number of updates done + b3Scalar m_updates_ratio; // m_updates_done/m_updates_call + int m_pid; // Parse id + int m_cid; // Cleanup index + bool m_releasepaircache; // Release pair cache on delete + bool m_deferedcollide; // Defere dynamic/static collision to collide call + bool m_needcleanup; // Need to run cleanup? +#if B3_DBVT_BP_PROFILE + b3Clock m_clock; + struct { + unsigned long m_total; + unsigned long m_ddcollide; + unsigned long m_fdcollide; + unsigned long m_cleanup; + unsigned long m_jobcount; + } m_profiling; +#endif + /* Methods */ + b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache=0); + virtual ~b3DynamicBvhBroadphase(); + void collide(b3Dispatcher* dispatcher); + void optimize(); + + /* b3BroadphaseInterface Implementation */ + b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int objectIndex,void* userPtr, int collisionFilterGroup, int collisionFilterMask); + virtual void destroyProxy(b3BroadphaseProxy* proxy,b3Dispatcher* dispatcher); + virtual void setAabb(int objectId,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher); + virtual void rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3MakeVector3(0,0,0), const b3Vector3& aabbMax = b3MakeVector3(0,0,0)); + virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback); + + //virtual void getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const; + virtual void getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const; + virtual void calculateOverlappingPairs(b3Dispatcher* dispatcher=0); + virtual b3OverlappingPairCache* getOverlappingPairCache(); + virtual const b3OverlappingPairCache* getOverlappingPairCache() const; + virtual void getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const; + virtual void printStats(); + + + ///reset broadphase internal structures, to ensure determinism/reproducability + virtual void resetPool(b3Dispatcher* dispatcher); + + void performDeferredRemoval(b3Dispatcher* dispatcher); + + void setVelocityPrediction(b3Scalar prediction) + { + m_prediction = prediction; + } + b3Scalar getVelocityPrediction() const + { + return m_prediction; + } + + ///this setAabbForceUpdate is similar to setAabb but always forces the aabb update. + ///it is not part of the b3BroadphaseInterface but specific to b3DynamicBvhBroadphase. + ///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see + ///http://code.google.com/p/bullet/issues/detail?id=223 + void setAabbForceUpdate( b3BroadphaseProxy* absproxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* /*dispatcher*/); + + //static void benchmark(b3BroadphaseInterface*); + + +}; + +#endif diff --git a/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h new file mode 100644 index 0000000000..39bf27de3e --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h @@ -0,0 +1,72 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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. +*/ + +#ifndef B3_OVERLAPPING_PAIR_H +#define B3_OVERLAPPING_PAIR_H + +#include "Bullet3Common/shared/b3Int4.h" + +#define B3_NEW_PAIR_MARKER -1 +#define B3_REMOVED_PAIR_MARKER -2 + +typedef b3Int4 b3BroadphasePair; + +inline b3Int4 b3MakeBroadphasePair(int xx,int yy) +{ + b3Int4 pair; + + if (xx < yy) + { + pair.x = xx; + pair.y = yy; + } + else + { + pair.x = yy; + pair.y = xx; + } + pair.z = B3_NEW_PAIR_MARKER; + pair.w = B3_NEW_PAIR_MARKER; + return pair; +} + +/*struct b3BroadphasePair : public b3Int4 +{ + explicit b3BroadphasePair(){} + +}; +*/ + +class b3BroadphasePairSortPredicate +{ + public: + + bool operator() ( const b3BroadphasePair& a, const b3BroadphasePair& b ) const + { + const int uidA0 = a.x; + const int uidB0 = b.x; + const int uidA1 = a.y; + const int uidB1 = b.y; + return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1); + } +}; + +B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b) +{ + return (a.x == b.x ) && (a.y == b.y ); +} + +#endif //B3_OVERLAPPING_PAIR_H + diff --git a/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp new file mode 100644 index 0000000000..e4bda61624 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp @@ -0,0 +1,638 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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. +*/ + + + +#include "b3OverlappingPairCache.h" + +//#include "b3Dispatcher.h" +//#include "b3CollisionAlgorithm.h" +#include "Bullet3Geometry/b3AabbUtil.h" + +#include <stdio.h> + +int b3g_overlappingPairs = 0; +int b3g_removePairs =0; +int b3g_addedPairs =0; +int b3g_findPairs =0; + + + + +b3HashedOverlappingPairCache::b3HashedOverlappingPairCache(): + m_overlapFilterCallback(0) +//, m_blockedForChanges(false) +{ + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); +} + + + + +b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache() +{ +} + + + +void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher) +{ +/* if (pair.m_algorithm) + { + { + pair.m_algorithm->~b3CollisionAlgorithm(); + dispatcher->freeCollisionAlgorithm(pair.m_algorithm); + pair.m_algorithm=0; + } + } + */ + +} + + + + +void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher) +{ + + class CleanPairCallback : public b3OverlapCallback + { + int m_cleanProxy; + b3OverlappingPairCache* m_pairCache; + b3Dispatcher* m_dispatcher; + + public: + CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher) + :m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) + { + } + virtual bool processOverlap(b3BroadphasePair& pair) + { + if ((pair.x == m_cleanProxy) || + (pair.y == m_cleanProxy)) + { + m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + } + return false; + } + + }; + + CleanPairCallback cleanPairs(proxy,this,dispatcher); + + processAllOverlappingPairs(&cleanPairs,dispatcher); + +} + + + + +void b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher) +{ + + class RemovePairCallback : public b3OverlapCallback + { + int m_obsoleteProxy; + + public: + RemovePairCallback(int obsoleteProxy) + :m_obsoleteProxy(obsoleteProxy) + { + } + virtual bool processOverlap(b3BroadphasePair& pair) + { + return ((pair.x == m_obsoleteProxy) || + (pair.y == m_obsoleteProxy)); + } + + }; + + + RemovePairCallback removeCallback(proxy); + + processAllOverlappingPairs(&removeCallback,dispatcher); +} + + + + + +b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1) +{ + b3g_findPairs++; + if(proxy0 >proxy1) + b3Swap(proxy0,proxy1); + int proxyId1 = proxy0; + int proxyId2 = proxy1; + + /*if (proxyId1 > proxyId2) + b3Swap(proxyId1, proxyId2);*/ + + int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + + if (hash >= m_hashTable.size()) + { + return NULL; + } + + int index = m_hashTable[hash]; + while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) + { + index = m_next[index]; + } + + if (index == B3_NULL_PAIR) + { + return NULL; + } + + b3Assert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; +} + +//#include <stdio.h> + +void b3HashedOverlappingPairCache::growTables() +{ + + int newCapacity = m_overlappingPairArray.capacity(); + + if (m_hashTable.size() < newCapacity) + { + //grow hashtable and next table + int curHashtableSize = m_hashTable.size(); + + m_hashTable.resize(newCapacity); + m_next.resize(newCapacity); + + + int i; + + for (i= 0; i < newCapacity; ++i) + { + m_hashTable[i] = B3_NULL_PAIR; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = B3_NULL_PAIR; + } + + for(i=0;i<curHashtableSize;i++) + { + + const b3BroadphasePair& pair = m_overlappingPairArray[i]; + int proxyId1 = pair.x; + int proxyId2 = pair.y; + /*if (proxyId1 > proxyId2) + b3Swap(proxyId1, proxyId2);*/ + int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + m_next[i] = m_hashTable[hashValue]; + m_hashTable[hashValue] = i; + } + + + } +} + +b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1) +{ + if(proxy0>proxy1) + b3Swap(proxy0,proxy1); + int proxyId1 = proxy0; + int proxyId2 = proxy1; + + /*if (proxyId1 > proxyId2) + b3Swap(proxyId1, proxyId2);*/ + + int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + + + b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); + if (pair != NULL) + { + return pair; + } + /*for(int i=0;i<m_overlappingPairArray.size();++i) + { + if( (m_overlappingPairArray[i].m_pProxy0==proxy0)&& + (m_overlappingPairArray[i].m_pProxy1==proxy1)) + { + printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2); + internalFindPair(proxy0, proxy1, hash); + } + }*/ + int count = m_overlappingPairArray.size(); + int oldCapacity = m_overlappingPairArray.capacity(); + pair = &m_overlappingPairArray.expandNonInitializing(); + + //this is where we add an actual pair, so also call the 'ghost' +// if (m_ghostPairCallback) +// m_ghostPairCallback->addOverlappingPair(proxy0,proxy1); + + int newCapacity = m_overlappingPairArray.capacity(); + + if (oldCapacity < newCapacity) + { + growTables(); + //hash with new capacity + hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + } + + *pair = b3MakeBroadphasePair(proxy0,proxy1); + +// pair->m_pProxy0 = proxy0; +// pair->m_pProxy1 = proxy1; + //pair->m_algorithm = 0; + //pair->m_internalTmpValue = 0; + + + m_next[count] = m_hashTable[hash]; + m_hashTable[hash] = count; + + return pair; +} + + + +void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1,b3Dispatcher* dispatcher) +{ + b3g_removePairs++; + if(proxy0>proxy1) + b3Swap(proxy0,proxy1); + int proxyId1 = proxy0; + int proxyId2 = proxy1; + + /*if (proxyId1 > proxyId2) + b3Swap(proxyId1, proxyId2);*/ + + int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + + b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); + if (pair == NULL) + { + return 0; + } + + cleanOverlappingPair(*pair,dispatcher); + + + + int pairIndex = int(pair - &m_overlappingPairArray[0]); + b3Assert(pairIndex < m_overlappingPairArray.size()); + + // Remove the pair from the hash table. + int index = m_hashTable[hash]; + b3Assert(index != B3_NULL_PAIR); + + int previous = B3_NULL_PAIR; + while (index != pairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != B3_NULL_PAIR) + { + b3Assert(m_next[previous] == pairIndex); + m_next[previous] = m_next[pairIndex]; + } + else + { + m_hashTable[hash] = m_next[pairIndex]; + } + + // We now move the last pair into spot of the + // pair being removed. We need to fix the hash + // table indices to support the move. + + int lastPairIndex = m_overlappingPairArray.size() - 1; + + //if (m_ghostPairCallback) + // m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); + + // If the removed pair is the last pair, we are done. + if (lastPairIndex == pairIndex) + { + m_overlappingPairArray.pop_back(); + return 0; + } + + // Remove the last pair from the hash table. + const b3BroadphasePair* last = &m_overlappingPairArray[lastPairIndex]; + /* missing swap here too, Nat. */ + int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->x), static_cast<unsigned int>(last->y)) & (m_overlappingPairArray.capacity()-1)); + + index = m_hashTable[lastHash]; + b3Assert(index != B3_NULL_PAIR); + + previous = B3_NULL_PAIR; + while (index != lastPairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != B3_NULL_PAIR) + { + b3Assert(m_next[previous] == lastPairIndex); + m_next[previous] = m_next[lastPairIndex]; + } + else + { + m_hashTable[lastHash] = m_next[lastPairIndex]; + } + + // Copy the last pair into the remove pair's spot. + m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; + + // Insert the last pair into the hash table + m_next[pairIndex] = m_hashTable[lastHash]; + m_hashTable[lastHash] = pairIndex; + + m_overlappingPairArray.pop_back(); + + return 0; +} +//#include <stdio.h> + +void b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher) +{ + + int i; + +// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size()); + for (i=0;i<m_overlappingPairArray.size();) + { + + b3BroadphasePair* pair = &m_overlappingPairArray[i]; + if (callback->processOverlap(*pair)) + { + removeOverlappingPair(pair->x,pair->y,dispatcher); + + b3g_overlappingPairs--; + } else + { + i++; + } + } +} + + + + + +void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher) +{ + ///need to keep hashmap in sync with pair address, so rebuild all + b3BroadphasePairArray tmpPairs; + int i; + for (i=0;i<m_overlappingPairArray.size();i++) + { + tmpPairs.push_back(m_overlappingPairArray[i]); + } + + for (i=0;i<tmpPairs.size();i++) + { + removeOverlappingPair(tmpPairs[i].x,tmpPairs[i].y,dispatcher); + } + + for (i = 0; i < m_next.size(); i++) + { + m_next[i] = B3_NULL_PAIR; + } + + tmpPairs.quickSort(b3BroadphasePairSortPredicate()); + + for (i=0;i<tmpPairs.size();i++) + { + addOverlappingPair(tmpPairs[i].x ,tmpPairs[i].y); + } + + +} + + +void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1, b3Dispatcher* dispatcher ) +{ + if (!hasDeferredRemoval()) + { + b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0,proxy1); + + + int findIndex = m_overlappingPairArray.findLinearSearch(findPair); + if (findIndex < m_overlappingPairArray.size()) + { + b3g_overlappingPairs--; + b3BroadphasePair& pair = m_overlappingPairArray[findIndex]; + + cleanOverlappingPair(pair,dispatcher); + //if (m_ghostPairCallback) + // m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); + + m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1); + m_overlappingPairArray.pop_back(); + return 0; + } + } + + return 0; +} + + + + + + + + +b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,int proxy1) +{ + //don't add overlap with own + b3Assert(proxy0 != proxy1); + + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing(); + *pair = b3MakeBroadphasePair(proxy0,proxy1); + + + b3g_overlappingPairs++; + b3g_addedPairs++; + +// if (m_ghostPairCallback) +// m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); + return pair; + +} + +///this findPair becomes really slow. Either sort the list to speedup the query, or +///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. +///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) +///Also we can use a 2D bitmap, which can be useful for a future GPU implementation + b3BroadphasePair* b3SortedOverlappingPairCache::findPair(int proxy0,int proxy1) +{ + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0,proxy1); + int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair); + + if (findIndex < m_overlappingPairArray.size()) + { + //b3Assert(it != m_overlappingPairSet.end()); + b3BroadphasePair* pair = &m_overlappingPairArray[findIndex]; + return pair; + } + return 0; +} + + + + + + + + + + +//#include <stdio.h> + +void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher) +{ + + int i; + + for (i=0;i<m_overlappingPairArray.size();) + { + + b3BroadphasePair* pair = &m_overlappingPairArray[i]; + if (callback->processOverlap(*pair)) + { + cleanOverlappingPair(*pair,dispatcher); + pair->x = -1; + pair->y = -1; + m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + m_overlappingPairArray.pop_back(); + b3g_overlappingPairs--; + } else + { + i++; + } + } +} + + + + +b3SortedOverlappingPairCache::b3SortedOverlappingPairCache(): + m_blockedForChanges(false), + m_hasDeferredRemoval(true), + m_overlapFilterCallback(0) + +{ + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); +} + +b3SortedOverlappingPairCache::~b3SortedOverlappingPairCache() +{ +} + +void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher) +{ +/* if (pair.m_algorithm) + { + { + pair.m_algorithm->~b3CollisionAlgorithm(); + dispatcher->freeCollisionAlgorithm(pair.m_algorithm); + pair.m_algorithm=0; + b3g_removePairs--; + } + } + */ +} + + +void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher) +{ + + class CleanPairCallback : public b3OverlapCallback + { + int m_cleanProxy; + b3OverlappingPairCache* m_pairCache; + b3Dispatcher* m_dispatcher; + + public: + CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher) + :m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) + { + } + virtual bool processOverlap(b3BroadphasePair& pair) + { + if ((pair.x == m_cleanProxy) || + (pair.y == m_cleanProxy)) + { + m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + } + return false; + } + + }; + + CleanPairCallback cleanPairs(proxy,this,dispatcher); + + processAllOverlappingPairs(&cleanPairs,dispatcher); + +} + + +void b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher) +{ + + class RemovePairCallback : public b3OverlapCallback + { + int m_obsoleteProxy; + + public: + RemovePairCallback(int obsoleteProxy) + :m_obsoleteProxy(obsoleteProxy) + { + } + virtual bool processOverlap(b3BroadphasePair& pair) + { + return ((pair.x == m_obsoleteProxy) || + (pair.y == m_obsoleteProxy)); + } + + }; + + RemovePairCallback removeCallback(proxy); + + processAllOverlappingPairs(&removeCallback,dispatcher); +} + +void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher) +{ + //should already be sorted +} + diff --git a/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h new file mode 100644 index 0000000000..f67eb676f1 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h @@ -0,0 +1,474 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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. +*/ + +#ifndef B3_OVERLAPPING_PAIR_CACHE_H +#define B3_OVERLAPPING_PAIR_CACHE_H + +#include "Bullet3Common/shared/b3Int2.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +class b3Dispatcher; +#include "b3OverlappingPair.h" + + + +typedef b3AlignedObjectArray<b3BroadphasePair> b3BroadphasePairArray; + +struct b3OverlapCallback +{ + virtual ~b3OverlapCallback() + {} + //return true for deletion of the pair + virtual bool processOverlap(b3BroadphasePair& pair) = 0; + +}; + +struct b3OverlapFilterCallback +{ + virtual ~b3OverlapFilterCallback() + {} + // return true when pairs need collision + virtual bool needBroadphaseCollision(int proxy0,int proxy1) const = 0; +}; + + + + + + + +extern int b3g_removePairs; +extern int b3g_addedPairs; +extern int b3g_findPairs; + +const int B3_NULL_PAIR=0xffffffff; + +///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the b3BroadphaseInterface broadphases. +///The b3HashedOverlappingPairCache and b3SortedOverlappingPairCache classes are two implementations. +class b3OverlappingPairCache +{ +public: + virtual ~b3OverlappingPairCache() {} // this is needed so we can get to the derived class destructor + + virtual b3BroadphasePair* getOverlappingPairArrayPtr() = 0; + + virtual const b3BroadphasePair* getOverlappingPairArrayPtr() const = 0; + + virtual b3BroadphasePairArray& getOverlappingPairArray() = 0; + + virtual void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher) = 0; + + virtual int getNumOverlappingPairs() const = 0; + + virtual void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher) = 0; + + virtual void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0; + + virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher) = 0; + + virtual b3BroadphasePair* findPair(int proxy0, int proxy1) = 0; + + virtual bool hasDeferredRemoval() = 0; + + //virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)=0; + + virtual b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1)=0; + virtual void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher)=0; + virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/)=0; + + virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) = 0; + + +}; + +/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com +class b3HashedOverlappingPairCache : public b3OverlappingPairCache +{ + b3BroadphasePairArray m_overlappingPairArray; + b3OverlapFilterCallback* m_overlapFilterCallback; +// bool m_blockedForChanges; + + +public: + b3HashedOverlappingPairCache(); + virtual ~b3HashedOverlappingPairCache(); + + + virtual void removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher); + + virtual void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher); + + B3_FORCE_INLINE bool needsBroadphaseCollision(int proxy0,int proxy1) const + { + if (m_overlapFilterCallback) + return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + + bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + //collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + + // Add a pair and return the new pair. If the pair already exists, + // no new pair is created and the old one is returned. + virtual b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1) + { + b3g_addedPairs++; + + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + return internalAddPair(proxy0,proxy1); + } + + + + void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher); + + + virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher); + + virtual b3BroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const b3BroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + b3BroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const b3BroadphasePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher); + + + + b3BroadphasePair* findPair(int proxy0, int proxy1); + + int GetCount() const { return m_overlappingPairArray.size(); } +// b3BroadphasePair* GetPairs() { return m_pairs; } + + b3OverlapFilterCallback* getOverlapFilterCallback() + { + return m_overlapFilterCallback; + } + + void setOverlapFilterCallback(b3OverlapFilterCallback* callback) + { + m_overlapFilterCallback = callback; + } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } +private: + + b3BroadphasePair* internalAddPair(int proxy0,int proxy1); + + void growTables(); + + B3_FORCE_INLINE bool equalsPair(const b3BroadphasePair& pair, int proxyId1, int proxyId2) + { + return pair.x == proxyId1 && pair.y == proxyId2; + } + + /* + // Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm + // This assumes proxyId1 and proxyId2 are 16-bit. + B3_FORCE_INLINE int getHash(int proxyId1, int proxyId2) + { + int key = (proxyId2 << 16) | proxyId1; + key = ~key + (key << 15); + key = key ^ (key >> 12); + key = key + (key << 2); + key = key ^ (key >> 4); + key = key * 2057; + key = key ^ (key >> 16); + return key; + } + */ + + + + B3_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) + { + int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16)); + // Thomas Wang's hash + + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return static_cast<unsigned int>(key); + } + + + + + + B3_FORCE_INLINE b3BroadphasePair* internalFindPair(int proxy0, int proxy1, int hash) + { + int proxyId1 = proxy0; + int proxyId2 = proxy1; + #if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat. + if (proxyId1 > proxyId2) + b3Swap(proxyId1, proxyId2); + #endif + + int index = m_hashTable[hash]; + + while( index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) + { + index = m_next[index]; + } + + if ( index == B3_NULL_PAIR ) + { + return NULL; + } + + b3Assert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; + } + + virtual bool hasDeferredRemoval() + { + return false; + } + +/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + */ + + virtual void sortOverlappingPairs(b3Dispatcher* dispatcher); + + +protected: + + b3AlignedObjectArray<int> m_hashTable; + b3AlignedObjectArray<int> m_next; +// b3OverlappingPairCallback* m_ghostPairCallback; + +}; + + + + +///b3SortedOverlappingPairCache maintains the objects with overlapping AABB +///Typically managed by the Broadphase, Axis3Sweep or b3SimpleBroadphase +class b3SortedOverlappingPairCache : public b3OverlappingPairCache +{ + protected: + //avoid brute-force finding all the time + b3BroadphasePairArray m_overlappingPairArray; + + //during the dispatch, check that user doesn't destroy/create proxy + bool m_blockedForChanges; + + ///by default, do the removal during the pair traversal + bool m_hasDeferredRemoval; + + //if set, use the callback instead of the built in filter in needBroadphaseCollision + b3OverlapFilterCallback* m_overlapFilterCallback; + +// b3OverlappingPairCallback* m_ghostPairCallback; + + public: + + b3SortedOverlappingPairCache(); + virtual ~b3SortedOverlappingPairCache(); + + virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher); + + void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher); + + void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher); + + b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1); + + b3BroadphasePair* findPair(int proxy0,int proxy1); + + + void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher); + + virtual void removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher); + + + inline bool needsBroadphaseCollision(int proxy0,int proxy1) const + { + if (m_overlapFilterCallback) + return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + + bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + //collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + + b3BroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const b3BroadphasePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + + + + b3BroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const b3BroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } + + b3OverlapFilterCallback* getOverlapFilterCallback() + { + return m_overlapFilterCallback; + } + + void setOverlapFilterCallback(b3OverlapFilterCallback* callback) + { + m_overlapFilterCallback = callback; + } + + virtual bool hasDeferredRemoval() + { + return m_hasDeferredRemoval; + } + +/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + */ + virtual void sortOverlappingPairs(b3Dispatcher* dispatcher); + + +}; + + + +///b3NullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing. +class b3NullPairCache : public b3OverlappingPairCache +{ + + b3BroadphasePairArray m_overlappingPairArray; + +public: + + virtual b3BroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + const b3BroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + b3BroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + virtual void cleanOverlappingPair(b3BroadphasePair& /*pair*/,b3Dispatcher* /*dispatcher*/) + { + + } + + virtual int getNumOverlappingPairs() const + { + return 0; + } + + virtual void cleanProxyFromPairs(int /*proxy*/,b3Dispatcher* /*dispatcher*/) + { + + } + + virtual void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/) + { + } + + virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* /*dispatcher*/) + { + } + + virtual b3BroadphasePair* findPair(int /*proxy0*/, int /*proxy1*/) + { + return 0; + } + + virtual bool hasDeferredRemoval() + { + return true; + } + +// virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */) +// { +// +// } + + virtual b3BroadphasePair* addOverlappingPair(int /*proxy0*/,int /*proxy1*/) + { + return 0; + } + + virtual void* removeOverlappingPair(int /*proxy0*/,int /*proxy1*/,b3Dispatcher* /*dispatcher*/) + { + return 0; + } + + virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/) + { + } + + virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) + { + (void) dispatcher; + } + + +}; + + +#endif //B3_OVERLAPPING_PAIR_CACHE_H + + diff --git a/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h new file mode 100644 index 0000000000..7f9bf990bf --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h @@ -0,0 +1,59 @@ + +#ifndef B3_AABB_H +#define B3_AABB_H + + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Common/shared/b3Mat3x3.h" + +typedef struct b3Aabb b3Aabb_t; + +struct b3Aabb +{ + union + { + float m_min[4]; + b3Float4 m_minVec; + int m_minIndices[4]; + }; + union + { + float m_max[4]; + b3Float4 m_maxVec; + int m_signedMaxIndices[4]; + }; +}; + +inline void b3TransformAabb2(b3Float4ConstArg localAabbMin,b3Float4ConstArg localAabbMax, float margin, + b3Float4ConstArg pos, + b3QuatConstArg orn, + b3Float4* aabbMinOut,b3Float4* aabbMaxOut) +{ + b3Float4 localHalfExtents = 0.5f*(localAabbMax-localAabbMin); + localHalfExtents+=b3MakeFloat4(margin,margin,margin,0.f); + b3Float4 localCenter = 0.5f*(localAabbMax+localAabbMin); + b3Mat3x3 m; + m = b3QuatGetRotationMatrix(orn); + b3Mat3x3 abs_b = b3AbsoluteMat3x3(m); + b3Float4 center = b3TransformPoint(localCenter,pos,orn); + + b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents,b3GetRow(abs_b,0)), + b3Dot3F4(localHalfExtents,b3GetRow(abs_b,1)), + b3Dot3F4(localHalfExtents,b3GetRow(abs_b,2)), + 0.f); + *aabbMinOut = center-extent; + *aabbMaxOut = center+extent; +} + +/// conservative test for overlap between two aabbs +inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aabbMax1, + b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2) +{ + bool overlap = true; + overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap; + overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap; + overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap; + return overlap; +} + +#endif //B3_AABB_H |