diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision')
33 files changed, 9625 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 diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Config.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Config.h new file mode 100644 index 0000000000..65d4a21613 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Config.h @@ -0,0 +1,41 @@ +#ifndef B3_CONFIG_H +#define B3_CONFIG_H + +struct b3Config +{ + int m_maxConvexBodies; + int m_maxConvexShapes; + int m_maxBroadphasePairs; + int m_maxContactCapacity; + int m_compoundPairCapacity; + + int m_maxVerticesPerFace; + int m_maxFacesPerShape; + int m_maxConvexVertices; + int m_maxConvexIndices; + int m_maxConvexUniqueEdges; + + int m_maxCompoundChildShapes; + + int m_maxTriConvexPairCapacity; + + b3Config() + :m_maxConvexBodies(128*1024), + m_maxVerticesPerFace(64), + m_maxFacesPerShape(12), + m_maxConvexVertices(8192), + m_maxConvexIndices(81920), + m_maxConvexUniqueEdges(8192), + m_maxCompoundChildShapes(8192), + m_maxTriConvexPairCapacity(256*1024) + { + m_maxConvexShapes = m_maxConvexBodies; + m_maxBroadphasePairs = 16*m_maxConvexBodies; + m_maxContactCapacity = m_maxBroadphasePairs; + m_compoundPairCapacity = 1024*1024; + } +}; + + +#endif//B3_CONFIG_H + diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h new file mode 100644 index 0000000000..fb25165673 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h @@ -0,0 +1,46 @@ +/* +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_CONTACT4_H +#define B3_CONTACT4_H + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +B3_ATTRIBUTE_ALIGNED16(struct) b3Contact4 : public b3Contact4Data +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + + int getBodyA()const {return abs(m_bodyAPtrAndSignBit);} + int getBodyB()const {return abs(m_bodyBPtrAndSignBit);} + bool isBodyAFixed()const { return m_bodyAPtrAndSignBit<0;} + bool isBodyBFixed()const { return m_bodyBPtrAndSignBit<0;} + // todo. make it safer + int& getBatchIdx() { return m_batchIdx; } + const int& getBatchIdx() const { return m_batchIdx; } + float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp/(float)0xffff); } + void setRestituitionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_restituitionCoeffCmp = (unsigned short)(c*0xffff); } + float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp/(float)0xffff); } + void setFrictionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_frictionCoeffCmp = (unsigned short)(c*0xffff); } + + //float& getNPoints() { return m_worldNormal[3]; } + int getNPoints() const { return (int) m_worldNormalOnB.w; } + + float getPenetration(int idx) const { return m_worldPosB[idx].w; } + + bool isInvalid() const { return (getBodyA()==0 || getBodyB()==0); } +}; + +#endif //B3_CONTACT4_H diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp new file mode 100644 index 0000000000..55706fa631 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp @@ -0,0 +1,520 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +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. +*/ +//Originally written by Erwin Coumans + + +#include "b3ConvexUtility.h" +#include "Bullet3Geometry/b3ConvexHullComputer.h" +#include "Bullet3Geometry/b3GrahamScan2dConvexHull.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3HashMap.h" + + + + + +b3ConvexUtility::~b3ConvexUtility() +{ +} + +bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles) +{ + + + + b3ConvexHullComputer conv; + conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3),numPoints,0.f,0.f); + + b3AlignedObjectArray<b3Vector3> faceNormals; + int numFaces = conv.faces.size(); + faceNormals.resize(numFaces); + b3ConvexHullComputer* convexUtil = &conv; + + + b3AlignedObjectArray<b3MyFace> tmpFaces; + tmpFaces.resize(numFaces); + + int numVertices = convexUtil->vertices.size(); + m_vertices.resize(numVertices); + for (int p=0;p<numVertices;p++) + { + m_vertices[p] = convexUtil->vertices[p]; + } + + + for (int i=0;i<numFaces;i++) + { + int face = convexUtil->faces[i]; + //printf("face=%d\n",face); + const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; + const b3ConvexHullComputer::Edge* edge = firstEdge; + + b3Vector3 edges[3]; + int numEdges = 0; + //compute face normals + + do + { + + int src = edge->getSourceVertex(); + tmpFaces[i].m_indices.push_back(src); + int targ = edge->getTargetVertex(); + b3Vector3 wa = convexUtil->vertices[src]; + + b3Vector3 wb = convexUtil->vertices[targ]; + b3Vector3 newEdge = wb-wa; + newEdge.normalize(); + if (numEdges<2) + edges[numEdges++] = newEdge; + + edge = edge->getNextEdgeOfFace(); + } while (edge!=firstEdge); + + b3Scalar planeEq = 1e30f; + + + if (numEdges==2) + { + faceNormals[i] = edges[0].cross(edges[1]); + faceNormals[i].normalize(); + tmpFaces[i].m_plane[0] = faceNormals[i].getX(); + tmpFaces[i].m_plane[1] = faceNormals[i].getY(); + tmpFaces[i].m_plane[2] = faceNormals[i].getZ(); + tmpFaces[i].m_plane[3] = planeEq; + + } + else + { + b3Assert(0);//degenerate? + faceNormals[i].setZero(); + } + + for (int v=0;v<tmpFaces[i].m_indices.size();v++) + { + b3Scalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]); + if (planeEq>eq) + { + planeEq=eq; + } + } + tmpFaces[i].m_plane[3] = -planeEq; + } + + //merge coplanar faces and copy them to m_polyhedron + + b3Scalar faceWeldThreshold= 0.999f; + b3AlignedObjectArray<int> todoFaces; + for (int i=0;i<tmpFaces.size();i++) + todoFaces.push_back(i); + + while (todoFaces.size()) + { + b3AlignedObjectArray<int> coplanarFaceGroup; + int refFace = todoFaces[todoFaces.size()-1]; + + coplanarFaceGroup.push_back(refFace); + b3MyFace& faceA = tmpFaces[refFace]; + todoFaces.pop_back(); + + b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]); + for (int j=todoFaces.size()-1;j>=0;j--) + { + int i = todoFaces[j]; + b3MyFace& faceB = tmpFaces[i]; + b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]); + if (faceNormalA.dot(faceNormalB)>faceWeldThreshold) + { + coplanarFaceGroup.push_back(i); + todoFaces.remove(i); + } + } + + + bool did_merge = false; + if (coplanarFaceGroup.size()>1) + { + //do the merge: use Graham Scan 2d convex hull + + b3AlignedObjectArray<b3GrahamVector3> orgpoints; + b3Vector3 averageFaceNormal = b3MakeVector3(0,0,0); + + for (int i=0;i<coplanarFaceGroup.size();i++) + { +// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); + + b3MyFace& face = tmpFaces[coplanarFaceGroup[i]]; + b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0],face.m_plane[1],face.m_plane[2]); + averageFaceNormal+=faceNormal; + for (int f=0;f<face.m_indices.size();f++) + { + int orgIndex = face.m_indices[f]; + b3Vector3 pt = m_vertices[orgIndex]; + + bool found = false; + + for (int i=0;i<orgpoints.size();i++) + { + //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001)) + if (orgpoints[i].m_orgIndex == orgIndex) + { + found=true; + break; + } + } + if (!found) + orgpoints.push_back(b3GrahamVector3(pt,orgIndex)); + } + } + + + + b3MyFace combinedFace; + for (int i=0;i<4;i++) + combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i]; + + b3AlignedObjectArray<b3GrahamVector3> hull; + + averageFaceNormal.normalize(); + b3GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal); + + for (int i=0;i<hull.size();i++) + { + combinedFace.m_indices.push_back(hull[i].m_orgIndex); + for(int k = 0; k < orgpoints.size(); k++) + { + if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) + { + orgpoints[k].m_orgIndex = -1; // invalidate... + break; + } + } + } + + // are there rejected vertices? + bool reject_merge = false; + + + + for(int i = 0; i < orgpoints.size(); i++) { + if(orgpoints[i].m_orgIndex == -1) + continue; // this is in the hull... + // this vertex is rejected -- is anybody else using this vertex? + for(int j = 0; j < tmpFaces.size(); j++) { + + b3MyFace& face = tmpFaces[j]; + // is this a face of the current coplanar group? + bool is_in_current_group = false; + for(int k = 0; k < coplanarFaceGroup.size(); k++) { + if(coplanarFaceGroup[k] == j) { + is_in_current_group = true; + break; + } + } + if(is_in_current_group) // ignore this face... + continue; + // does this face use this rejected vertex? + for(int v = 0; v < face.m_indices.size(); v++) { + if(face.m_indices[v] == orgpoints[i].m_orgIndex) { + // this rejected vertex is used in another face -- reject merge + reject_merge = true; + break; + } + } + if(reject_merge) + break; + } + if(reject_merge) + break; + } + + if (!reject_merge) + { + // do this merge! + did_merge = true; + m_faces.push_back(combinedFace); + } + } + if(!did_merge) + { + for (int i=0;i<coplanarFaceGroup.size();i++) + { + b3MyFace face = tmpFaces[coplanarFaceGroup[i]]; + m_faces.push_back(face); + } + + } + + + + } + + initialize(); + + return true; +} + + + + + + +inline bool IsAlmostZero(const b3Vector3& v) +{ + if(fabsf(v.getX())>1e-6 || fabsf(v.getY())>1e-6 || fabsf(v.getZ())>1e-6) return false; + return true; +} + +struct b3InternalVertexPair +{ + b3InternalVertexPair(short int v0,short int v1) + :m_v0(v0), + m_v1(v1) + { + if (m_v1>m_v0) + b3Swap(m_v0,m_v1); + } + short int m_v0; + short int m_v1; + int getHash() const + { + return m_v0+(m_v1<<16); + } + bool equals(const b3InternalVertexPair& other) const + { + return m_v0==other.m_v0 && m_v1==other.m_v1; + } +}; + +struct b3InternalEdge +{ + b3InternalEdge() + :m_face0(-1), + m_face1(-1) + { + } + short int m_face0; + short int m_face1; +}; + +// + +#ifdef TEST_INTERNAL_OBJECTS +bool b3ConvexUtility::testContainment() const +{ + for(int p=0;p<8;p++) + { + b3Vector3 LocalPt; + if(p==0) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]); + else if(p==1) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]); + else if(p==2) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]); + else if(p==3) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]); + else if(p==4) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]); + else if(p==5) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]); + else if(p==6) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]); + else if(p==7) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]); + + for(int i=0;i<m_faces.size();i++) + { + const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]); + const b3Scalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3]; + if(d>0.0f) + return false; + } + } + return true; +} +#endif + +void b3ConvexUtility::initialize() +{ + + b3HashMap<b3InternalVertexPair,b3InternalEdge> edges; + + b3Scalar TotalArea = 0.0f; + + m_localCenter.setValue(0, 0, 0); + for(int i=0;i<m_faces.size();i++) + { + int numVertices = m_faces[i].m_indices.size(); + int NbTris = numVertices; + for(int j=0;j<NbTris;j++) + { + int k = (j+1)%numVertices; + b3InternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]); + b3InternalEdge* edptr = edges.find(vp); + b3Vector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0]; + edge.normalize(); + + bool found = false; + b3Vector3 diff,diff2; + + for (int p=0;p<m_uniqueEdges.size();p++) + { + diff = m_uniqueEdges[p]-edge; + diff2 = m_uniqueEdges[p]+edge; + + // if ((diff.length2()==0.f) || + // (diff2.length2()==0.f)) + + if (IsAlmostZero(diff) || + IsAlmostZero(diff2)) + { + found = true; + break; + } + } + + if (!found) + { + m_uniqueEdges.push_back(edge); + } + + if (edptr) + { + //TBD: figure out why I added this assert +// b3Assert(edptr->m_face0>=0); + // b3Assert(edptr->m_face1<0); + edptr->m_face1 = i; + } else + { + b3InternalEdge ed; + ed.m_face0 = i; + edges.insert(vp,ed); + } + } + } + +#ifdef USE_CONNECTED_FACES + for(int i=0;i<m_faces.size();i++) + { + int numVertices = m_faces[i].m_indices.size(); + m_faces[i].m_connectedFaces.resize(numVertices); + + for(int j=0;j<numVertices;j++) + { + int k = (j+1)%numVertices; + b3InternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]); + b3InternalEdge* edptr = edges.find(vp); + b3Assert(edptr); + b3Assert(edptr->m_face0>=0); + b3Assert(edptr->m_face1>=0); + + int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0; + m_faces[i].m_connectedFaces[j] = connectedFace; + } + } +#endif//USE_CONNECTED_FACES + + for(int i=0;i<m_faces.size();i++) + { + int numVertices = m_faces[i].m_indices.size(); + int NbTris = numVertices-2; + + const b3Vector3& p0 = m_vertices[m_faces[i].m_indices[0]]; + for(int j=1;j<=NbTris;j++) + { + int k = (j+1)%numVertices; + const b3Vector3& p1 = m_vertices[m_faces[i].m_indices[j]]; + const b3Vector3& p2 = m_vertices[m_faces[i].m_indices[k]]; + b3Scalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f; + b3Vector3 Center = (p0+p1+p2)/3.0f; + m_localCenter += Area * Center; + TotalArea += Area; + } + } + m_localCenter /= TotalArea; + + + + +#ifdef TEST_INTERNAL_OBJECTS + if(1) + { + m_radius = FLT_MAX; + for(int i=0;i<m_faces.size();i++) + { + const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]); + const b3Scalar dist = b3Fabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]); + if(dist<m_radius) + m_radius = dist; + } + + + b3Scalar MinX = FLT_MAX; + b3Scalar MinY = FLT_MAX; + b3Scalar MinZ = FLT_MAX; + b3Scalar MaxX = -FLT_MAX; + b3Scalar MaxY = -FLT_MAX; + b3Scalar MaxZ = -FLT_MAX; + for(int i=0; i<m_vertices.size(); i++) + { + const b3Vector3& pt = m_vertices[i]; + if(pt.getX()<MinX) MinX = pt.getX(); + if(pt.getX()>MaxX) MaxX = pt.getX(); + if(pt.getY()<MinY) MinY = pt.getY(); + if(pt.getY()>MaxY) MaxY = pt.getY(); + if(pt.getZ()<MinZ) MinZ = pt.getZ(); + if(pt.getZ()>MaxZ) MaxZ = pt.getZ(); + } + mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ); + mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ); + + + +// const b3Scalar r = m_radius / sqrtf(2.0f); + const b3Scalar r = m_radius / sqrtf(3.0f); + const int LargestExtent = mE.maxAxis(); + const b3Scalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f; + m_extents[0] = m_extents[1] = m_extents[2] = r; + m_extents[LargestExtent] = mE[LargestExtent]*0.5f; + bool FoundBox = false; + for(int j=0;j<1024;j++) + { + if(testContainment()) + { + FoundBox = true; + break; + } + + m_extents[LargestExtent] -= Step; + } + if(!FoundBox) + { + m_extents[0] = m_extents[1] = m_extents[2] = r; + } + else + { + // Refine the box + const b3Scalar Step = (m_radius - r)/1024.0f; + const int e0 = (1<<LargestExtent) & 3; + const int e1 = (1<<e0) & 3; + + for(int j=0;j<1024;j++) + { + const b3Scalar Saved0 = m_extents[e0]; + const b3Scalar Saved1 = m_extents[e1]; + m_extents[e0] += Step; + m_extents[e1] += Step; + + if(!testContainment()) + { + m_extents[e0] = Saved0; + m_extents[e1] = Saved1; + break; + } + } + } + } +#endif +} diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h new file mode 100644 index 0000000000..86c4151f8c --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h @@ -0,0 +1,62 @@ + +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +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. +*/ +//Originally written by Erwin Coumans + +#ifndef _BT_CONVEX_UTILITY_H +#define _BT_CONVEX_UTILITY_H + +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Transform.h" + + + + +struct b3MyFace +{ + b3AlignedObjectArray<int> m_indices; + b3Scalar m_plane[4]; +}; + +B3_ATTRIBUTE_ALIGNED16(class) b3ConvexUtility +{ + public: + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3Vector3 m_localCenter; + b3Vector3 m_extents; + b3Vector3 mC; + b3Vector3 mE; + b3Scalar m_radius; + + b3AlignedObjectArray<b3Vector3> m_vertices; + b3AlignedObjectArray<b3MyFace> m_faces; + b3AlignedObjectArray<b3Vector3> m_uniqueEdges; + + + b3ConvexUtility() + { + } + virtual ~b3ConvexUtility(); + + bool initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles=true); + + void initialize(); + bool testContainment() const; + + + +}; +#endif +
\ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp new file mode 100644 index 0000000000..c3134b2c65 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp @@ -0,0 +1,323 @@ +#include "b3CpuNarrowPhase.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h" + + +struct b3CpuNarrowPhaseInternalData +{ + b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU; + b3AlignedObjectArray<b3Collidable> m_collidablesCPU; + b3AlignedObjectArray<b3ConvexUtility*> m_convexData; + b3Config m_config; + + + b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra; + b3AlignedObjectArray<b3Vector3> m_uniqueEdges; + b3AlignedObjectArray<b3Vector3> m_convexVertices; + b3AlignedObjectArray<int> m_convexIndices; + b3AlignedObjectArray<b3GpuFace> m_convexFaces; + + b3AlignedObjectArray<b3Contact4Data> m_contacts; + + int m_numAcceleratedShapes; +}; + + +const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const +{ + return m_data->m_contacts; +} + +b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) +{ + return m_data->m_collidablesCPU[collidableIndex]; +} + +const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) const +{ + return m_data->m_collidablesCPU[collidableIndex]; +} + + +b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config) +{ + m_data = new b3CpuNarrowPhaseInternalData; + m_data->m_config = config; + m_data->m_numAcceleratedShapes = 0; +} + +b3CpuNarrowPhase::~b3CpuNarrowPhase() +{ + delete m_data; +} + +void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies) +{ + int nPairs = pairs.size(); + int numContacts = 0; + int maxContactCapacity = m_data->m_config.m_maxContactCapacity; + m_data->m_contacts.resize(maxContactCapacity); + + for (int i=0;i<nPairs;i++) + { + int bodyIndexA = pairs[i].x; + int bodyIndexB = pairs[i].y; + int collidableIndexA = bodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = bodies[bodyIndexB].m_collidableIdx; + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { +// computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE) + { +// computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); + //printf("convex-sphere\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE) + { +// computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); +// printf("convex-plane\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { +// computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); +// printf("plane-convex\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) + { +// computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0], +// nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU); +// printf("convex-plane\n"); + + } + + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE) + { +// computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); +// printf("convex-plane\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) + { +// computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); +// printf("plane-convex\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { + //printf("pairs[i].z=%d\n",pairs[i].z); + //int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies, + // m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts); + int contactIndex = b3ContactConvexConvexSAT(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies, + m_data->m_collidablesCPU,m_data->m_convexPolyhedra,m_data->m_convexVertices,m_data->m_uniqueEdges,m_data->m_convexIndices,m_data->m_convexFaces,m_data->m_contacts,numContacts,maxContactCapacity); + + + if (contactIndex>=0) + { + pairs[i].z = contactIndex; + } +// printf("plane-convex\n"); + + } + + + } + + m_data->m_contacts.resize(numContacts); +} + +int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr) +{ + int collidableIndex = allocateCollidable(); + if (collidableIndex<0) + return collidableIndex; + + + b3Collidable& col = m_data->m_collidablesCPU[collidableIndex]; + col.m_shapeType = SHAPE_CONVEX_HULL; + col.m_shapeIndex = -1; + + + { + b3Vector3 localCenter=b3MakeVector3(0,0,0); + for (int i=0;i<utilPtr->m_vertices.size();i++) + localCenter+=utilPtr->m_vertices[i]; + localCenter*= (1.f/utilPtr->m_vertices.size()); + utilPtr->m_localCenter = localCenter; + + col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col); + } + + if (col.m_shapeIndex>=0) + { + b3Aabb aabb; + + b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f); + b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f); + + for (int i=0;i<utilPtr->m_vertices.size();i++) + { + myAabbMin.setMin(utilPtr->m_vertices[i]); + myAabbMax.setMax(utilPtr->m_vertices[i]); + } + aabb.m_min[0] = myAabbMin[0]; + aabb.m_min[1] = myAabbMin[1]; + aabb.m_min[2] = myAabbMin[2]; + aabb.m_minIndices[3] = 0; + + aabb.m_max[0] = myAabbMax[0]; + aabb.m_max[1] = myAabbMax[1]; + aabb.m_max[2] = myAabbMax[2]; + aabb.m_signedMaxIndices[3] = 0; + + m_data->m_localShapeAABBCPU.push_back(aabb); + + } + + return collidableIndex; +} + +int b3CpuNarrowPhase::allocateCollidable() +{ + int curSize = m_data->m_collidablesCPU.size(); + if (curSize<m_data->m_config.m_maxConvexShapes) + { + m_data->m_collidablesCPU.expand(); + return curSize; + } + else + { + b3Error("allocateCollidable out-of-range %d\n",m_data->m_config.m_maxConvexShapes); + } + return -1; + +} + +int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling) +{ + b3AlignedObjectArray<b3Vector3> verts; + + unsigned char* vts = (unsigned char*) vertices; + for (int i=0;i<numVertices;i++) + { + float* vertex = (float*) &vts[i*strideInBytes]; + verts.push_back(b3MakeVector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2])); + } + + b3ConvexUtility* utilPtr = new b3ConvexUtility(); + bool merge = true; + if (numVertices) + { + utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge); + } + + int collidableIndex = registerConvexHullShape(utilPtr); + + delete utilPtr; + return collidableIndex; +} + + +int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col) +{ + + m_data->m_convexData.resize(m_data->m_numAcceleratedShapes+1); + m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1); + + + b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1); + convex.mC = convexPtr->mC; + convex.mE = convexPtr->mE; + convex.m_extents= convexPtr->m_extents; + convex.m_localCenter = convexPtr->m_localCenter; + convex.m_radius = convexPtr->m_radius; + + convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size(); + int edgeOffset = m_data->m_uniqueEdges.size(); + convex.m_uniqueEdgesOffset = edgeOffset; + + m_data->m_uniqueEdges.resize(edgeOffset+convex.m_numUniqueEdges); + + //convex data here + int i; + for ( i=0;i<convexPtr->m_uniqueEdges.size();i++) + { + m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[i]; + } + + int faceOffset = m_data->m_convexFaces.size(); + convex.m_faceOffset = faceOffset; + convex.m_numFaces = convexPtr->m_faces.size(); + + m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces); + + + for (i=0;i<convexPtr->m_faces.size();i++) + { + m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0], + convexPtr->m_faces[i].m_plane[1], + convexPtr->m_faces[i].m_plane[2], + convexPtr->m_faces[i].m_plane[3]); + + + int indexOffset = m_data->m_convexIndices.size(); + int numIndices = convexPtr->m_faces[i].m_indices.size(); + m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices; + m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset; + m_data->m_convexIndices.resize(indexOffset+numIndices); + for (int p=0;p<numIndices;p++) + { + m_data->m_convexIndices[indexOffset+p] = convexPtr->m_faces[i].m_indices[p]; + } + } + + convex.m_numVertices = convexPtr->m_vertices.size(); + int vertexOffset = m_data->m_convexVertices.size(); + convex.m_vertexOffset =vertexOffset; + + m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices); + for (int i=0;i<convexPtr->m_vertices.size();i++) + { + m_data->m_convexVertices[vertexOffset+i] = convexPtr->m_vertices[i]; + } + + (m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr; + + + + return m_data->m_numAcceleratedShapes++; +} + +const b3Aabb& b3CpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const +{ + return m_data->m_localShapeAABBCPU[collidableIndex]; +} diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h new file mode 100644 index 0000000000..528be3346d --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h @@ -0,0 +1,105 @@ +#ifndef B3_CPU_NARROWPHASE_H +#define B3_CPU_NARROWPHASE_H + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +class b3CpuNarrowPhase +{ +protected: + + struct b3CpuNarrowPhaseInternalData* m_data; + int m_acceleratedCompanionShapeIndex; + int m_planeBodyIndex; + int m_static0Index; + + int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr,b3Collidable& col); + int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling); + +public: + + + + + b3CpuNarrowPhase(const struct b3Config& config); + + virtual ~b3CpuNarrowPhase(void); + + int registerSphereShape(float radius); + int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant); + + int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes); + int registerFace(const b3Vector3& faceNormal, float faceConstant); + + int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling); + + //do they need to be merged? + + int registerConvexHullShape(b3ConvexUtility* utilPtr); + int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling); + + //int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu); + void setObjectTransform(const float* position, const float* orientation , int bodyIndex); + + void writeAllBodiesToGpu(); + void reset(); + void readbackAllBodiesToCpu(); + bool getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const; + + void setObjectTransformCpu(float* position, float* orientation , int bodyIndex); + void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex); + + + //virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects); + virtual void computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies); + + + + const struct b3RigidBodyData* getBodiesCpu() const; + //struct b3RigidBodyData* getBodiesCpu(); + + int getNumBodiesGpu() const; + + + int getNumBodyInertiasGpu() const; + + + const struct b3Collidable* getCollidablesCpu() const; + int getNumCollidablesGpu() const; + + + /*const struct b3Contact4* getContactsCPU() const; + + + int getNumContactsGpu() const; + */ + + const b3AlignedObjectArray<b3Contact4Data>& getContacts() const; + + + int getNumRigidBodies() const; + + int allocateCollidable(); + + int getStatic0Index() const + { + return m_static0Index; + } + b3Collidable& getCollidableCpu(int collidableIndex); + const b3Collidable& getCollidableCpu(int collidableIndex) const; + + const b3CpuNarrowPhaseInternalData* getInternalData() const + { + return m_data; + } + + const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const; +}; + +#endif //B3_CPU_NARROWPHASE_H + diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h new file mode 100644 index 0000000000..fba8bd07a4 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h @@ -0,0 +1,24 @@ + +#ifndef B3_RAYCAST_INFO_H +#define B3_RAYCAST_INFO_H + +#include "Bullet3Common/b3Vector3.h" + +B3_ATTRIBUTE_ALIGNED16(struct) b3RayInfo +{ + b3Vector3 m_from; + b3Vector3 m_to; +}; + +B3_ATTRIBUTE_ALIGNED16(struct) b3RayHit +{ + b3Scalar m_hitFraction; + int m_hitBody; + int m_hitResult1; + int m_hitResult2; + b3Vector3 m_hitPoint; + b3Vector3 m_hitNormal; +}; + +#endif //B3_RAYCAST_INFO_H + diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h new file mode 100644 index 0000000000..d58f71802f --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h @@ -0,0 +1,30 @@ +/* +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_RIGID_BODY_CL +#define B3_RIGID_BODY_CL + +#include "Bullet3Common/b3Scalar.h" +#include "Bullet3Common/b3Matrix3x3.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + + +inline float b3GetInvMass(const b3RigidBodyData& body) +{ + return body.m_invMass; +} + + +#endif//B3_RIGID_BODY_CL diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h new file mode 100644 index 0000000000..8788ccbb47 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h @@ -0,0 +1,20 @@ + +#ifndef B3_BVH_SUBTREE_INFO_DATA_H +#define B3_BVH_SUBTREE_INFO_DATA_H + +typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t; + +struct b3BvhSubtreeInfoData +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes, points to the root of the subtree + int m_rootNodeIndex; + //4 bytes + int m_subtreeSize; + int m_padding[3]; +}; + +#endif //B3_BVH_SUBTREE_INFO_DATA_H + diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h new file mode 100644 index 0000000000..2618da24bc --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h @@ -0,0 +1,126 @@ + + +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" + + + +// work-in-progress +void b3BvhTraversal( __global const b3Int4* pairs, + __global const b3RigidBodyData* rigidBodies, + __global const b3Collidable* collidables, + __global b3Aabb* aabbs, + __global b3Int4* concavePairsOut, + __global volatile int* numConcavePairsOut, + __global const b3BvhSubtreeInfo* subtreeHeadersRoot, + __global const b3QuantizedBvhNode* quantizedNodesRoot, + __global const b3BvhInfo* bvhInfos, + int numPairs, + int maxNumConcavePairsCapacity, + int id) +{ + + int bodyIndexA = pairs[id].x; + int bodyIndexB = pairs[id].y; + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + //once the broadphase avoids static-static pairs, we can remove this test + if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0)) + { + return; + } + + if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH) + return; + + int shapeTypeB = collidables[collidableIndexB].m_shapeType; + + if (shapeTypeB!=SHAPE_CONVEX_HULL && + shapeTypeB!=SHAPE_SPHERE && + shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS + ) + return; + + b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes]; + + b3Float4 bvhAabbMin = bvhInfo.m_aabbMin; + b3Float4 bvhAabbMax = bvhInfo.m_aabbMax; + b3Float4 bvhQuantization = bvhInfo.m_quantization; + int numSubtreeHeaders = bvhInfo.m_numSubTrees; + __global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset]; + __global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset]; + + + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization); + b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization); + + for (int i=0;i<numSubtreeHeaders;i++) + { + b3BvhSubtreeInfoData subtree = subtreeHeaders[i]; + + int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap != 0) + { + int startNodeIndex = subtree.m_rootNodeIndex; + int endNodeIndex = subtree.m_rootNodeIndex+subtree.m_subtreeSize; + int curIndex = startNodeIndex; + int escapeIndex; + int isLeafNode; + int aabbOverlap; + while (curIndex < endNodeIndex) + { + b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex]; + aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode.m_quantizedAabbMin,rootNode.m_quantizedAabbMax); + isLeafNode = b3IsLeaf(&rootNode); + if (aabbOverlap) + { + if (isLeafNode) + { + int triangleIndex = b3GetTriangleIndex(&rootNode); + if (shapeTypeB==SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + int numChildrenB = collidables[collidableIndexB].m_numChildShapes; + int pairIdx = b3AtomicAdd (numConcavePairsOut,numChildrenB); + for (int b=0;b<numChildrenB;b++) + { + if ((pairIdx+b)<maxNumConcavePairsCapacity) + { + int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+b; + b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,childShapeIndexB); + concavePairsOut[pairIdx+b] = newPair; + } + } + } else + { + int pairIdx = b3AtomicInc(numConcavePairsOut); + if (pairIdx<maxNumConcavePairsCapacity) + { + b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,0); + concavePairsOut[pairIdx] = newPair; + } + } + } + curIndex++; + } else + { + if (isLeafNode) + { + curIndex++; + } else + { + escapeIndex = b3GetEscapeIndex(&rootNode); + curIndex += escapeIndex; + } + } + } + } + } + +}
\ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h new file mode 100644 index 0000000000..8009e7d6e0 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h @@ -0,0 +1,188 @@ +#ifndef B3_CLIP_FACES_H +#define B3_CLIP_FACES_H + + +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" + + +inline b3Float4 b3Lerp3(b3Float4ConstArg a,b3Float4ConstArg b, float t) +{ + return b3MakeFloat4( a.x + (b.x - a.x) * t, + a.y + (b.y - a.y) * t, + a.z + (b.z - a.z) * t, + 0.f); +} + +// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut +int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS,float planeEqWS, __global b3Float4* ppVtxOut) +{ + + int ve; + float ds, de; + int numVertsOut = 0; + //double-check next test + // if (numVertsIn < 2) + // return 0; + + b3Float4 firstVertex=pVtxIn[numVertsIn-1]; + b3Float4 endVertex = pVtxIn[0]; + + ds = b3Dot(planeNormalWS,firstVertex)+planeEqWS; + + for (ve = 0; ve < numVertsIn; ve++) + { + endVertex=pVtxIn[ve]; + de = b3Dot(planeNormalWS,endVertex)+planeEqWS; + if (ds<0) + { + if (de<0) + { + // Start < 0, end < 0, so output endVertex + ppVtxOut[numVertsOut++] = endVertex; + } + else + { + // Start < 0, end >= 0, so output intersection + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + } + } + else + { + if (de<0) + { + // Start >= 0, end < 0 so output intersection and end + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + ppVtxOut[numVertsOut++] = endVertex; + } + } + firstVertex = endVertex; + ds = de; + } + return numVertsOut; +} + + +__kernel void clipFacesAndFindContactsKernel( __global const b3Float4* separatingNormals, + __global const int* hasSeparatingAxis, + __global b3Int4* clippingFacesOut, + __global b3Float4* worldVertsA1, + __global b3Float4* worldNormalsA1, + __global b3Float4* worldVertsB1, + __global b3Float4* worldVertsB2, + int vertexFaceCapacity, + int pairIndex + ) +{ +// int i = get_global_id(0); + //int pairIndex = i; + int i = pairIndex; + + float minDist = -1e30f; + float maxDist = 0.02f; + +// if (i<numPairs) + { + + if (hasSeparatingAxis[i]) + { + +// int bodyIndexA = pairs[i].x; + // int bodyIndexB = pairs[i].y; + + int numLocalContactsOut = 0; + + int capacityWorldVertsB2 = vertexFaceCapacity; + + __global b3Float4* pVtxIn = &worldVertsB1[pairIndex*capacityWorldVertsB2]; + __global b3Float4* pVtxOut = &worldVertsB2[pairIndex*capacityWorldVertsB2]; + + + { + __global b3Int4* clippingFaces = clippingFacesOut; + + + int closestFaceA = clippingFaces[pairIndex].x; + // int closestFaceB = clippingFaces[pairIndex].y; + int numVertsInA = clippingFaces[pairIndex].z; + int numVertsInB = clippingFaces[pairIndex].w; + + int numVertsOut = 0; + + if (closestFaceA>=0) + { + + + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + + for(int e0=0;e0<numVertsInA;e0++) + { + const b3Float4 aw = worldVertsA1[pairIndex*capacityWorldVertsB2+e0]; + const b3Float4 bw = worldVertsA1[pairIndex*capacityWorldVertsB2+((e0+1)%numVertsInA)]; + const b3Float4 WorldEdge0 = aw - bw; + b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex]; + b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0,worldPlaneAnormal1); + b3Float4 worldA1 = aw; + float planeEqWS1 = -b3Dot(worldA1,planeNormalWS1); + b3Float4 planeNormalWS = planeNormalWS1; + float planeEqWS=planeEqWS1; + numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS,planeEqWS, pVtxOut); + __global b3Float4* tmp = pVtxOut; + pVtxOut = pVtxIn; + pVtxIn = tmp; + numVertsInB = numVertsOut; + numVertsOut = 0; + } + + b3Float4 planeNormalWS = worldNormalsA1[pairIndex]; + float planeEqWS=-b3Dot(planeNormalWS,worldVertsA1[pairIndex*capacityWorldVertsB2]); + + for (int i=0;i<numVertsInB;i++) + { + float depth = b3Dot(planeNormalWS,pVtxIn[i])+planeEqWS; + if (depth <=minDist) + { + depth = minDist; + } +/* + static float maxDepth = 0.f; + if (depth < maxDepth) + { + maxDepth = depth; + if (maxDepth < -10) + { + printf("error at framecount %d?\n",myframecount); + } + printf("maxDepth = %f\n", maxDepth); + + } +*/ + if (depth <=maxDist) + { + b3Float4 pointInWorld = pVtxIn[i]; + pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth); + } + } + + } + clippingFaces[pairIndex].w =numLocalContactsOut; + + + } + + for (int i=0;i<numLocalContactsOut;i++) + pVtxIn[i] = pVtxOut[i]; + + }// if (hasSeparatingAxis[i]) + }// if (i<numPairs) + +} + +#endif //B3_CLIP_FACES_H + diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h new file mode 100644 index 0000000000..77cdc7b7a9 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h @@ -0,0 +1,76 @@ + +#ifndef B3_COLLIDABLE_H +#define B3_COLLIDABLE_H + + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Common/shared/b3Quat.h" + +enum b3ShapeTypes +{ + SHAPE_HEIGHT_FIELD=1, + + SHAPE_CONVEX_HULL=3, + SHAPE_PLANE=4, + SHAPE_CONCAVE_TRIMESH=5, + SHAPE_COMPOUND_OF_CONVEX_HULLS=6, + SHAPE_SPHERE=7, + MAX_NUM_SHAPE_TYPES, +}; + +typedef struct b3Collidable b3Collidable_t; + + +struct b3Collidable +{ + union { + int m_numChildShapes; + int m_bvhIndex; + }; + union + { + float m_radius; + int m_compoundBvhIndex; + }; + + int m_shapeType; + union + { + int m_shapeIndex; + float m_height; + }; +}; + +typedef struct b3GpuChildShape b3GpuChildShape_t; +struct b3GpuChildShape +{ + b3Float4 m_childPosition; + b3Quat m_childOrientation; + union + { + int m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS + int m_capsuleAxis; + }; + union + { + float m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES + int m_numChildShapes;//used for compound shape + }; + union + { + float m_height;//used for childshape of SHAPE_COMPOUND_OF_CAPSULES + int m_collidableShapeIndex; + }; + int m_shapeType; +}; + +struct b3CompoundOverlappingPair +{ + int m_bodyIndexA; + int m_bodyIndexB; +// int m_pairType; + int m_childShapeIndexA; + int m_childShapeIndexB; +}; + +#endif //B3_COLLIDABLE_H diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h new file mode 100644 index 0000000000..dfd45cc566 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h @@ -0,0 +1,40 @@ +#ifndef B3_CONTACT4DATA_H +#define B3_CONTACT4DATA_H + +#include "Bullet3Common/shared/b3Float4.h" + +typedef struct b3Contact4Data b3Contact4Data_t; + +struct b3Contact4Data +{ + b3Float4 m_worldPosB[4]; +// b3Float4 m_localPosA[4]; +// b3Float4 m_localPosB[4]; + b3Float4 m_worldNormalOnB; // w: m_nPoints + unsigned short m_restituitionCoeffCmp; + unsigned short m_frictionCoeffCmp; + int m_batchIdx; + int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr + int m_bodyBPtrAndSignBit; + + int m_childIndexA; + int m_childIndexB; + int m_unused1; + int m_unused2; + + +}; + +inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact) +{ + return (int)contact->m_worldNormalOnB.w; +}; + +inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints) +{ + contact->m_worldNormalOnB.w = (float)numPoints; +}; + + + +#endif //B3_CONTACT4DATA_H
\ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h new file mode 100644 index 0000000000..f295f01a6c --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h @@ -0,0 +1,520 @@ + +#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H +#define B3_CONTACT_CONVEX_CONVEX_SAT_H + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h" + +#define B3_MAX_VERTS 1024 + + + +inline b3Float4 b3Lerp3(const b3Float4& a,const b3Float4& b, float t) +{ + return b3MakeVector3( a.x + (b.x - a.x) * t, + a.y + (b.y - a.y) * t, + a.z + (b.z - a.z) * t, + 0.f); +} + + +// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut +inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS,float planeEqWS, b3Float4* ppVtxOut) +{ + + int ve; + float ds, de; + int numVertsOut = 0; + if (numVertsIn < 2) + return 0; + + b3Float4 firstVertex=pVtxIn[numVertsIn-1]; + b3Float4 endVertex = pVtxIn[0]; + + ds = b3Dot3F4(planeNormalWS,firstVertex)+planeEqWS; + + for (ve = 0; ve < numVertsIn; ve++) + { + endVertex=pVtxIn[ve]; + + de = b3Dot3F4(planeNormalWS,endVertex)+planeEqWS; + + if (ds<0) + { + if (de<0) + { + // Start < 0, end < 0, so output endVertex + ppVtxOut[numVertsOut++] = endVertex; + } + else + { + // Start < 0, end >= 0, so output intersection + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + } + } + else + { + if (de<0) + { + // Start >= 0, end < 0 so output intersection and end + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + ppVtxOut[numVertsOut++] = endVertex; + } + } + firstVertex = endVertex; + ds = de; + } + return numVertsOut; +} + + +inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA, + const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1, + b3Float4* worldVertsB2, int capacityWorldVertsB2, + const float minDist, float maxDist, + const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA, + //const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB, + b3Float4* contactsOut, + int contactCapacity) +{ + int numContactsOut = 0; + + b3Float4* pVtxIn = worldVertsB1; + b3Float4* pVtxOut = worldVertsB2; + + int numVertsIn = numWorldVertsB1; + int numVertsOut = 0; + + int closestFaceA=-1; + { + float dmin = FLT_MAX; + for(int face=0;face<hullA->m_numFaces;face++) + { + const b3Float4 Normal = b3MakeVector3( + facesA[hullA->m_faceOffset+face].m_plane.x, + facesA[hullA->m_faceOffset+face].m_plane.y, + facesA[hullA->m_faceOffset+face].m_plane.z,0.f); + const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal); + + float d = b3Dot3F4(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + } + } + } + if (closestFaceA<0) + return numContactsOut; + + b3GpuFace polyA = facesA[hullA->m_faceOffset+closestFaceA]; + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + //int numContacts = numWorldVertsB1; + int numVerticesA = polyA.m_numIndices; + for(int e0=0;e0<numVerticesA;e0++) + { + const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+e0]]; + const b3Float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]]; + const b3Float4 edge0 = a - b; + const b3Float4 WorldEdge0 = b3QuatRotate(ornA,edge0); + b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA,planeNormalA); + + b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0,worldPlaneAnormal1); + b3Float4 worldA1 = b3TransformPoint(a,posA,ornA); + float planeEqWS1 = -b3Dot3F4(worldA1,planeNormalWS1); + + b3Float4 planeNormalWS = planeNormalWS1; + float planeEqWS=planeEqWS1; + + //clip face + //clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); + numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut); + + //btSwap(pVtxIn,pVtxOut); + b3Float4* tmp = pVtxOut; + pVtxOut = pVtxIn; + pVtxIn = tmp; + numVertsIn = numVertsOut; + numVertsOut = 0; + } + + + // only keep points that are behind the witness face + { + b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + float localPlaneEq = polyA.m_plane.w; + b3Float4 planeNormalWS = b3QuatRotate(ornA,localPlaneNormal); + float planeEqWS=localPlaneEq-b3Dot3F4(planeNormalWS,posA); + for (int i=0;i<numVertsIn;i++) + { + float depth = b3Dot3F4(planeNormalWS,pVtxIn[i])+planeEqWS; + if (depth <=minDist) + { + depth = minDist; + } + if (numContactsOut<contactCapacity) + { + if (depth <=maxDist) + { + b3Float4 pointInWorld = pVtxIn[i]; + //resultOut.addContactPoint(separatingNormal,point,depth); + contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth); + //printf("depth=%f\n",depth); + } + } else + { + b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut,contactCapacity); + } + } + } + + return numContactsOut; +} + + + +inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal, + const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const b3Float4& posA, const b3Quaternion& ornA,const b3Float4& posB, const b3Quaternion& ornB, + b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts, + const float minDist, float maxDist, + const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA, + const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB, + + b3Float4* contactsOut, + int contactCapacity) +{ + int numContactsOut = 0; + int numWorldVertsB1= 0; + + B3_PROFILE("clipHullAgainstHull"); + + //float curMaxDist=maxDist; + int closestFaceB=-1; + float dmax = -FLT_MAX; + + { + //B3_PROFILE("closestFaceB"); + if (hullB.m_numFaces!=1) + { + //printf("wtf\n"); + } + static bool once = true; + //printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z); + + for(int face=0;face<hullB.m_numFaces;face++) + { +#ifdef BT_DEBUG_SAT_FACE + if (once) + printf("face %d\n",face); + const b3GpuFace* faceB = &facesB[hullB.m_faceOffset+face]; + if (once) + { + for (int i=0;i<faceB->m_numIndices;i++) + { + b3Float4 vert = verticesB[hullB.m_vertexOffset+indicesB[faceB->m_indexOffset+i]]; + printf("vert[%d] = %f,%f,%f\n",i,vert.x,vert.y,vert.z); + } + } +#endif //BT_DEBUG_SAT_FACE + //if (facesB[hullB.m_faceOffset+face].m_numIndices>2) + { + const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset+face].m_plane.x, + facesB[hullB.m_faceOffset+face].m_plane.y, facesB[hullB.m_faceOffset+face].m_plane.z,0.f); + const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal); +#ifdef BT_DEBUG_SAT_FACE + if (once) + printf("faceNormal = %f,%f,%f\n",Normal.x,Normal.y,Normal.z); +#endif + float d = b3Dot3F4(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + once = false; + } + + + b3Assert(closestFaceB>=0); + { + //B3_PROFILE("worldVertsB1"); + const b3GpuFace& polyB = facesB[hullB.m_faceOffset+closestFaceB]; + const int numVertices = polyB.m_numIndices; + for(int e0=0;e0<numVertices;e0++) + { + const b3Float4& b = verticesB[hullB.m_vertexOffset+indicesB[polyB.m_indexOffset+e0]]; + worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b,posB,ornB); + } + } + + if (closestFaceB>=0) + { + //B3_PROFILE("clipFaceAgainstHull"); + numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA, + posA,ornA, + worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist, + verticesA, facesA, indicesA, + contactsOut,contactCapacity); + } + + return numContactsOut; +} + + + + +inline int b3ClipHullHullSingle( + int bodyIndexA, int bodyIndexB, + const b3Float4& posA, + const b3Quaternion& ornA, + const b3Float4& posB, + const b3Quaternion& ornB, + + int collidableIndexA, int collidableIndexB, + + const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf, + b3AlignedObjectArray<b3Contact4Data>* globalContactOut, + int& nContacts, + + const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA, + const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB, + + const b3AlignedObjectArray<b3Vector3>& verticesA, + const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA, + const b3AlignedObjectArray<b3GpuFace>& facesA, + const b3AlignedObjectArray<int>& indicesA, + + const b3AlignedObjectArray<b3Vector3>& verticesB, + const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB, + const b3AlignedObjectArray<b3GpuFace>& facesB, + const b3AlignedObjectArray<int>& indicesB, + + const b3AlignedObjectArray<b3Collidable>& hostCollidablesA, + const b3AlignedObjectArray<b3Collidable>& hostCollidablesB, + const b3Vector3& sepNormalWorldSpace, + int maxContactCapacity ) +{ + int contactIndex = -1; + b3ConvexPolyhedronData hullA, hullB; + + b3Collidable colA = hostCollidablesA[collidableIndexA]; + hullA = hostConvexDataA[colA.m_shapeIndex]; + //printf("numvertsA = %d\n",hullA.m_numVertices); + + + b3Collidable colB = hostCollidablesB[collidableIndexB]; + hullB = hostConvexDataB[colB.m_shapeIndex]; + //printf("numvertsB = %d\n",hullB.m_numVertices); + + + b3Float4 contactsOut[B3_MAX_VERTS]; + int localContactCapacity = B3_MAX_VERTS; + +#ifdef _WIN32 + b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x)); + b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x)); +#endif + + + { + + b3Float4 worldVertsB1[B3_MAX_VERTS]; + b3Float4 worldVertsB2[B3_MAX_VERTS]; + int capacityWorldVerts = B3_MAX_VERTS; + + b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z,0.f); + int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex; + int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex; + + b3Scalar minDist = -1; + b3Scalar maxDist = 0.; + + + + b3Transform trA,trB; + { + //B3_PROFILE("b3TransformPoint computation"); + //trA.setIdentity(); + trA.setOrigin(b3MakeVector3(posA.x,posA.y,posA.z)); + trA.setRotation(b3Quaternion(ornA.x,ornA.y,ornA.z,ornA.w)); + + //trB.setIdentity(); + trB.setOrigin(b3MakeVector3(posB.x,posB.y,posB.z)); + trB.setRotation(b3Quaternion(ornB.x,ornB.y,ornB.z,ornB.w)); + } + + b3Quaternion trAorn = trA.getRotation(); + b3Quaternion trBorn = trB.getRotation(); + + int numContactsOut = b3ClipHullAgainstHull(hostNormal, + hostConvexDataA.at(shapeA), + hostConvexDataB.at(shapeB), + (b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn, + (b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn, + worldVertsB1,worldVertsB2,capacityWorldVerts, + minDist, maxDist, + verticesA, facesA,indicesA, + verticesB, facesB,indicesB, + + contactsOut,localContactCapacity); + + if (numContactsOut>0) + { + B3_PROFILE("overlap"); + + b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal; +// b3Float4 centerOut; + + b3Int4 contactIdx; + contactIdx.x = 0; + contactIdx.y = 1; + contactIdx.z = 2; + contactIdx.w = 3; + + int numPoints = 0; + + { + B3_PROFILE("extractManifold"); + numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx); + } + + b3Assert(numPoints); + + if (nContacts<maxContactCapacity) + { + contactIndex = nContacts; + globalContactOut->expand(); + b3Contact4Data& contact = globalContactOut->at(nContacts); + contact.m_batchIdx = 0;//i; + contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA; + contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB; + + contact.m_frictionCoeffCmp = 45874; + contact.m_restituitionCoeffCmp = 0; + + // float distance = 0.f; + for (int p=0;p<numPoints;p++) + { + contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];//check if it is actually on B + contact.m_worldNormalOnB = normalOnSurfaceB; + } + //printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints); + contact.m_worldNormalOnB.w = (b3Scalar)numPoints; + nContacts++; + } else + { + b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts,maxContactCapacity); + } + } + } + return contactIndex; +} + + + + + +inline int b3ContactConvexConvexSAT( + int pairIndex, + int bodyIndexA, int bodyIndexB, + int collidableIndexA, int collidableIndexB, + const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies, + const b3AlignedObjectArray<b3Collidable>& collidables, + const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes, + const b3AlignedObjectArray<b3Float4>& convexVertices, + const b3AlignedObjectArray<b3Float4>& uniqueEdges, + const b3AlignedObjectArray<int>& convexIndices, + const b3AlignedObjectArray<b3GpuFace>& faces, + b3AlignedObjectArray<b3Contact4Data>& globalContactsOut, + int& nGlobalContactsOut, + int maxContactCapacity) +{ + int contactIndex = -1; + + + b3Float4 posA = rigidBodies[bodyIndexA].m_pos; + b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat; + b3Float4 posB = rigidBodies[bodyIndexB].m_pos; + b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat; + + + b3ConvexPolyhedronData hullA, hullB; + + b3Float4 sepNormalWorldSpace; + + + + b3Collidable colA = collidables[collidableIndexA]; + hullA = convexShapes[colA.m_shapeIndex]; + //printf("numvertsA = %d\n",hullA.m_numVertices); + + + b3Collidable colB = collidables[collidableIndexB]; + hullB = convexShapes[colB.m_shapeIndex]; + //printf("numvertsB = %d\n",hullB.m_numVertices); + + + + +#ifdef _WIN32 + b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x)); + b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x)); +#endif + + bool foundSepAxis = b3FindSeparatingAxis(hullA,hullB, + posA, + ornA, + posB, + ornB, + + convexVertices,uniqueEdges,faces,convexIndices, + convexVertices,uniqueEdges,faces,convexIndices, + + sepNormalWorldSpace + ); + + + if (foundSepAxis) + { + + + contactIndex = b3ClipHullHullSingle( + bodyIndexA, bodyIndexB, + posA,ornA, + posB,ornB, + collidableIndexA, collidableIndexB, + &rigidBodies, + &globalContactsOut, + nGlobalContactsOut, + + convexShapes, + convexShapes, + + convexVertices, + uniqueEdges, + faces, + convexIndices, + + convexVertices, + uniqueEdges, + faces, + convexIndices, + + collidables, + collidables, + sepNormalWorldSpace, + maxContactCapacity); + + } + + return contactIndex; +} + +#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h new file mode 100644 index 0000000000..a3fa82287b --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h @@ -0,0 +1,162 @@ + +#ifndef B3_CONTACT_SPHERE_SPHERE_H +#define B3_CONTACT_SPHERE_SPHERE_H + + + + + +void computeContactSphereConvex(int pairIndex, + int bodyIndexA, int bodyIndexB, + int collidableIndexA, int collidableIndexB, + const b3RigidBodyData* rigidBodies, + const b3Collidable* collidables, + const b3ConvexPolyhedronData* convexShapes, + const b3Vector3* convexVertices, + const int* convexIndices, + const b3GpuFace* faces, + b3Contact4* globalContactsOut, + int& nGlobalContactsOut, + int maxContactCapacity) +{ + + float radius = collidables[collidableIndexA].m_radius; + float4 spherePos1 = rigidBodies[bodyIndexA].m_pos; + b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat; + + + + float4 pos = rigidBodies[bodyIndexB].m_pos; + + + b3Quaternion quat = rigidBodies[bodyIndexB].m_quat; + + b3Transform tr; + tr.setIdentity(); + tr.setOrigin(pos); + tr.setRotation(quat); + b3Transform trInv = tr.inverse(); + + float4 spherePos = trInv(spherePos1); + + int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx; + int shapeIndex = collidables[collidableIndex].m_shapeIndex; + int numFaces = convexShapes[shapeIndex].m_numFaces; + float4 closestPnt = b3MakeVector3(0, 0, 0, 0); + float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0); + float minDist = -1000000.f; // TODO: What is the largest/smallest float? + bool bCollide = true; + int region = -1; + float4 localHitNormal; + for ( int f = 0; f < numFaces; f++ ) + { + b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f]; + float4 planeEqn; + float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); + float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal); + planeEqn = n1; + planeEqn[3] = face.m_plane.w; + + float4 pntReturn; + float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn); + + if ( dist > radius) + { + bCollide = false; + break; + } + + if ( dist > 0 ) + { + //might hit an edge or vertex + b3Vector3 out; + + bool isInPoly = IsPointInPolygon(spherePos, + &face, + &convexVertices[convexShapes[shapeIndex].m_vertexOffset], + convexIndices, + &out); + if (isInPoly) + { + if (dist>minDist) + { + minDist = dist; + closestPnt = pntReturn; + localHitNormal = planeEqn; + region=1; + } + } else + { + b3Vector3 tmp = spherePos-out; + b3Scalar l2 = tmp.length2(); + if (l2<radius*radius) + { + dist = b3Sqrt(l2); + if (dist>minDist) + { + minDist = dist; + closestPnt = out; + localHitNormal = tmp/dist; + region=2; + } + + } else + { + bCollide = false; + break; + } + } + } + else + { + if ( dist > minDist ) + { + minDist = dist; + closestPnt = pntReturn; + localHitNormal = planeEqn; + region=3; + } + } + } + static int numChecks = 0; + numChecks++; + + if (bCollide && minDist > -10000) + { + + float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld; + float4 pOnB1 = tr(closestPnt); + //printf("dist ,%f,",minDist); + float actualDepth = minDist-radius; + if (actualDepth<0) + { + //printf("actualDepth = ,%f,", actualDepth); + //printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z); + //printf("region=,%d,\n", region); + pOnB1[3] = actualDepth; + + int dstIdx; +// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx ); + + if (nGlobalContactsOut < maxContactCapacity) + { + dstIdx=nGlobalContactsOut; + nGlobalContactsOut++; + + b3Contact4* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = normalOnSurfaceB1; + c->setFrictionCoeff(0.7); + c->setRestituitionCoeff(0.f); + + c->m_batchIdx = pairIndex; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB; + c->m_worldPosB[0] = pOnB1; + int numPoints = 1; + c->m_worldNormalOnB.w = (b3Scalar)numPoints; + }//if (dstIdx < numPairs) + } + }//if (hasCollision) + +} +#endif //B3_CONTACT_SPHERE_SPHERE_H diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h new file mode 100644 index 0000000000..5c5f4e297f --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h @@ -0,0 +1,40 @@ + +#ifndef B3_CONVEX_POLYHEDRON_DATA_H +#define B3_CONVEX_POLYHEDRON_DATA_H + + + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Common/shared/b3Quat.h" + +typedef struct b3GpuFace b3GpuFace_t; +struct b3GpuFace +{ + b3Float4 m_plane; + int m_indexOffset; + int m_numIndices; + int m_unusedPadding1; + int m_unusedPadding2; +}; + +typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t; + +struct b3ConvexPolyhedronData +{ + b3Float4 m_localCenter; + b3Float4 m_extents; + b3Float4 mC; + b3Float4 mE; + + float m_radius; + int m_faceOffset; + int m_numFaces; + int m_numVertices; + + int m_vertexOffset; + int m_uniqueEdgesOffset; + int m_numUniqueEdges; + int m_unused; +}; + +#endif //B3_CONVEX_POLYHEDRON_DATA_H diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h new file mode 100644 index 0000000000..89993f3565 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h @@ -0,0 +1,832 @@ +#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H +#define B3_FIND_CONCAVE_SEPARATING_AXIS_H + +#define B3_TRIANGLE_NUM_CONVEX_FACES 5 + + +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" + + +inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn, +const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max) +{ + min[0] = FLT_MAX; + max[0] = -FLT_MAX; + int numVerts = hull->m_numVertices; + + const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),*dir); + float offset = b3Dot(pos,*dir); + for(int i=0;i<numVerts;i++) + { + float dp = b3Dot(vertices[hull->m_vertexOffset+i],localDir); + if(dp < min[0]) + min[0] = dp; + if(dp > max[0]) + max[0] = dp; + } + if(min[0]>max[0]) + { + float tmp = min[0]; + min[0] = max[0]; + max[0] = tmp; + } + min[0] += offset; + max[0] += offset; +} + + +inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, + b3Float4ConstArg posA,b3QuatConstArg ornA, + b3Float4ConstArg posB,b3QuatConstArg ornB, + b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB,float* depth) +{ + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1); + + if(Max0<Min1 || Max1<Min0) + return false; + + float d0 = Max0 - Min1; + float d1 = Max1 - Min0; + *depth = d0<d1 ? d0:d1; + return true; +} + + +bool b3FindSeparatingAxis( const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, + b3Float4ConstArg posA1, + b3QuatConstArg ornA, + b3Float4ConstArg posB1, + b3QuatConstArg ornB, + b3Float4ConstArg DeltaC2, + + const b3Float4* verticesA, + const b3Float4* uniqueEdgesA, + const b3GpuFace* facesA, + const int* indicesA, + + __global const b3Float4* verticesB, + __global const b3Float4* uniqueEdgesB, + __global const b3GpuFace* facesB, + __global const int* indicesB, + b3Float4* sep, + float* dmin) +{ + + + b3Float4 posA = posA1; + posA.w = 0.f; + b3Float4 posB = posB1; + posB.w = 0.f; +/* + static int maxFaceVertex = 0; + + int curFaceVertexAB = hullA->m_numFaces*hullB->m_numVertices; + curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices; + + if (curFaceVertexAB>maxFaceVertex) + { + maxFaceVertex = curFaceVertexAB; + printf("curFaceVertexAB = %d\n",curFaceVertexAB); + printf("hullA->m_numFaces = %d\n",hullA->m_numFaces); + printf("hullA->m_numVertices = %d\n",hullA->m_numVertices); + printf("hullB->m_numVertices = %d\n",hullB->m_numVertices); + } +*/ + + int curPlaneTests=0; + { + int numFacesA = hullA->m_numFaces; + // Test normals from hullA + for(int i=0;i<numFacesA;i++) + { + const b3Float4 normal = facesA[hullA->m_faceOffset+i].m_plane; + b3Float4 faceANormalWS = b3QuatRotate(ornA,normal); + if (b3Dot(DeltaC2,faceANormalWS)<0) + faceANormalWS*=-1.f; + curPlaneTests++; + float d; + if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d)) + return false; + if(d<*dmin) + { + *dmin = d; + *sep = faceANormalWS; + } + } + } + if((b3Dot(-DeltaC2,*sep))>0.0f) + { + *sep = -(*sep); + } + return true; +} + + +b3Vector3 unitSphere162[]= +{ + b3MakeVector3(0.000000,-1.000000,0.000000), +b3MakeVector3(0.203181,-0.967950,0.147618), +b3MakeVector3(-0.077607,-0.967950,0.238853), +b3MakeVector3(0.723607,-0.447220,0.525725), +b3MakeVector3(0.609547,-0.657519,0.442856), +b3MakeVector3(0.812729,-0.502301,0.295238), +b3MakeVector3(-0.251147,-0.967949,0.000000), +b3MakeVector3(-0.077607,-0.967950,-0.238853), +b3MakeVector3(0.203181,-0.967950,-0.147618), +b3MakeVector3(0.860698,-0.251151,0.442858), +b3MakeVector3(-0.276388,-0.447220,0.850649), +b3MakeVector3(-0.029639,-0.502302,0.864184), +b3MakeVector3(-0.155215,-0.251152,0.955422), +b3MakeVector3(-0.894426,-0.447216,0.000000), +b3MakeVector3(-0.831051,-0.502299,0.238853), +b3MakeVector3(-0.956626,-0.251149,0.147618), +b3MakeVector3(-0.276388,-0.447220,-0.850649), +b3MakeVector3(-0.483971,-0.502302,-0.716565), +b3MakeVector3(-0.436007,-0.251152,-0.864188), +b3MakeVector3(0.723607,-0.447220,-0.525725), +b3MakeVector3(0.531941,-0.502302,-0.681712), +b3MakeVector3(0.687159,-0.251152,-0.681715), +b3MakeVector3(0.687159,-0.251152,0.681715), +b3MakeVector3(-0.436007,-0.251152,0.864188), +b3MakeVector3(-0.956626,-0.251149,-0.147618), +b3MakeVector3(-0.155215,-0.251152,-0.955422), +b3MakeVector3(0.860698,-0.251151,-0.442858), +b3MakeVector3(0.276388,0.447220,0.850649), +b3MakeVector3(0.483971,0.502302,0.716565), +b3MakeVector3(0.232822,0.657519,0.716563), +b3MakeVector3(-0.723607,0.447220,0.525725), +b3MakeVector3(-0.531941,0.502302,0.681712), +b3MakeVector3(-0.609547,0.657519,0.442856), +b3MakeVector3(-0.723607,0.447220,-0.525725), +b3MakeVector3(-0.812729,0.502301,-0.295238), +b3MakeVector3(-0.609547,0.657519,-0.442856), +b3MakeVector3(0.276388,0.447220,-0.850649), +b3MakeVector3(0.029639,0.502302,-0.864184), +b3MakeVector3(0.232822,0.657519,-0.716563), +b3MakeVector3(0.894426,0.447216,0.000000), +b3MakeVector3(0.831051,0.502299,-0.238853), +b3MakeVector3(0.753442,0.657515,0.000000), +b3MakeVector3(-0.232822,-0.657519,0.716563), +b3MakeVector3(-0.162456,-0.850654,0.499995), +b3MakeVector3(0.052790,-0.723612,0.688185), +b3MakeVector3(0.138199,-0.894429,0.425321), +b3MakeVector3(0.262869,-0.525738,0.809012), +b3MakeVector3(0.361805,-0.723611,0.587779), +b3MakeVector3(0.531941,-0.502302,0.681712), +b3MakeVector3(0.425323,-0.850654,0.309011), +b3MakeVector3(0.812729,-0.502301,-0.295238), +b3MakeVector3(0.609547,-0.657519,-0.442856), +b3MakeVector3(0.850648,-0.525736,0.000000), +b3MakeVector3(0.670817,-0.723611,-0.162457), +b3MakeVector3(0.670817,-0.723610,0.162458), +b3MakeVector3(0.425323,-0.850654,-0.309011), +b3MakeVector3(0.447211,-0.894428,0.000001), +b3MakeVector3(-0.753442,-0.657515,0.000000), +b3MakeVector3(-0.525730,-0.850652,0.000000), +b3MakeVector3(-0.638195,-0.723609,0.262864), +b3MakeVector3(-0.361801,-0.894428,0.262864), +b3MakeVector3(-0.688189,-0.525736,0.499997), +b3MakeVector3(-0.447211,-0.723610,0.525729), +b3MakeVector3(-0.483971,-0.502302,0.716565), +b3MakeVector3(-0.232822,-0.657519,-0.716563), +b3MakeVector3(-0.162456,-0.850654,-0.499995), +b3MakeVector3(-0.447211,-0.723611,-0.525727), +b3MakeVector3(-0.361801,-0.894429,-0.262863), +b3MakeVector3(-0.688189,-0.525736,-0.499997), +b3MakeVector3(-0.638195,-0.723609,-0.262863), +b3MakeVector3(-0.831051,-0.502299,-0.238853), +b3MakeVector3(0.361804,-0.723612,-0.587779), +b3MakeVector3(0.138197,-0.894429,-0.425321), +b3MakeVector3(0.262869,-0.525738,-0.809012), +b3MakeVector3(0.052789,-0.723611,-0.688186), +b3MakeVector3(-0.029639,-0.502302,-0.864184), +b3MakeVector3(0.956626,0.251149,0.147618), +b3MakeVector3(0.956626,0.251149,-0.147618), +b3MakeVector3(0.951058,-0.000000,0.309013), +b3MakeVector3(1.000000,0.000000,0.000000), +b3MakeVector3(0.947213,-0.276396,0.162458), +b3MakeVector3(0.951058,0.000000,-0.309013), +b3MakeVector3(0.947213,-0.276396,-0.162458), +b3MakeVector3(0.155215,0.251152,0.955422), +b3MakeVector3(0.436007,0.251152,0.864188), +b3MakeVector3(-0.000000,-0.000000,1.000000), +b3MakeVector3(0.309017,0.000000,0.951056), +b3MakeVector3(0.138199,-0.276398,0.951055), +b3MakeVector3(0.587786,0.000000,0.809017), +b3MakeVector3(0.447216,-0.276398,0.850648), +b3MakeVector3(-0.860698,0.251151,0.442858), +b3MakeVector3(-0.687159,0.251152,0.681715), +b3MakeVector3(-0.951058,-0.000000,0.309013), +b3MakeVector3(-0.809018,0.000000,0.587783), +b3MakeVector3(-0.861803,-0.276396,0.425324), +b3MakeVector3(-0.587786,0.000000,0.809017), +b3MakeVector3(-0.670819,-0.276397,0.688191), +b3MakeVector3(-0.687159,0.251152,-0.681715), +b3MakeVector3(-0.860698,0.251151,-0.442858), +b3MakeVector3(-0.587786,-0.000000,-0.809017), +b3MakeVector3(-0.809018,-0.000000,-0.587783), +b3MakeVector3(-0.670819,-0.276397,-0.688191), +b3MakeVector3(-0.951058,0.000000,-0.309013), +b3MakeVector3(-0.861803,-0.276396,-0.425324), +b3MakeVector3(0.436007,0.251152,-0.864188), +b3MakeVector3(0.155215,0.251152,-0.955422), +b3MakeVector3(0.587786,-0.000000,-0.809017), +b3MakeVector3(0.309017,-0.000000,-0.951056), +b3MakeVector3(0.447216,-0.276398,-0.850648), +b3MakeVector3(0.000000,0.000000,-1.000000), +b3MakeVector3(0.138199,-0.276398,-0.951055), +b3MakeVector3(0.670820,0.276396,0.688190), +b3MakeVector3(0.809019,-0.000002,0.587783), +b3MakeVector3(0.688189,0.525736,0.499997), +b3MakeVector3(0.861804,0.276394,0.425323), +b3MakeVector3(0.831051,0.502299,0.238853), +b3MakeVector3(-0.447216,0.276397,0.850649), +b3MakeVector3(-0.309017,-0.000001,0.951056), +b3MakeVector3(-0.262869,0.525738,0.809012), +b3MakeVector3(-0.138199,0.276397,0.951055), +b3MakeVector3(0.029639,0.502302,0.864184), +b3MakeVector3(-0.947213,0.276396,-0.162458), +b3MakeVector3(-1.000000,0.000001,0.000000), +b3MakeVector3(-0.850648,0.525736,-0.000000), +b3MakeVector3(-0.947213,0.276397,0.162458), +b3MakeVector3(-0.812729,0.502301,0.295238), +b3MakeVector3(-0.138199,0.276397,-0.951055), +b3MakeVector3(-0.309016,-0.000000,-0.951057), +b3MakeVector3(-0.262869,0.525738,-0.809012), +b3MakeVector3(-0.447215,0.276397,-0.850649), +b3MakeVector3(-0.531941,0.502302,-0.681712), +b3MakeVector3(0.861804,0.276396,-0.425322), +b3MakeVector3(0.809019,0.000000,-0.587782), +b3MakeVector3(0.688189,0.525736,-0.499997), +b3MakeVector3(0.670821,0.276397,-0.688189), +b3MakeVector3(0.483971,0.502302,-0.716565), +b3MakeVector3(0.077607,0.967950,0.238853), +b3MakeVector3(0.251147,0.967949,0.000000), +b3MakeVector3(0.000000,1.000000,0.000000), +b3MakeVector3(0.162456,0.850654,0.499995), +b3MakeVector3(0.361800,0.894429,0.262863), +b3MakeVector3(0.447209,0.723612,0.525728), +b3MakeVector3(0.525730,0.850652,0.000000), +b3MakeVector3(0.638194,0.723610,0.262864), +b3MakeVector3(-0.203181,0.967950,0.147618), +b3MakeVector3(-0.425323,0.850654,0.309011), +b3MakeVector3(-0.138197,0.894430,0.425320), +b3MakeVector3(-0.361804,0.723612,0.587778), +b3MakeVector3(-0.052790,0.723612,0.688185), +b3MakeVector3(-0.203181,0.967950,-0.147618), +b3MakeVector3(-0.425323,0.850654,-0.309011), +b3MakeVector3(-0.447210,0.894429,0.000000), +b3MakeVector3(-0.670817,0.723611,-0.162457), +b3MakeVector3(-0.670817,0.723611,0.162457), +b3MakeVector3(0.077607,0.967950,-0.238853), +b3MakeVector3(0.162456,0.850654,-0.499995), +b3MakeVector3(-0.138197,0.894430,-0.425320), +b3MakeVector3(-0.052790,0.723612,-0.688185), +b3MakeVector3(-0.361804,0.723612,-0.587778), +b3MakeVector3(0.361800,0.894429,-0.262863), +b3MakeVector3(0.638194,0.723610,-0.262864), +b3MakeVector3(0.447209,0.723612,-0.525728) +}; + + +bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, + b3Float4ConstArg posA1, + b3QuatConstArg ornA, + b3Float4ConstArg posB1, + b3QuatConstArg ornB, + b3Float4ConstArg DeltaC2, + const b3Float4* verticesA, + const b3Float4* uniqueEdgesA, + const b3GpuFace* facesA, + const int* indicesA, + __global const b3Float4* verticesB, + __global const b3Float4* uniqueEdgesB, + __global const b3GpuFace* facesB, + __global const int* indicesB, + b3Float4* sep, + float* dmin, + bool searchAllEdgeEdge) +{ + + + b3Float4 posA = posA1; + posA.w = 0.f; + b3Float4 posB = posB1; + posB.w = 0.f; + +// int curPlaneTests=0; + + int curEdgeEdge = 0; + // Test edges + static int maxEdgeTests = 0; + int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges; + if (curEdgeTests >maxEdgeTests ) + { + maxEdgeTests = curEdgeTests ; + printf("maxEdgeTests = %d\n",maxEdgeTests ); + printf("hullA->m_numUniqueEdges = %d\n",hullA->m_numUniqueEdges); + printf("hullB->m_numUniqueEdges = %d\n",hullB->m_numUniqueEdges); + + } + + + if (searchAllEdgeEdge) + { + for(int e0=0;e0<hullA->m_numUniqueEdges;e0++) + { + const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0]; + b3Float4 edge0World = b3QuatRotate(ornA,edge0); + + for(int e1=0;e1<hullB->m_numUniqueEdges;e1++) + { + const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1]; + b3Float4 edge1World = b3QuatRotate(ornB,edge1); + + + b3Float4 crossje = b3Cross(edge0World,edge1World); + + curEdgeEdge++; + if(!b3IsAlmostZero(crossje)) + { + crossje = b3Normalized(crossje); + if (b3Dot(DeltaC2,crossje)<0) + crossje *= -1.f; + + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); + + if(Max0<Min1 || Max1<Min0) + return false; + + float d0 = Max0 - Min1; + float d1 = Max1 - Min0; + dist = d0<d1 ? d0:d1; + result = true; + + } + + + if(dist<*dmin) + { + *dmin = dist; + *sep = crossje; + } + } + } + + } + } else + { + int numDirections = sizeof(unitSphere162)/sizeof(b3Vector3); + //printf("numDirections =%d\n",numDirections ); + + + for(int i=0;i<numDirections;i++) + { + b3Float4 crossje = unitSphere162[i]; + { + //if (b3Dot(DeltaC2,crossje)>0) + { + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); + + if(Max0<Min1 || Max1<Min0) + return false; + + float d0 = Max0 - Min1; + float d1 = Max1 - Min0; + dist = d0<d1 ? d0:d1; + result = true; + + } + + + if(dist<*dmin) + { + *dmin = dist; + *sep = crossje; + } + } + } + } + + } + + + if((b3Dot(-DeltaC2,*sep))>0.0f) + { + *sep = -(*sep); + } + return true; +} + + + +inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal, + __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB, + b3Float4ConstArg posA, b3QuatConstArg ornA,b3Float4ConstArg posB, b3QuatConstArg ornB, + __global b3Float4* worldVertsA1, + __global b3Float4* worldNormalsA1, + __global b3Float4* worldVertsB1, + int capacityWorldVerts, + const float minDist, float maxDist, + __global const b3Float4* verticesA, + __global const b3GpuFace_t* facesA, + __global const int* indicesA, + __global const b3Float4* verticesB, + __global const b3GpuFace_t* facesB, + __global const int* indicesB, + + __global b3Int4* clippingFaces, int pairIndex) +{ + int numContactsOut = 0; + int numWorldVertsB1= 0; + + + int closestFaceB=-1; + float dmax = -FLT_MAX; + + { + for(int face=0;face<hullB->m_numFaces;face++) + { + const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset+face].m_plane.x, + facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f); + const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal); + float d = b3Dot(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + + { + const b3GpuFace_t polyB = facesB[hullB->m_faceOffset+closestFaceB]; + const int numVertices = polyB.m_numIndices; + for(int e0=0;e0<numVertices;e0++) + { + const b3Float4 b = verticesB[hullB->m_vertexOffset+indicesB[polyB.m_indexOffset+e0]]; + worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = b3TransformPoint(b,posB,ornB); + } + } + + int closestFaceA=-1; + { + float dmin = FLT_MAX; + for(int face=0;face<hullA->m_numFaces;face++) + { + const b3Float4 Normal = b3MakeFloat4( + facesA[hullA->m_faceOffset+face].m_plane.x, + facesA[hullA->m_faceOffset+face].m_plane.y, + facesA[hullA->m_faceOffset+face].m_plane.z, + 0.f); + const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal); + + float d = b3Dot(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + worldNormalsA1[pairIndex] = faceANormalWS; + } + } + } + + int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices; + for(int e0=0;e0<numVerticesA;e0++) + { + const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]]; + worldVertsA1[pairIndex*capacityWorldVerts+e0] = b3TransformPoint(a, posA,ornA); + } + + clippingFaces[pairIndex].x = closestFaceA; + clippingFaces[pairIndex].y = closestFaceB; + clippingFaces[pairIndex].z = numVerticesA; + clippingFaces[pairIndex].w = numWorldVertsB1; + + + return numContactsOut; +} + + + + +__kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs, + __global const b3RigidBodyData* rigidBodies, + __global const b3Collidable* collidables, + __global const b3ConvexPolyhedronData* convexShapes, + __global const b3Float4* vertices, + __global const b3Float4* uniqueEdges, + __global const b3GpuFace* faces, + __global const int* indices, + __global const b3GpuChildShape* gpuChildShapes, + __global b3Aabb* aabbs, + __global b3Float4* concaveSeparatingNormalsOut, + __global b3Int4* clippingFacesOut, + __global b3Vector3* worldVertsA1Out, + __global b3Vector3* worldNormalsA1Out, + __global b3Vector3* worldVertsB1Out, + __global int* hasSeparatingNormals, + int vertexFaceCapacity, + int numConcavePairs, + int pairIdx + ) +{ + int i = pairIdx; +/* int i = get_global_id(0); + if (i>=numConcavePairs) + return; + int pairIdx = i; + */ + + int bodyIndexA = concavePairs[i].x; + int bodyIndexB = concavePairs[i].y; + + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&& + collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + concavePairs[pairIdx].w = -1; + return; + } + + hasSeparatingNormals[i] = 0; + +// int numFacesA = convexShapes[shapeIndexA].m_numFaces; + int numActualConcaveConvexTests = 0; + + int f = concavePairs[i].z; + + bool overlap = false; + + b3ConvexPolyhedronData convexPolyhedronA; + + //add 3 vertices of the triangle + convexPolyhedronA.m_numVertices = 3; + convexPolyhedronA.m_vertexOffset = 0; + b3Float4 localCenter = b3MakeFloat4(0.f,0.f,0.f,0.f); + + b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f]; + b3Aabb triAabb; + triAabb.m_minVec = b3MakeFloat4(1e30f,1e30f,1e30f,0.f); + triAabb.m_maxVec = b3MakeFloat4(-1e30f,-1e30f,-1e30f,0.f); + + b3Float4 verticesA[3]; + for (int i=0;i<3;i++) + { + int index = indices[face.m_indexOffset+i]; + b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index]; + verticesA[i] = vert; + localCenter += vert; + + triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec,vert); + triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec,vert); + + } + + overlap = true; + overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap; + overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap; + overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap; + + if (overlap) + { + float dmin = FLT_MAX; + int hasSeparatingAxis=5; + b3Float4 sepAxis=b3MakeFloat4(1,2,3,4); + + // int localCC=0; + numActualConcaveConvexTests++; + + //a triangle has 3 unique edges + convexPolyhedronA.m_numUniqueEdges = 3; + convexPolyhedronA.m_uniqueEdgesOffset = 0; + b3Float4 uniqueEdgesA[3]; + + uniqueEdgesA[0] = (verticesA[1]-verticesA[0]); + uniqueEdgesA[1] = (verticesA[2]-verticesA[1]); + uniqueEdgesA[2] = (verticesA[0]-verticesA[2]); + + + convexPolyhedronA.m_faceOffset = 0; + + b3Float4 normal = b3MakeFloat4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); + + b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES]; + int indicesA[3+3+2+2+2]; + int curUsedIndices=0; + int fidx=0; + + //front size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[0] = 0; + indicesA[1] = 1; + indicesA[2] = 2; + curUsedIndices+=3; + float c = face.m_plane.w; + facesA[fidx].m_plane.x = normal.x; + facesA[fidx].m_plane.y = normal.y; + facesA[fidx].m_plane.z = normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + //back size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[3]=2; + indicesA[4]=1; + indicesA[5]=0; + curUsedIndices+=3; + float c = b3Dot(normal,verticesA[0]); + // float c1 = -face.m_plane.w; + facesA[fidx].m_plane.x = -normal.x; + facesA[fidx].m_plane.y = -normal.y; + facesA[fidx].m_plane.z = -normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + + bool addEdgePlanes = true; + if (addEdgePlanes) + { + int numVertices=3; + int prevVertex = numVertices-1; + for (int i=0;i<numVertices;i++) + { + b3Float4 v0 = verticesA[i]; + b3Float4 v1 = verticesA[prevVertex]; + + b3Float4 edgeNormal = b3Normalized(b3Cross(normal,v1-v0)); + float c = -b3Dot(edgeNormal,v0); + + facesA[fidx].m_numIndices = 2; + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[curUsedIndices++]=i; + indicesA[curUsedIndices++]=prevVertex; + + facesA[fidx].m_plane.x = edgeNormal.x; + facesA[fidx].m_plane.y = edgeNormal.y; + facesA[fidx].m_plane.z = edgeNormal.z; + facesA[fidx].m_plane.w = c; + fidx++; + prevVertex = i; + } + } + convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES; + convexPolyhedronA.m_localCenter = localCenter*(1.f/3.f); + + + b3Float4 posA = rigidBodies[bodyIndexA].m_pos; + posA.w = 0.f; + b3Float4 posB = rigidBodies[bodyIndexB].m_pos; + posB.w = 0.f; + + b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat; + b3Quaternion ornB =rigidBodies[bodyIndexB].m_quat; + + + + + /////////////////// + ///compound shape support + + if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + int compoundChild = concavePairs[pairIdx].w; + int childShapeIndexB = compoundChild;//collidables[collidableIndexB].m_shapeIndex+compoundChild; + int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex; + b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition; + b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation; + b3Float4 newPosB = b3TransformPoint(childPosB,posB,ornB); + b3Quaternion newOrnB = b3QuatMul(ornB,childOrnB); + posB = newPosB; + ornB = newOrnB; + shapeIndexB = collidables[childColIndexB].m_shapeIndex; + } + ////////////////// + + b3Float4 c0local = convexPolyhedronA.m_localCenter; + b3Float4 c0 = b3TransformPoint(c0local, posA, ornA); + b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter; + b3Float4 c1 = b3TransformPoint(c1local,posB,ornB); + const b3Float4 DeltaC2 = c0 - c1; + + + bool sepA = b3FindSeparatingAxis( &convexPolyhedronA, &convexShapes[shapeIndexB], + posA,ornA, + posB,ornB, + DeltaC2, + verticesA,uniqueEdgesA,facesA,indicesA, + vertices,uniqueEdges,faces,indices, + &sepAxis,&dmin); + hasSeparatingAxis = 4; + if (!sepA) + { + hasSeparatingAxis = 0; + } else + { + bool sepB = b3FindSeparatingAxis( &convexShapes[shapeIndexB],&convexPolyhedronA, + posB,ornB, + posA,ornA, + DeltaC2, + vertices,uniqueEdges,faces,indices, + verticesA,uniqueEdgesA,facesA,indicesA, + &sepAxis,&dmin); + + if (!sepB) + { + hasSeparatingAxis = 0; + } else + { + bool sepEE = b3FindSeparatingAxisEdgeEdge( &convexPolyhedronA, &convexShapes[shapeIndexB], + posA,ornA, + posB,ornB, + DeltaC2, + verticesA,uniqueEdgesA,facesA,indicesA, + vertices,uniqueEdges,faces,indices, + &sepAxis,&dmin,true); + + if (!sepEE) + { + hasSeparatingAxis = 0; + } else + { + hasSeparatingAxis = 1; + } + } + } + + if (hasSeparatingAxis) + { + hasSeparatingNormals[i]=1; + sepAxis.w = dmin; + concaveSeparatingNormalsOut[pairIdx]=sepAxis; + + //now compute clipping faces A and B, and world-space clipping vertices A and B... + + float minDist = -1e30f; + float maxDist = 0.02f; + + b3FindClippingFaces(sepAxis, + &convexPolyhedronA, + &convexShapes[shapeIndexB], + posA,ornA, + posB,ornB, + worldVertsA1Out, + worldNormalsA1Out, + worldVertsB1Out, + vertexFaceCapacity, + minDist, maxDist, + verticesA, + facesA, + indicesA, + + vertices, + faces, + indices, + clippingFacesOut, pairIdx); + + } else + { + //mark this pair as in-active + concavePairs[pairIdx].w = -1; + } + } + else + { + //mark this pair as in-active + concavePairs[pairIdx].w = -1; + } +} + + +#endif //B3_FIND_CONCAVE_SEPARATING_AXIS_H + diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h new file mode 100644 index 0000000000..332dbc278c --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h @@ -0,0 +1,206 @@ +#ifndef B3_FIND_SEPARATING_AXIS_H +#define B3_FIND_SEPARATING_AXIS_H + + +inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max) +{ + min = FLT_MAX; + max = -FLT_MAX; + int numVerts = hull.m_numVertices; + + const b3Float4 localDir = b3QuatRotate(orn.inverse(),dir); + + b3Scalar offset = b3Dot3F4(pos,dir); + + for(int i=0;i<numVerts;i++) + { + //b3Vector3 pt = trans * vertices[m_vertexOffset+i]; + //b3Scalar dp = pt.dot(dir); + //b3Vector3 vertex = vertices[hull.m_vertexOffset+i]; + b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset+i],localDir); + //b3Assert(dp==dpL); + if(dp < min) min = dp; + if(dp > max) max = dp; + } + if(min>max) + { + b3Scalar tmp = min; + min = max; + max = tmp; + } + min += offset; + max += offset; +} + + +inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const b3Float4& posA,const b3Quaternion& ornA, + const b3Float4& posB,const b3Quaternion& ornB, + const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB,b3Scalar& depth) +{ + b3Scalar Min0,Max0; + b3Scalar Min1,Max1; + b3ProjectAxis(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0); + b3ProjectAxis(hullB,posB,ornB, sep_axis,verticesB, Min1, Max1); + + if(Max0<Min1 || Max1<Min0) + return false; + + b3Scalar d0 = Max0 - Min1; + b3Assert(d0>=0.0f); + b3Scalar d1 = Max1 - Min0; + b3Assert(d1>=0.0f); + depth = d0<d1 ? d0:d1; + return true; +} + + +inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const b3Float4& posA1, + const b3Quaternion& ornA, + const b3Float4& posB1, + const b3Quaternion& ornB, + const b3AlignedObjectArray<b3Vector3>& verticesA, + const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA, + const b3AlignedObjectArray<b3GpuFace>& facesA, + const b3AlignedObjectArray<int>& indicesA, + const b3AlignedObjectArray<b3Vector3>& verticesB, + const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB, + const b3AlignedObjectArray<b3GpuFace>& facesB, + const b3AlignedObjectArray<int>& indicesB, + + b3Vector3& sep) +{ + B3_PROFILE("findSeparatingAxis"); + + b3Float4 posA = posA1; + posA.w = 0.f; + b3Float4 posB = posB1; + posB.w = 0.f; +//#ifdef TEST_INTERNAL_OBJECTS + b3Float4 c0local = (b3Float4&)hullA.m_localCenter; + + b3Float4 c0 = b3TransformPoint(c0local, posA, ornA); + b3Float4 c1local = (b3Float4&)hullB.m_localCenter; + b3Float4 c1 = b3TransformPoint(c1local,posB,ornB); + const b3Float4 deltaC2 = c0 - c1; +//#endif + + b3Scalar dmin = FLT_MAX; + int curPlaneTests=0; + + int numFacesA = hullA.m_numFaces; + // Test normals from hullA + for(int i=0;i<numFacesA;i++) + { + const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset+i].m_plane; + b3Float4 faceANormalWS = b3QuatRotate(ornA,normal); + + if (b3Dot3F4(deltaC2,faceANormalWS)<0) + faceANormalWS*=-1.f; + + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + + b3Scalar d; + if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,faceANormalWS, verticesA, verticesB,d)) + return false; + + if(d<dmin) + { + dmin = d; + sep = (b3Vector3&)faceANormalWS; + } + } + + int numFacesB = hullB.m_numFaces; + // Test normals from hullB + for(int i=0;i<numFacesB;i++) + { + b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset+i].m_plane; + b3Float4 WorldNormal = b3QuatRotate(ornB, normal); + + if (b3Dot3F4(deltaC2,WorldNormal)<0) + { + WorldNormal*=-1.f; + } + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + b3Scalar d; + if(!b3TestSepAxis(hullA, hullB,posA,ornA,posB,ornB,WorldNormal,verticesA,verticesB,d)) + return false; + + if(d<dmin) + { + dmin = d; + sep = (b3Vector3&)WorldNormal; + } + } + +// b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend; + + int curEdgeEdge = 0; + // Test edges + for(int e0=0;e0<hullA.m_numUniqueEdges;e0++) + { + const b3Float4& edge0 = (b3Float4&) uniqueEdgesA[hullA.m_uniqueEdgesOffset+e0]; + b3Float4 edge0World = b3QuatRotate(ornA,(b3Float4&)edge0); + + for(int e1=0;e1<hullB.m_numUniqueEdges;e1++) + { + const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset+e1]; + b3Float4 edge1World = b3QuatRotate(ornB,(b3Float4&)edge1); + + + b3Float4 crossje = b3Cross3(edge0World,edge1World); + + curEdgeEdge++; + if(!b3IsAlmostZero((b3Vector3&)crossje)) + { + crossje = b3FastNormalized3(crossje); + if (b3Dot3F4(deltaC2,crossje)<0) + crossje*=-1.f; + + +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + b3Scalar dist; + if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,crossje, verticesA,verticesB,dist)) + return false; + + if(dist<dmin) + { + dmin = dist; + sep = (b3Vector3&)crossje; + } + } + } + + } + + + if((b3Dot3F4(-deltaC2,(b3Float4&)sep))>0.0f) + sep = -sep; + + return true; +} + +#endif //B3_FIND_SEPARATING_AXIS_H + diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h new file mode 100644 index 0000000000..6c3ad7c9dd --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h @@ -0,0 +1,920 @@ + +/*** + * --------------------------------- + * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz> + * + * This file was ported from mpr.c file, part of libccd. + * The Minkoski Portal Refinement implementation was ported + * to OpenCL by Erwin Coumans for the Bullet 3 Physics library. + * at http://github.com/erwincoumans/bullet3 + * + * Distributed under the OSI-approved BSD License (the "License"); + * see <http://www.opensource.org/licenses/bsd-license.php>. + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + + + + +#ifndef B3_MPR_PENETRATION_H +#define B3_MPR_PENETRATION_H + +#include "Bullet3Common/shared/b3PlatformDefinitions.h" +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" + + + + +#ifdef __cplusplus +#define B3_MPR_SQRT sqrtf +#else +#define B3_MPR_SQRT sqrt +#endif +#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y)) +#define B3_MPR_FABS fabs + +#define B3_MPR_TOLERANCE 1E-6f +#define B3_MPR_MAX_ITERATIONS 1000 + +struct _b3MprSupport_t +{ + b3Float4 v; //!< Support point in minkowski sum + b3Float4 v1; //!< Support point in obj1 + b3Float4 v2; //!< Support point in obj2 +}; +typedef struct _b3MprSupport_t b3MprSupport_t; + +struct _b3MprSimplex_t +{ + b3MprSupport_t ps[4]; + int last; //!< index of last added point +}; +typedef struct _b3MprSimplex_t b3MprSimplex_t; + +inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx) +{ + return &s->ps[idx]; +} + +inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size) +{ + s->last = size - 1; +} + + +inline int b3MprSimplexSize(const b3MprSimplex_t *s) +{ + return s->last + 1; +} + + +inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} + +inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s) +{ + *d = *s; +} + +inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a) +{ + b3MprSupportCopy(s->ps + pos, a); +} + + +inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2) +{ + b3MprSupport_t supp; + + b3MprSupportCopy(&supp, &s->ps[pos1]); + b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]); + b3MprSupportCopy(&s->ps[pos2], &supp); +} + + +inline int b3MprIsZero(float val) +{ + return B3_MPR_FABS(val) < FLT_EPSILON; +} + + + +inline int b3MprEq(float _a, float _b) +{ + float ab; + float a, b; + + ab = B3_MPR_FABS(_a - _b); + if (B3_MPR_FABS(ab) < FLT_EPSILON) + return 1; + + a = B3_MPR_FABS(_a); + b = B3_MPR_FABS(_b); + if (b > a){ + return ab < FLT_EPSILON * b; + }else{ + return ab < FLT_EPSILON * a; + } +} + + +inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b) +{ + return b3MprEq((*a).x, (*b).x) + && b3MprEq((*a).y, (*b).y) + && b3MprEq((*a).z, (*b).z); +} + + + +inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, b3ConstArray(b3Float4) verticesA) +{ + b3Float4 supVec = b3MakeFloat4(0,0,0,0); + float maxDot = -B3_LARGE_FLOAT; + + if( 0 < hull->m_numVertices ) + { + const b3Float4 scaled = supportVec; + int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot); + return verticesA[hull->m_vertexOffset+index]; + } + + return supVec; + +} + + +B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + const b3Float4* _dir, b3Float4* outp, int logme) +{ + //dir is in worldspace, move to local space + + b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos; + b3Quat orn = cpuBodyBuf[bodyIndex].m_quat; + + b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f); + + const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir); + + + //find local support vertex + int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx; + + b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL); + __global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex]; + + b3Float4 pInA; + if (logme) + { + + + // b3Float4 supVec = b3MakeFloat4(0,0,0,0); + float maxDot = -B3_LARGE_FLOAT; + + if( 0 < hull->m_numVertices ) + { + const b3Float4 scaled = localDir; + int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot); + pInA = cpuVertices[hull->m_vertexOffset+index]; + + } + + + } else + { + pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices); + } + + //move vertex to world space + *outp = b3TransformPoint(pInA,pos,orn); + +} + +inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + const b3Float4* _dir, b3MprSupport_t *supp) +{ + b3Float4 dir; + dir = *_dir; + b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0); + dir = *_dir*-1.f; + b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0); + supp->v = supp->v1 - supp->v2; +} + + + + + + + + + +inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center) +{ + + center->v1 = cpuBodyBuf[bodyIndexA].m_pos; + center->v2 = cpuBodyBuf[bodyIndexB].m_pos; + center->v = center->v1 - center->v2; +} + +inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z) +{ + (*v).x = x; + (*v).y = y; + (*v).z = z; + (*v).w = 0.f; +} + +inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w) +{ + (*v).x += (*w).x; + (*v).y += (*w).y; + (*v).z += (*w).z; +} + +inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w) +{ + *v = *w; +} + +inline void b3MprVec3Scale(b3Float4 *d, float k) +{ + *d *= k; +} + +inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b) +{ + float dot; + + dot = b3Dot3F4(*a,*b); + return dot; +} + + +inline float b3MprVec3Len2(const b3Float4 *v) +{ + return b3MprVec3Dot(v, v); +} + +inline void b3MprVec3Normalize(b3Float4 *d) +{ + float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d)); + b3MprVec3Scale(d, k); +} + +inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b) +{ + *d = b3Cross3(*a,*b); + +} + + +inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w) +{ + *d = *v - *w; +} + +inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir) +{ + b3Float4 v2v1, v3v1; + + b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 1)->v); + b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 1)->v); + b3MprVec3Cross(dir, &v2v1, &v3v1); + b3MprVec3Normalize(dir); +} + + +inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal, + const b3Float4 *dir) +{ + float dot; + dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v); + return b3MprIsZero(dot) || dot > 0.f; +} + +inline int portalReachTolerance(const b3MprSimplex_t *portal, + const b3MprSupport_t *v4, + const b3Float4 *dir) +{ + float dv1, dv2, dv3, dv4; + float dot1, dot2, dot3; + + // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} + + dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir); + dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir); + dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir); + dv4 = b3MprVec3Dot(&v4->v, dir); + + dot1 = dv4 - dv1; + dot2 = dv4 - dv2; + dot3 = dv4 - dv3; + + dot1 = B3_MPR_FMIN(dot1, dot2); + dot1 = B3_MPR_FMIN(dot1, dot3); + + return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE; +} + +inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal, + const b3MprSupport_t *v4, + const b3Float4 *dir) +{ + float dot; + dot = b3MprVec3Dot(&v4->v, dir); + return b3MprIsZero(dot) || dot > 0.f; +} + +inline void b3ExpandPortal(b3MprSimplex_t *portal, + const b3MprSupport_t *v4) +{ + float dot; + b3Float4 v4v0; + + b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v); + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0); + if (dot > 0.f){ + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0); + if (dot > 0.f){ + b3MprSimplexSet(portal, 1, v4); + }else{ + b3MprSimplexSet(portal, 3, v4); + } + }else{ + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0); + if (dot > 0.f){ + b3MprSimplexSet(portal, 2, v4); + }else{ + b3MprSimplexSet(portal, 1, v4); + } + } +} + + + +B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + __global int* hasSepAxis, + b3MprSimplex_t *portal) +{ + b3Float4 dir, va, vb; + float dot; + int cont; + + + + // vertex 0 is center of portal + b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0)); + // vertex 0 is center of portal + b3MprSimplexSetSize(portal, 1); + + + + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){ + // Portal's center lies on origin (0,0,0) => we know that objects + // intersect but we would need to know penetration info. + // So move center little bit... + b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); + b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va); + } + + + // vertex 1 = support in direction of origin + b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Scale(&dir, -1.f); + b3MprVec3Normalize(&dir); + + + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1)); + + b3MprSimplexSetSize(portal, 2); + + // test if origin isn't outside of v1 + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir); + + + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + + // vertex 2 + b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v, + &b3MprSimplexPoint(portal, 1)->v); + if (b3MprIsZero(b3MprVec3Len2(&dir))){ + if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){ + // origin lies on v1 + return 1; + }else{ + // origin lies on v0-v1 segment + return 2; + } + } + + b3MprVec3Normalize(&dir); + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2)); + + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir); + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + b3MprSimplexSetSize(portal, 3); + + // vertex 3 direction + b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Cross(&dir, &va, &vb); + b3MprVec3Normalize(&dir); + + // it is better to form portal faces to be oriented "outside" origin + dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v); + if (dot > 0.f){ + b3MprSimplexSwap(portal, 1, 2); + b3MprVec3Scale(&dir, -1.f); + } + + while (b3MprSimplexSize(portal) < 4){ + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3)); + + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir); + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + cont = 0; + + // test if origin is outside (v1, v0, v3) - set v2 as v3 and + // continue + b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 3)->v); + dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !b3MprIsZero(dot)){ + b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3)); + cont = 1; + } + + if (!cont){ + // test if origin is outside (v3, v0, v2) - set v1 as v3 and + // continue + b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 2)->v); + dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !b3MprIsZero(dot)){ + b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3)); + cont = 1; + } + } + + if (cont){ + b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Cross(&dir, &va, &vb); + b3MprVec3Normalize(&dir); + }else{ + b3MprSimplexSetSize(portal, 4); + } + } + + return 0; +} + + +B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + b3MprSimplex_t *portal) +{ + b3Float4 dir; + b3MprSupport_t v4; + + for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++) + //while (1) + { + // compute direction outside the portal (from v0 throught v1,v2,v3 + // face) + b3PortalDir(portal, &dir); + + // test if origin is inside the portal + if (portalEncapsulesOrigin(portal, &dir)) + return 0; + + // get next support point + + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4); + + + // test if v4 can expand portal to contain origin and if portal + // expanding doesn't reach given tolerance + if (!portalCanEncapsuleOrigin(portal, &v4, &dir) + || portalReachTolerance(portal, &v4, &dir)) + { + return -1; + } + + // v1-v2-v3 triangle must be rearranged to face outside Minkowski + // difference (direction from v0). + b3ExpandPortal(portal, &v4); + } + + return -1; +} + +B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos) +{ + + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + b3Float4 dir; + size_t i; + float b[4], sum, inv; + b3Float4 vec, p1, p2; + + b3PortalDir(portal, &dir); + + // use barycentric coordinates of tetrahedron to find origin + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 2)->v); + b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 2)->v); + b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v); + + sum = b[0] + b[1] + b[2] + b[3]; + + if (b3MprIsZero(sum) || sum < 0.f){ + b[0] = 0.f; + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 3)->v); + b[1] = b3MprVec3Dot(&vec, &dir); + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[2] = b3MprVec3Dot(&vec, &dir); + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 2)->v); + b[3] = b3MprVec3Dot(&vec, &dir); + + sum = b[1] + b[2] + b[3]; + } + + inv = 1.f / sum; + + b3MprVec3Copy(&p1, b3mpr_vec3_origin); + b3MprVec3Copy(&p2, b3mpr_vec3_origin); + for (i = 0; i < 4; i++){ + b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1); + b3MprVec3Scale(&vec, b[i]); + b3MprVec3Add(&p1, &vec); + + b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2); + b3MprVec3Scale(&vec, b[i]); + b3MprVec3Add(&p2, &vec); + } + b3MprVec3Scale(&p1, inv); + b3MprVec3Scale(&p2, inv); + + b3MprVec3Copy(pos, &p1); + b3MprVec3Add(pos, &p2); + b3MprVec3Scale(pos, 0.5); +} + +inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b) +{ + b3Float4 ab; + b3MprVec3Sub2(&ab, a, b); + return b3MprVec3Len2(&ab); +} + +inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P, + const b3Float4 *x0, + const b3Float4 *b, + b3Float4 *witness) +{ + // The computation comes from solving equation of segment: + // S(t) = x0 + t.d + // where - x0 is initial point of segment + // - d is direction of segment from x0 (|d| > 0) + // - t belongs to <0, 1> interval + // + // Than, distance from a segment to some point P can be expressed: + // D(t) = |x0 + t.d - P|^2 + // which is distance from any point on segment. Minimization + // of this function brings distance from P to segment. + // Minimization of D(t) leads to simple quadratic equation that's + // solving is straightforward. + // + // Bonus of this method is witness point for free. + + float dist, t; + b3Float4 d, a; + + // direction of segment + b3MprVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + b3MprVec3Sub2(&a, x0, P); + + t = -1.f * b3MprVec3Dot(&a, &d); + t /= b3MprVec3Len2(&d); + + if (t < 0.f || b3MprIsZero(t)){ + dist = b3MprVec3Dist2(x0, P); + if (witness) + b3MprVec3Copy(witness, x0); + }else if (t > 1.f || b3MprEq(t, 1.f)){ + dist = b3MprVec3Dist2(b, P); + if (witness) + b3MprVec3Copy(witness, b); + }else{ + if (witness){ + b3MprVec3Copy(witness, &d); + b3MprVec3Scale(witness, t); + b3MprVec3Add(witness, x0); + dist = b3MprVec3Dist2(witness, P); + }else{ + // recycling variables + b3MprVec3Scale(&d, t); + b3MprVec3Add(&d, &a); + dist = b3MprVec3Len2(&d); + } + } + + return dist; +} + + +inline float b3MprVec3PointTriDist2(const b3Float4 *P, + const b3Float4 *x0, const b3Float4 *B, + const b3Float4 *C, + b3Float4 *witness) +{ + // Computation comes from analytic expression for triangle (x0, B, C) + // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and + // Then equation for distance is: + // D(s, t) = | T(s, t) - P |^2 + // This leads to minimization of quadratic function of two variables. + // The solution from is taken only if s is between 0 and 1, t is + // between 0 and 1 and t + s < 1, otherwise distance from segment is + // computed. + + b3Float4 d1, d2, a; + float u, v, w, p, q, r; + float s, t, dist, dist2; + b3Float4 witness2; + + b3MprVec3Sub2(&d1, B, x0); + b3MprVec3Sub2(&d2, C, x0); + b3MprVec3Sub2(&a, x0, P); + + u = b3MprVec3Dot(&a, &a); + v = b3MprVec3Dot(&d1, &d1); + w = b3MprVec3Dot(&d2, &d2); + p = b3MprVec3Dot(&a, &d1); + q = b3MprVec3Dot(&a, &d2); + r = b3MprVec3Dot(&d1, &d2); + + s = (q * r - w * p) / (w * v - r * r); + t = (-s * r - q) / w; + + if ((b3MprIsZero(s) || s > 0.f) + && (b3MprEq(s, 1.f) || s < 1.f) + && (b3MprIsZero(t) || t > 0.f) + && (b3MprEq(t, 1.f) || t < 1.f) + && (b3MprEq(t + s, 1.f) || t + s < 1.f)){ + + if (witness){ + b3MprVec3Scale(&d1, s); + b3MprVec3Scale(&d2, t); + b3MprVec3Copy(witness, x0); + b3MprVec3Add(witness, &d1); + b3MprVec3Add(witness, &d2); + + dist = b3MprVec3Dist2(witness, P); + }else{ + dist = s * s * v; + dist += t * t * w; + dist += 2.f * s * t * r; + dist += 2.f * s * p; + dist += 2.f * t * q; + dist += u; + } + }else{ + dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + b3MprVec3Copy(witness, &witness2); + } + + dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + b3MprVec3Copy(witness, &witness2); + } + } + + return dist; +} + + +B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + b3MprSimplex_t *portal, + float *depth, b3Float4 *pdir, b3Float4 *pos) +{ + b3Float4 dir; + b3MprSupport_t v4; + unsigned long iterations; + + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + + iterations = 1UL; + for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++) + //while (1) + { + // compute portal direction and obtain next support point + b3PortalDir(portal, &dir); + + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4); + + + // reached tolerance -> find penetration info + if (portalReachTolerance(portal, &v4, &dir) + || iterations ==B3_MPR_MAX_ITERATIONS) + { + *depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir); + *depth = B3_MPR_SQRT(*depth); + + if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z)) + { + + *pdir = dir; + } + b3MprVec3Normalize(pdir); + + // barycentric coordinates: + b3FindPos(portal, pos); + + + return; + } + + b3ExpandPortal(portal, &v4); + + iterations++; + } +} + +B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos) +{ + // Touching contact on portal's v1 - so depth is zero and direction + // is unimportant and pos can be guessed + *depth = 0.f; + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + + b3MprVec3Copy(dir, b3mpr_vec3_origin); + + b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1); + b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2); + b3MprVec3Scale(pos, 0.5); +} + +B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal, + float *depth, b3Float4 *dir, b3Float4 *pos) +{ + + // Origin lies on v0-v1 segment. + // Depth is distance to v1, direction also and position must be + // computed + + b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1); + b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2); + b3MprVec3Scale(pos, 0.5f); + + + b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v); + *depth = B3_MPR_SQRT(b3MprVec3Len2(dir)); + b3MprVec3Normalize(dir); +} + + + +inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB, + b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + __global int* hasSepAxis, + float *depthOut, b3Float4* dirOut, b3Float4* posOut) +{ + + b3MprSimplex_t portal; + + +// if (!hasSepAxis[pairIndex]) + // return -1; + + hasSepAxis[pairIndex] = 0; + int res; + + // Phase 1: Portal discovery + res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal); + + + //sepAxis[pairIndex] = *pdir;//or -dir? + + switch (res) + { + case 0: + { + // Phase 2: Portal refinement + + res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal); + if (res < 0) + return -1; + + // Phase 3. Penetration info + b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut); + hasSepAxis[pairIndex] = 1; + sepAxis[pairIndex] = -*dirOut; + break; + } + case 1: + { + // Touching contact on portal's v1. + b3FindPenetrTouch(&portal, depthOut, dirOut, posOut); + break; + } + case 2: + { + + b3FindPenetrSegment( &portal, depthOut, dirOut, posOut); + break; + } + default: + { + hasSepAxis[pairIndex]=0; + //if (res < 0) + //{ + // Origin isn't inside portal - no collision. + return -1; + //} + } + }; + + return 0; +}; + + + +#endif //B3_MPR_PENETRATION_H diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h new file mode 100644 index 0000000000..718222ebca --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h @@ -0,0 +1,196 @@ + +#ifndef B3_NEW_CONTACT_REDUCTION_H +#define B3_NEW_CONTACT_REDUCTION_H + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +#define GET_NPOINTS(x) (x).m_worldNormalOnB.w + + +int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx) +{ + if( nPoints == 0 ) + return 0; + + if (nPoints <=4) + return nPoints; + + + if (nPoints >64) + nPoints = 64; + + b3Float4 center = b3MakeFloat4(0,0,0,0); + { + + for (int i=0;i<nPoints;i++) + center += p[i]; + center /= (float)nPoints; + } + + + + // sample 4 directions + + b3Float4 aVector = p[0] - center; + b3Float4 u = b3Cross( nearNormal, aVector ); + b3Float4 v = b3Cross( nearNormal, u ); + u = b3Normalized( u ); + v = b3Normalized( v ); + + + //keep point with deepest penetration + float minW= FLT_MAX; + + int minIndex=-1; + + b3Float4 maxDots; + maxDots.x = FLT_MIN; + maxDots.y = FLT_MIN; + maxDots.z = FLT_MIN; + maxDots.w = FLT_MIN; + + // idx, distance + for(int ie = 0; ie<nPoints; ie++ ) + { + if (p[ie].w<minW) + { + minW = p[ie].w; + minIndex=ie; + } + float f; + b3Float4 r = p[ie]-center; + f = b3Dot( u, r ); + if (f<maxDots.x) + { + maxDots.x = f; + contactIdx[0].x = ie; + } + + f = b3Dot( -u, r ); + if (f<maxDots.y) + { + maxDots.y = f; + contactIdx[0].y = ie; + } + + + f = b3Dot( v, r ); + if (f<maxDots.z) + { + maxDots.z = f; + contactIdx[0].z = ie; + } + + f = b3Dot( -v, r ); + if (f<maxDots.w) + { + maxDots.w = f; + contactIdx[0].w = ie; + } + + } + + if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex) + { + //replace the first contact with minimum (todo: replace contact with least penetration) + contactIdx[0].x = minIndex; + } + + return 4; + +} + +__kernel void b3NewContactReductionKernel( __global b3Int4* pairs, + __global const b3RigidBodyData_t* rigidBodies, + __global const b3Float4* separatingNormals, + __global const int* hasSeparatingAxis, + __global struct b3Contact4Data* globalContactsOut, + __global b3Int4* clippingFaces, + __global b3Float4* worldVertsB2, + volatile __global int* nGlobalContactsOut, + int vertexFaceCapacity, + int contactCapacity, + int numPairs, + int pairIndex + ) +{ +// int i = get_global_id(0); + //int pairIndex = i; + int i = pairIndex; + + b3Int4 contactIdx; + contactIdx=b3MakeInt4(0,1,2,3); + + if (i<numPairs) + { + + if (hasSeparatingAxis[i]) + { + + + + + int nPoints = clippingFaces[pairIndex].w; + + if (nPoints>0) + { + + __global b3Float4* pointsIn = &worldVertsB2[pairIndex*vertexFaceCapacity]; + b3Float4 normal = -separatingNormals[i]; + + int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx); + + int dstIdx; + dstIdx = b3AtomicInc( nGlobalContactsOut); + +//#if 0 + b3Assert(dstIdx < contactCapacity); + if (dstIdx < contactCapacity) + { + + __global struct b3Contact4Data* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = -normal; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + int bodyA = pairs[pairIndex].x; + int bodyB = pairs[pairIndex].y; + + pairs[pairIndex].w = dstIdx; + + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB; + c->m_childIndexA =-1; + c->m_childIndexB =-1; + + switch (nReducedContacts) + { + case 4: + c->m_worldPosB[3] = pointsIn[contactIdx.w]; + case 3: + c->m_worldPosB[2] = pointsIn[contactIdx.z]; + case 2: + c->m_worldPosB[1] = pointsIn[contactIdx.y]; + case 1: + c->m_worldPosB[0] = pointsIn[contactIdx.x]; + default: + { + } + }; + + GET_NPOINTS(*c) = nReducedContacts; + + } + + +//#endif + + }// if (numContactsOut>0) + }// if (hasSeparatingAxis[i]) + }// if (i<numPairs) + + + +} +#endif diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h new file mode 100644 index 0000000000..3661e43cf1 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h @@ -0,0 +1,90 @@ + + +#ifndef B3_QUANTIZED_BVH_NODE_H +#define B3_QUANTIZED_BVH_NODE_H + +#include "Bullet3Common/shared/b3Float4.h" + +#define B3_MAX_NUM_PARTS_IN_BITS 10 + +///b3QuantizedBvhNodeData is a compressed aabb node, 16 bytes. +///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). +typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t; + +struct b3QuantizedBvhNodeData +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes + int m_escapeIndexOrTriangleIndex; +}; + +inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode) +{ + unsigned int x=0; + unsigned int y = (~(x&0))<<(31-B3_MAX_NUM_PARTS_IN_BITS); + // Get only the lower bits where the triangle index is stored + return (rootNode->m_escapeIndexOrTriangleIndex&~(y)); +} + +inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode) +{ + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0; +} + +inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode) +{ + return -rootNode->m_escapeIndexOrTriangleIndex; +} + +inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization) +{ + b3Float4 clampedPoint = b3MaxFloat4(point2,bvhAabbMin); + clampedPoint = b3MinFloat4 (clampedPoint, bvhAabbMax); + + b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization; + if (isMax) + { + out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1)); + out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1)); + out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1)); + } else + { + out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe)); + out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe)); + out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe)); + } + +} + + +inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow( + const unsigned short int* aabbMin1, + const unsigned short int* aabbMax1, + const unsigned short int* aabbMin2, + const unsigned short int* aabbMax2) +{ + //int overlap = 1; + if (aabbMin1[0] > aabbMax2[0]) + return 0; + if (aabbMax1[0] < aabbMin2[0]) + return 0; + if (aabbMin1[1] > aabbMax2[1]) + return 0; + if (aabbMax1[1] < aabbMin2[1]) + return 0; + if (aabbMin1[2] > aabbMax2[2]) + return 0; + if (aabbMax1[2] < aabbMin2[2]) + return 0; + return 1; + //overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap; + //overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap; + //overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap; + //return overlap; +} + + +#endif //B3_QUANTIZED_BVH_NODE_H diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h new file mode 100644 index 0000000000..35b5197006 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h @@ -0,0 +1,97 @@ +#ifndef B3_REDUCE_CONTACTS_H +#define B3_REDUCE_CONTACTS_H + +inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx) +{ + if( nPoints == 0 ) + return 0; + + if (nPoints <=4) + return nPoints; + + + if (nPoints >64) + nPoints = 64; + + b3Float4 center = b3MakeFloat4(0,0,0,0); + { + + for (int i=0;i<nPoints;i++) + center += p[i]; + center /= (float)nPoints; + } + + + + // sample 4 directions + + b3Float4 aVector = p[0] - center; + b3Float4 u = b3Cross3( nearNormal, aVector ); + b3Float4 v = b3Cross3( nearNormal, u ); + u = b3FastNormalized3( u ); + v = b3FastNormalized3( v ); + + + //keep point with deepest penetration + float minW= FLT_MAX; + + int minIndex=-1; + + b3Float4 maxDots; + maxDots.x = FLT_MIN; + maxDots.y = FLT_MIN; + maxDots.z = FLT_MIN; + maxDots.w = FLT_MIN; + + // idx, distance + for(int ie = 0; ie<nPoints; ie++ ) + { + if (p[ie].w<minW) + { + minW = p[ie].w; + minIndex=ie; + } + float f; + b3Float4 r = p[ie]-center; + f = b3Dot3F4( u, r ); + if (f<maxDots.x) + { + maxDots.x = f; + contactIdx[0].x = ie; + } + + f = b3Dot3F4( -u, r ); + if (f<maxDots.y) + { + maxDots.y = f; + contactIdx[0].y = ie; + } + + + f = b3Dot3F4( v, r ); + if (f<maxDots.z) + { + maxDots.z = f; + contactIdx[0].z = ie; + } + + f = b3Dot3F4( -v, r ); + if (f<maxDots.w) + { + maxDots.w = f; + contactIdx[0].w = ie; + } + + } + + if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex) + { + //replace the first contact with minimum (todo: replace contact with least penetration) + contactIdx[0].x = minIndex; + } + + return 4; + +} + +#endif //B3_REDUCE_CONTACTS_H diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h new file mode 100644 index 0000000000..50632c871f --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h @@ -0,0 +1,34 @@ +#ifndef B3_RIGIDBODY_DATA_H +#define B3_RIGIDBODY_DATA_H + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Common/shared/b3Quat.h" +#include "Bullet3Common/shared/b3Mat3x3.h" + +typedef struct b3RigidBodyData b3RigidBodyData_t; + + +struct b3RigidBodyData +{ + b3Float4 m_pos; + b3Quat m_quat; + b3Float4 m_linVel; + b3Float4 m_angVel; + + int m_collidableIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +}; + +typedef struct b3InertiaData b3InertiaData_t; + +struct b3InertiaData +{ + b3Mat3x3 m_invInertiaWorld; + b3Mat3x3 m_initInvInertia; +}; + + +#endif //B3_RIGIDBODY_DATA_H +
\ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h new file mode 100644 index 0000000000..8d40d19a03 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h @@ -0,0 +1,40 @@ +#ifndef B3_UPDATE_AABBS_H +#define B3_UPDATE_AABBS_H + + + +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + + + +void b3ComputeWorldAabb( int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs) +{ + __global const b3RigidBodyData_t* body = &bodies[bodyId]; + + b3Float4 position = body->m_pos; + b3Quat orientation = body->m_quat; + + int collidableIndex = body->m_collidableIdx; + int shapeIndex = collidables[collidableIndex].m_shapeIndex; + + if (shapeIndex>=0) + { + + b3Aabb_t localAabb = localShapeAABB[collidableIndex]; + b3Aabb_t worldAabb; + + b3Float4 aabbAMinOut,aabbAMaxOut; + float margin = 0.f; + b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&aabbAMinOut,&aabbAMaxOut); + + worldAabb.m_minVec =aabbAMinOut; + worldAabb.m_minIndices[3] = bodyId; + worldAabb.m_maxVec = aabbAMaxOut; + worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass==0.f? 0 : 1; + worldAabbs[bodyId] = worldAabb; + } +} + +#endif //B3_UPDATE_AABBS_H |