diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp new file mode 100644 index 00000000000..7c41c8d8f71 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -0,0 +1,1289 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btDbvt implementation by Nathanael Presson + +#include "btDbvt.h" + +// +typedef btAlignedObjectArray tNodeArray; +typedef btAlignedObjectArray tConstNodeArray; + +// +struct btDbvtNodeEnumerator : btDbvt::ICollide +{ +tConstNodeArray nodes; +void Process(const btDbvtNode* n) { nodes.push_back(n); } +}; + +// +static DBVT_INLINE int indexof(const btDbvtNode* node) +{ +return(node->parent->childs[1]==node); +} + +// +static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, + const btDbvtVolume& b) +{ +#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE +DBVT_ALIGN char locals[sizeof(btDbvtAabbMm)]; +btDbvtVolume& res=*(btDbvtVolume*)locals; +#else +btDbvtVolume res; +#endif +Merge(a,b,res); +return(res); +} + +// volume+edge lengths +static DBVT_INLINE btScalar size(const btDbvtVolume& a) +{ +const btVector3 edges=a.Lengths(); +return( edges.x()*edges.y()*edges.z()+ + edges.x()+edges.y()+edges.z()); +} + +// +static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth) +{ +if(node->isinternal()) + { + getmaxdepth(node->childs[0],depth+1,maxdepth); + getmaxdepth(node->childs[0],depth+1,maxdepth); + } else maxdepth=btMax(maxdepth,depth); +} + +// +static DBVT_INLINE void deletenode( btDbvt* pdbvt, + btDbvtNode* node) +{ +btAlignedFree(pdbvt->m_free); +pdbvt->m_free=node; +} + +// +static void recursedeletenode( btDbvt* pdbvt, + btDbvtNode* node) +{ +if(!node->isleaf()) + { + recursedeletenode(pdbvt,node->childs[0]); + recursedeletenode(pdbvt,node->childs[1]); + } +if(node==pdbvt->m_root) pdbvt->m_root=0; +deletenode(pdbvt,node); +} + +// +static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, + btDbvtNode* parent, + void* data) +{ +btDbvtNode* node; +if(pdbvt->m_free) + { node=pdbvt->m_free;pdbvt->m_free=0; } + else + { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); } +node->parent = parent; +node->data = data; +node->childs[1] = 0; +return(node); +} + +// +static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, + btDbvtNode* parent, + const btDbvtVolume& volume, + void* data) +{ +btDbvtNode* node=createnode(pdbvt,parent,data); +node->volume=volume; +return(node); +} + +// +static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, + btDbvtNode* parent, + const btDbvtVolume& volume0, + const btDbvtVolume& volume1, + void* data) +{ +btDbvtNode* node=createnode(pdbvt,parent,data); +Merge(volume0,volume1,node->volume); +return(node); +} + +// +static void insertleaf( btDbvt* pdbvt, + btDbvtNode* root, + btDbvtNode* leaf) +{ +if(!pdbvt->m_root) + { + pdbvt->m_root = leaf; + leaf->parent = 0; + } + else + { + if(!root->isleaf()) + { + do { + root=root->childs[Select( leaf->volume, + root->childs[0]->volume, + root->childs[1]->volume)]; + } while(!root->isleaf()); + } + btDbvtNode* prev=root->parent; + btDbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0); + if(prev) + { + prev->childs[indexof(root)] = node; + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + do { + if(!prev->volume.Contain(node->volume)) + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + else + break; + node=prev; + } while(0!=(prev=node->parent)); + } + else + { + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + pdbvt->m_root = node; + } + } +} + +// +static btDbvtNode* removeleaf( btDbvt* pdbvt, + btDbvtNode* leaf) +{ +if(leaf==pdbvt->m_root) + { + pdbvt->m_root=0; + return(0); + } + else + { + btDbvtNode* parent=leaf->parent; + btDbvtNode* prev=parent->parent; + btDbvtNode* sibling=parent->childs[1-indexof(leaf)]; + if(prev) + { + prev->childs[indexof(parent)]=sibling; + sibling->parent=prev; + deletenode(pdbvt,parent); + while(prev) + { + const btDbvtVolume pb=prev->volume; + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + if(NotEqual(pb,prev->volume)) + { + prev=prev->parent; + } else break; + } + return(prev?prev:pdbvt->m_root); + } + else + { + pdbvt->m_root=sibling; + sibling->parent=0; + deletenode(pdbvt,parent); + return(pdbvt->m_root); + } + } +} + +// +static void fetchleaves(btDbvt* pdbvt, + btDbvtNode* root, + tNodeArray& leaves, + int depth=-1) +{ +if(root->isinternal()&&depth) + { + fetchleaves(pdbvt,root->childs[0],leaves,depth-1); + fetchleaves(pdbvt,root->childs[1],leaves,depth-1); + deletenode(pdbvt,root); + } + else + { + leaves.push_back(root); + } +} + +// +static void split( const tNodeArray& leaves, + tNodeArray& left, + tNodeArray& right, + const btVector3& org, + const btVector3& axis) +{ +left.resize(0); +right.resize(0); +for(int i=0,ni=leaves.size();ivolume.Center()-org)<0) + left.push_back(leaves[i]); + else + right.push_back(leaves[i]); + } +} + +// +static btDbvtVolume bounds( const tNodeArray& leaves) +{ +#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE +DBVT_ALIGN char locals[sizeof(btDbvtVolume)]; +btDbvtVolume& volume=*(btDbvtVolume*)locals; +volume=leaves[0]->volume; +#else +btDbvtVolume volume=leaves[0]->volume; +#endif +for(int i=1,ni=leaves.size();ivolume,volume); + } +return(volume); +} + +// +static void bottomup( btDbvt* pdbvt, + tNodeArray& leaves) +{ +while(leaves.size()>1) + { + btScalar minsize=SIMD_INFINITY; + int minidx[2]={-1,-1}; + for(int i=0;ivolume,leaves[j]->volume)); + if(szvolume,n[1]->volume,0); + p->childs[0] = n[0]; + p->childs[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + leaves[minidx[0]] = p; + leaves.swap(minidx[1],leaves.size()-1); + leaves.pop_back(); + } +} + +// +static btDbvtNode* topdown(btDbvt* pdbvt, + tNodeArray& leaves, + int bu_treshold) +{ +static const btVector3 axis[]={btVector3(1,0,0), + btVector3(0,1,0), + btVector3(0,0,1)}; +if(leaves.size()>1) + { + if(leaves.size()>bu_treshold) + { + const btDbvtVolume vol=bounds(leaves); + const btVector3 org=vol.Center(); + tNodeArray sets[2]; + int bestaxis=-1; + int bestmidp=leaves.size(); + int splitcount[3][2]={{0,0},{0,0},{0,0}}; + int i; + for( i=0;ivolume.Center()-org; + for(int j=0;j<3;++j) + { + ++splitcount[j][dot(x,axis[j])>0?1:0]; + } + } + for( i=0;i<3;++i) + { + if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) + { + const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1])); + if(midp=0) + { + sets[0].reserve(splitcount[bestaxis][0]); + sets[1].reserve(splitcount[bestaxis][1]); + split(leaves,sets[0],sets[1],org,axis[bestaxis]); + } + else + { + sets[0].reserve(leaves.size()/2+1); + sets[1].reserve(leaves.size()/2); + for(int i=0,ni=leaves.size();ichilds[0]=topdown(pdbvt,sets[0],bu_treshold); + node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); + node->childs[0]->parent=node; + node->childs[1]->parent=node; + return(node); + } + else + { + bottomup(pdbvt,leaves); + return(leaves[0]); + } + } +return(leaves[0]); +} + +// +static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r) +{ +btDbvtNode* p=n->parent; +btAssert(n->isinternal()); +if(p>n) + { + const int i=indexof(n); + const int j=1-i; + btDbvtNode* s=p->childs[j]; + btDbvtNode* q=p->parent; + btAssert(n==p->childs[i]); + if(q) q->childs[indexof(p)]=n; else r=n; + s->parent=n; + p->parent=n; + n->parent=q; + p->childs[0]=n->childs[0]; + p->childs[1]=n->childs[1]; + n->childs[0]->parent=p; + n->childs[1]->parent=p; + n->childs[i]=p; + n->childs[j]=s; + btSwap(p->volume,n->volume); + return(p); + } +return(n); +} + +// +static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count) +{ +while(n&&(count--)) n=n->parent; +return(n); +} + +// +// Api +// + +// + btDbvt::btDbvt() +{ +m_root = 0; +m_free = 0; +m_lkhd = -1; +m_leaves = 0; +m_opath = 0; +} + +// + btDbvt::~btDbvt() +{ +clear(); +} + +// +void btDbvt::clear() +{ +if(m_root) recursedeletenode(this,m_root); +btAlignedFree(m_free); +m_free=0; +} + +// +void btDbvt::optimizeBottomUp() +{ +if(m_root) + { + tNodeArray leaves; + leaves.reserve(m_leaves); + fetchleaves(this,m_root,leaves); + bottomup(this,leaves); + m_root=leaves[0]; + } +} + +// +void btDbvt::optimizeTopDown(int bu_treshold) +{ +if(m_root) + { + tNodeArray leaves; + leaves.reserve(m_leaves); + fetchleaves(this,m_root,leaves); + m_root=topdown(this,leaves,bu_treshold); + } +} + +// +void btDbvt::optimizeIncremental(int passes) +{ +if(passes<0) passes=m_leaves; +if(m_root&&(passes>0)) + { + do { + btDbvtNode* node=m_root; + unsigned bit=0; + while(node->isinternal()) + { + node=sort(node,m_root)->childs[(m_opath>>bit)&1]; + bit=(bit+1)&(sizeof(unsigned)*8-1); + } + update(node); + ++m_opath; + } while(--passes); + } +} + +// +btDbvtNode* btDbvt::insert(const btDbvtVolume& volume,void* data) +{ +btDbvtNode* leaf=createnode(this,0,volume,data); +insertleaf(this,m_root,leaf); +++m_leaves; +return(leaf); +} + +// +void btDbvt::update(btDbvtNode* leaf,int lookahead) +{ +btDbvtNode* root=removeleaf(this,leaf); +if(root) + { + if(lookahead>=0) + { + for(int i=0;(iparent;++i) + { + root=root->parent; + } + } else root=m_root; + } +insertleaf(this,root,leaf); +} + +// +void btDbvt::update(btDbvtNode* leaf,const btDbvtVolume& volume) +{ +btDbvtNode* root=removeleaf(this,leaf); +if(root) + { + if(m_lkhd>=0) + { + for(int i=0;(iparent;++i) + { + root=root->parent; + } + } else root=m_root; + } +leaf->volume=volume; +insertleaf(this,root,leaf); +} + +// +bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin) +{ +if(leaf->volume.Contain(volume)) return(false); +volume.Expand(btVector3(margin,margin,margin)); +volume.SignedExpand(velocity); +update(leaf,volume); +return(true); +} + +// +bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity) +{ +if(leaf->volume.Contain(volume)) return(false); +volume.SignedExpand(velocity); +update(leaf,volume); +return(true); +} + +// +bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin) +{ +if(leaf->volume.Contain(volume)) return(false); +volume.Expand(btVector3(margin,margin,margin)); +update(leaf,volume); +return(true); +} + +// +void btDbvt::remove(btDbvtNode* leaf) +{ +removeleaf(this,leaf); +deletenode(this,leaf); +--m_leaves; +} + +// +void btDbvt::write(IWriter* iwriter) const +{ +btDbvtNodeEnumerator nodes; +nodes.nodes.reserve(m_leaves*2); +enumNodes(m_root,nodes); +iwriter->Prepare(m_root,nodes.nodes.size()); +for(int i=0;iparent) p=nodes.nodes.findLinearSearch(n->parent); + if(n->isinternal()) + { + const int c0=nodes.nodes.findLinearSearch(n->childs[0]); + const int c1=nodes.nodes.findLinearSearch(n->childs[1]); + iwriter->WriteNode(n,i,p,c0,c1); + } + else + { + iwriter->WriteLeaf(n,i,p); + } + } +} + +// +void btDbvt::clone(btDbvt& dest,IClone* iclone) const +{ +dest.clear(); +if(m_root!=0) + { + btAlignedObjectArray stack; + stack.reserve(m_leaves); + stack.push_back(sStkCLN(m_root,0)); + do { + const int i=stack.size()-1; + const sStkCLN e=stack[i]; + btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data); + stack.pop_back(); + if(e.parent!=0) + e.parent->childs[i&1]=n; + else + dest.m_root=n; + if(e.node->isinternal()) + { + stack.push_back(sStkCLN(e.node->childs[0],n)); + stack.push_back(sStkCLN(e.node->childs[1],n)); + } + else + { + iclone->CloneLeaf(n); + } + } while(stack.size()>0); + } +} + +// +int btDbvt::maxdepth(const btDbvtNode* node) +{ +int depth=0; +if(node) getmaxdepth(node,1,depth); +return(depth); +} + +// +int btDbvt::countLeaves(const btDbvtNode* node) +{ +if(node->isinternal()) + return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); + else + return(1); +} + +// +void btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves) +{ +if(node->isinternal()) + { + extractLeaves(node->childs[0],leaves); + extractLeaves(node->childs[1],leaves); + } + else + { + leaves.push_back(node); + } +} + +// +#if DBVT_ENABLE_BENCHMARK + +#include +#include +#include "LinearMath/btQuickProf.h" + +/* +q6600,2.4ghz + +/Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32" +/GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch" +/Fo"..\..\out\release8\build\libbulletcollision\\" +/Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb" +/W3 /nologo /c /Wp64 /Zi /errorReport:prompt + +Benchmarking dbvt... + World scale: 100.000000 + Extents base: 1.000000 + Extents range: 4.000000 + Leaves: 8192 + sizeof(btDbvtVolume): 32 bytes + sizeof(btDbvtNode): 44 bytes +[1] btDbvtVolume intersections: 3499 ms (-1%) +[2] btDbvtVolume merges: 1934 ms (0%) +[3] btDbvt::collideTT: 5485 ms (-21%) +[4] btDbvt::collideTT self: 2814 ms (-20%) +[5] btDbvt::collideTT xform: 7379 ms (-1%) +[6] btDbvt::collideTT xform,self: 7270 ms (-2%) +[7] btDbvt::collideRAY: 6314 ms (0%),(332143 r/s) +[8] insert/remove: 2093 ms (0%),(1001983 ir/s) +[9] updates (teleport): 1879 ms (-3%),(1116100 u/s) +[10] updates (jitter): 1244 ms (-4%),(1685813 u/s) +[11] optimize (incremental): 2514 ms (0%),(1668000 o/s) +[12] btDbvtVolume notequal: 3659 ms (0%) +[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s) +[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s) +[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s) +[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s) +[17] btDbvtVolume select: 3419 ms (0%) +*/ + +struct btDbvtBenchmark +{ +struct NilPolicy : btDbvt::ICollide + { + NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {} + void Process(const btDbvtNode*,const btDbvtNode*) { ++m_pcount; } + void Process(const btDbvtNode*) { ++m_pcount; } + void Process(const btDbvtNode*,btScalar depth) + { + ++m_pcount; + if(m_checksort) + { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); } + } + int m_pcount; + btScalar m_depth; + bool m_checksort; + }; +struct P14 : btDbvt::ICollide + { + struct Node + { + const btDbvtNode* leaf; + btScalar depth; + }; + void Process(const btDbvtNode* leaf,btScalar depth) + { + Node n; + n.leaf = leaf; + n.depth = depth; + } + static int sortfnc(const Node& a,const Node& b) + { + if(a.depthb.depth) return(-1); + return(0); + } + btAlignedObjectArray m_nodes; + }; +struct P15 : btDbvt::ICollide + { + struct Node + { + const btDbvtNode* leaf; + btScalar depth; + }; + void Process(const btDbvtNode* leaf) + { + Node n; + n.leaf = leaf; + n.depth = dot(leaf->volume.Center(),m_axis); + } + static int sortfnc(const Node& a,const Node& b) + { + if(a.depthb.depth) return(-1); + return(0); + } + btAlignedObjectArray m_nodes; + btVector3 m_axis; + }; +static btScalar RandUnit() + { + return(rand()/(btScalar)RAND_MAX); + } +static btVector3 RandVector3() + { + return(btVector3(RandUnit(),RandUnit(),RandUnit())); + } +static btVector3 RandVector3(btScalar cs) + { + return(RandVector3()*cs-btVector3(cs,cs,cs)/2); + } +static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es) + { + return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es)); + } +static btTransform RandTransform(btScalar cs) + { + btTransform t; + t.setOrigin(RandVector3(cs)); + t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized()); + return(t); + } +static void RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt) + { + dbvt.clear(); + for(int i=0;i volumes; + btAlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i volumes; + btAlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark5_Iterations); + for(int i=0;i transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark6_Iterations); + for(int i=0;i rayorg; + btAlignedObjectArray raydir; + btDbvtBenchmark::NilPolicy policy; + rayorg.resize(cfgBenchmark7_Iterations); + raydir.resize(cfgBenchmark7_Iterations); + for(int i=0;i leaves; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + dbvt.extractLeaves(dbvt.m_root,leaves); + printf("[9] updates (teleport): "); + wallclock.reset(); + for(int i=0;i(leaves[rand()%cfgLeaves]), + btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale)); + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations; + printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time); + } +if(cfgBenchmark10_Enable) + {// Benchmark 10 + srand(380843); + btDbvt dbvt; + btAlignedObjectArray leaves; + btAlignedObjectArray vectors; + vectors.resize(cfgBenchmark10_Iterations); + for(int i=0;i(leaves[rand()%cfgLeaves]); + btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d); + dbvt.update(l,v); + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations; + printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time); + } +if(cfgBenchmark11_Enable) + {// Benchmark 11 + srand(380843); + btDbvt dbvt; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[11] optimize (incremental): "); + wallclock.reset(); + for(int i=0;i volumes; + btAlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i vectors; + btDbvtBenchmark::NilPolicy policy; + vectors.resize(cfgBenchmark13_Iterations); + for(int i=0;i vectors; + btDbvtBenchmark::P14 policy; + vectors.resize(cfgBenchmark14_Iterations); + for(int i=0;i vectors; + btDbvtBenchmark::P15 policy; + vectors.resize(cfgBenchmark15_Iterations); + for(int i=0;i batch; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + batch.reserve(cfgBenchmark16_BatchCount); + printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); + wallclock.reset(); + for(int i=0;i volumes; + btAlignedObjectArray results; + btAlignedObjectArray indices; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + indices.resize(cfgLeaves); + for(int i=0;i= 1400) + #define DBVT_USE_TEMPLATE 1 + #else + #define DBVT_USE_TEMPLATE 0 +#endif +#else +#define DBVT_USE_TEMPLATE 0 +#endif + +// Use only intrinsics instead of inline asm +#define DBVT_USE_INTRINSIC_SSE 1 + +// Using memmov for collideOCL +#define DBVT_USE_MEMMOVE 1 + +// Enable benchmarking code +#define DBVT_ENABLE_BENCHMARK 0 + +// Inlining +#define DBVT_INLINE SIMD_FORCE_INLINE +// Align +#ifdef WIN32 +#define DBVT_ALIGN __declspec(align(16)) +#else +#define DBVT_ALIGN +#endif + +// Specific methods implementation + +#ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds +#define DBVT_SELECT_IMPL DBVT_IMPL_SSE +#define DBVT_MERGE_IMPL DBVT_IMPL_SSE +#define DBVT_INT0_IMPL DBVT_IMPL_SSE +#else +#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC +#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC +#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC +#endif + +#if (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)|| \ + (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)|| \ + (DBVT_INT0_IMPL==DBVT_IMPL_SSE) +#include +#endif + +// +// Auto config and checks +// + +#if DBVT_USE_TEMPLATE +#define DBVT_VIRTUAL +#define DBVT_VIRTUAL_DTOR(a) +#define DBVT_PREFIX template +#define DBVT_IPOLICY T& policy +#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)0; +#else +#define DBVT_VIRTUAL_DTOR(a) virtual ~a() {} +#define DBVT_VIRTUAL virtual +#define DBVT_PREFIX +#define DBVT_IPOLICY ICollide& policy +#define DBVT_CHECKTYPE +#endif + +#if DBVT_USE_MEMMOVE +#ifndef __CELLOS_LV2__ +#include +#endif +#include +#endif + +#ifndef DBVT_USE_TEMPLATE +#error "DBVT_USE_TEMPLATE undefined" +#endif + +#ifndef DBVT_USE_MEMMOVE +#error "DBVT_USE_MEMMOVE undefined" +#endif + +#ifndef DBVT_ENABLE_BENCHMARK +#error "DBVT_ENABLE_BENCHMARK undefined" +#endif + +#ifndef DBVT_SELECT_IMPL +#error "DBVT_SELECT_IMPL undefined" +#endif + +#ifndef DBVT_MERGE_IMPL +#error "DBVT_MERGE_IMPL undefined" +#endif + +#ifndef DBVT_INT0_IMPL +#error "DBVT_INT0_IMPL undefined" +#endif + +// +// Defaults volumes +// + +/* btDbvtAabbMm */ +struct btDbvtAabbMm +{ +DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } +DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } +DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } +DBVT_INLINE const btVector3& Mins() const { return(mi); } +DBVT_INLINE const btVector3& Maxs() const { return(mx); } +static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); +static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); +static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); +static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); +static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); +DBVT_INLINE void Expand(const btVector3& e); +DBVT_INLINE void SignedExpand(const btVector3& e); +DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; +DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; +DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; +DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); +DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + const btTransform& xform); +DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btVector3& b); +DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btVector3& org, + const btVector3& invdir, + const unsigned* signs); +DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); +DBVT_INLINE friend int Select( const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b); +DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + btDbvtAabbMm& r); +DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); +private: +DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; +private: +btVector3 mi,mx; +}; + +// Types +typedef btDbvtAabbMm btDbvtVolume; + +/* btDbvtNode */ +struct btDbvtNode +{ + btDbvtVolume volume; + btDbvtNode* parent; + DBVT_INLINE bool isleaf() const { return(childs[1]==0); } + DBVT_INLINE bool isinternal() const { return(!isleaf()); } + union { + btDbvtNode* childs[2]; + void* data; + int dataAsInt; + }; +}; + +///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). +///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes. +///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. +struct btDbvt + { + /* Stack element */ + struct sStkNN + { + const btDbvtNode* a; + const btDbvtNode* b; + sStkNN() {} + sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} + }; + struct sStkNP + { + const btDbvtNode* node; + int mask; + sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} + }; + struct sStkNPS + { + const btDbvtNode* node; + int mask; + btScalar value; + sStkNPS() {} + sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} + }; + struct sStkCLN + { + const btDbvtNode* node; + btDbvtNode* parent; + sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} + }; + // Policies/Interfaces + + /* ICollide */ + struct ICollide + { + DBVT_VIRTUAL_DTOR(ICollide) + DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); } + DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); } + DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); } + }; + /* IWriter */ + struct IWriter + { + virtual ~IWriter() {} + virtual void Prepare(const btDbvtNode* root,int numnodes)=0; + virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0; + virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0; + }; + /* IClone */ + struct IClone + { + virtual ~IClone() {} + virtual void CloneLeaf(btDbvtNode*) {} + }; + + // Constants + enum { + SIMPLE_STACKSIZE = 64, + DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 + }; + + // Fields + btDbvtNode* m_root; + btDbvtNode* m_free; + int m_lkhd; + int m_leaves; + unsigned m_opath; + // Methods + btDbvt(); + ~btDbvt(); + void clear(); + bool empty() const { return(0==m_root); } + void optimizeBottomUp(); + void optimizeTopDown(int bu_treshold=128); + void optimizeIncremental(int passes); + btDbvtNode* insert(const btDbvtVolume& box,void* data); + void update(btDbvtNode* leaf,int lookahead=-1); + void update(btDbvtNode* leaf,const btDbvtVolume& volume); + bool update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin); + bool update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity); + bool update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin); + void remove(btDbvtNode* leaf); + void write(IWriter* iwriter) const; + void clone(btDbvt& dest,IClone* iclone=0) const; + static int maxdepth(const btDbvtNode* node); + static int countLeaves(const btDbvtNode* node); + static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves); + #if DBVT_ENABLE_BENCHMARK + static void benchmark(); + #else + static void benchmark(){} + #endif + // DBVT_IPOLICY must support ICollide policy/interface + DBVT_PREFIX + static void enumNodes( const btDbvtNode* root, + DBVT_IPOLICY); + DBVT_PREFIX + static void enumLeaves( const btDbvtNode* root, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideTT( const btDbvtNode* root0, + const btTransform& xform0, + const btDbvtNode* root1, + const btTransform& xform1, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideTV( const btDbvtNode* root, + const btDbvtVolume& volume, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideRAY( const btDbvtNode* root, + const btVector3& origin, + const btVector3& direction, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideOCL( const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fullsort=true); + DBVT_PREFIX + static void collideTU( const btDbvtNode* root, + DBVT_IPOLICY); + // Helpers + static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h) + { + int m=0; + while(l>1; + if(a[i[m]].value>=v) l=m+1; else h=m; + } + return(h); + } + static DBVT_INLINE int allocate( btAlignedObjectArray& ifree, + btAlignedObjectArray& 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: + btDbvt(const btDbvt&) {} + }; + +// +// Inline's +// + +// +inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) +{ +btDbvtAabbMm box; +box.mi=c-e;box.mx=c+e; +return(box); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) +{ +return(FromCE(c,btVector3(r,r,r))); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) +{ +btDbvtAabbMm box; +box.mi=mi;box.mx=mx; +return(box); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) +{ +btDbvtAabbMm box; +box.mi=box.mx=pts[0]; +for(int i=1;i0) 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]); +} + +// +DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& 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())); +} + +// +DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const +{ +btVector3 pi,px; +switch(s) + { + case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z()); + pi=btVector3(mx.x(),mx.y(),mx.z());break; + case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z()); + pi=btVector3(mi.x(),mx.y(),mx.z());break; + case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z()); + pi=btVector3(mx.x(),mi.y(),mx.z());break; + case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z()); + pi=btVector3(mi.x(),mi.y(),mx.z());break; + case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z()); + pi=btVector3(mx.x(),mx.y(),mi.z());break; + case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z()); + pi=btVector3(mi.x(),mx.y(),mi.z());break; + case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z()); + pi=btVector3(mx.x(),mi.y(),mi.z());break; + case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z()); + pi=btVector3(mi.x(),mi.y(),mi.z());break; + } +if((dot(n,px)+o)<0) return(-1); +if((dot(n,pi)+o)>=0) return(+1); +return(0); +} + +// +DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const +{ +const btVector3* b[]={&mx,&mi}; +const btVector3 p( b[(signs>>0)&1]->x(), + b[(signs>>1)&1]->y(), + b[(signs>>2)&1]->z()); +return(dot(p,v)); +} + +// +DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& 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]; } + } +} + +// +DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +#if DBVT_INT0_IMPL == 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)))); +const __int32* pu((const __int32*)&rt); +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 +} + +// +DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + const btTransform& xform) +{ +const btVector3 d0=xform*b.Center()-a.Center(); +const btVector3 d1=d0*xform.getBasis(); +btScalar s0[2]={0,0}; +btScalar s1[2]={dot(xform.getOrigin(),d0),s1[0]}; +a.AddSpan(d0,s0[0],s0[1]); +b.AddSpan(d1,s1[0],s1[1]); +if(s0[0]>(s1[1])) return(false); +if(s0[1]<(s1[0])) return(false); +return(true); +} + +// +DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, + const btVector3& 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())); +} + +// +DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, + const btVector3& org, + const btVector3& invdir, + const unsigned* signs) +{ +#if 0 +const btVector3 b0((a.mi-org)*invdir); +const btVector3 b1((a.mx-org)*invdir); +const btVector3 tmin(btMin(b0[0],b1[0]),btMin(b0[1],b1[1]),btMin(b0[2],b1[2])); +const btVector3 tmax(btMax(b0[0],b1[0]),btMax(b0[1],b1[1]),btMax(b0[2],b1[2])); +const btScalar tin=btMax(tmin[0],btMax(tmin[1],tmin[2])); +const btScalar tout=btMin(tmax[0],btMin(tmax[1],tmax[2])); +return(tinx()-org[0])*invdir[0]; +btScalar txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0]; +const btScalar tymin=(bounds[ signs[1]]->y()-org[1])*invdir[1]; +const btScalar tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1]; +if((txmin>tymax)||(tymin>txmax)) return(false); +if(tymin>txmin) txmin=tymin; +if(tymaxz()-org[2])*invdir[2]; +const btScalar tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2]; +if((txmin>tzmax)||(tzmin>txmax)) return(false); +if(tzmin>txmin) txmin=tzmin; +if(tzmax0); +#endif +} + +// +DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); +return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); +} + +// +DBVT_INLINE int Select( const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE +static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; + // TODO: the intrinsic version is 11% slower + #if DBVT_USE_INTRINSIC_SSE + __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)); + return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1); + #else + DBVT_ALIGN __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(Proximity(o,a)b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; + } +#endif +} + +// +DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, + const btDbvtAabbMm& 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 +// + +// +DBVT_PREFIX +inline void btDbvt::enumNodes( const btDbvtNode* root, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +policy.Process(root); +if(root->isinternal()) + { + enumNodes(root->childs[0],policy); + enumNodes(root->childs[1],policy); + } +} + +// +DBVT_PREFIX +inline void btDbvt::enumLeaves( const btDbvtNode* root, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root->isinternal()) + { + enumLeaves(root->childs[0],policy); + enumLeaves(root->childs[1],policy); + } + else + { + policy.Process(root); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root0&&root1) + { + btAlignedObjectArray stack; + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + stack.resize(DOUBLE_STACKSIZE); + stack[0]=sStkNN(root0,root1); + do { + sStkNN p=stack[--depth]; + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-4; + } + if(p.a==p.b) + { + if(p.a->isinternal()) + { + stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + } + } + else if(Intersect(p.a->volume,p.b->volume)) + { + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stack[depth++]=sStkNN(p.a->childs[0],p.b); + stack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stack[depth++]=sStkNN(p.a,p.b->childs[0]); + stack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root0&&root1) + { + btAlignedObjectArray stack; + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + stack.resize(DOUBLE_STACKSIZE); + stack[0]=sStkNN(root0,root1); + do { + sStkNN p=stack[--depth]; + if(Intersect(p.a->volume,p.b->volume,xform)) + { + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-4; + } + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stack[depth++]=sStkNN(p.a->childs[0],p.b); + stack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stack[depth++]=sStkNN(p.a,p.b->childs[0]); + stack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTT( const btDbvtNode* root0, + const btTransform& xform0, + const btDbvtNode* root1, + const btTransform& xform1, + DBVT_IPOLICY) +{ +const btTransform xform=xform0.inverse()*xform1; +collideTT(root0,root1,xform,policy); +} + +// +DBVT_PREFIX +inline void btDbvt::collideTV( const btDbvtNode* root, + const btDbvtVolume& vol, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root) + { + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); + btAlignedObjectArray stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(Intersect(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); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideRAY( const btDbvtNode* root, + const btVector3& origin, + const btVector3& direction, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root) + { + const btVector3 normal=direction.normalized(); + const btVector3 invdir( 1/normal.x(), + 1/normal.y(), + 1/normal.z()); + const unsigned signs[]={ direction.x()<0, + direction.y()<0, + direction.z()<0}; + btAlignedObjectArray stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* node=stack[stack.size()-1]; + stack.pop_back(); + if(Intersect(node->volume,origin,invdir,signs)) + { + if(node->isinternal()) + { + stack.push_back(node->childs[0]); + stack.push_back(node->childs[1]); + } + else + { + policy.Process(node); + } + } + } while(stack.size()); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root) + { + const int inside=(1< stack; + int signs[sizeof(unsigned)*8]; + btAssert(count=0)?1:0)+ + ((normals[i].y()>=0)?2:0)+ + ((normals[i].z()>=0)?4:0); + } + stack.reserve(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)&&(ivolume.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()); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideOCL( const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fsort) +{ +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< stock; + btAlignedObjectArray ifree; + btAlignedObjectArray stack; + int signs[sizeof(unsigned)*8]; + btAssert(count=0)?1:0)+ + ((normals[i].y()>=0)?2:0)+ + ((normals[i].z()>=0)?4:0); + } + stock.reserve(SIMPLE_STACKSIZE); + stack.reserve(SIMPLE_STACKSIZE); + ifree.reserve(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)&&(ivolume.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 btDbvtNode* 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].value0)) + { + /* Insert 0 */ + j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); + stack.push_back(0); + #if 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 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()); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTU( const btDbvtNode* root, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root) + { + btAlignedObjectArray stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* 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 DBVT_USE_MEMMOVE +#undef DBVT_USE_TEMPLATE +#undef DBVT_VIRTUAL_DTOR +#undef DBVT_VIRTUAL +#undef DBVT_PREFIX +#undef DBVT_IPOLICY +#undef DBVT_CHECKTYPE +#undef DBVT_IMPL_GENERIC +#undef DBVT_IMPL_SSE +#undef DBVT_USE_INTRINSIC_SSE +#undef DBVT_SELECT_IMPL +#undef DBVT_MERGE_IMPL +#undef DBVT_INT0_IMPL + +#endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp new file mode 100644 index 00000000000..e00fc6aa5e3 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -0,0 +1,548 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btDbvtBroadphase implementation by Nathanael Presson + +#include "btDbvtBroadphase.h" + +// +// Profiling +// + +#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK +#include +#endif + +#if DBVT_BP_PROFILE +struct ProfileScope + { + __forceinline ProfileScope(btClock& clock,unsigned long& value) : + m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) + { + } + __forceinline ~ProfileScope() + { + (*m_value)+=m_clock->getTimeMicroseconds()-m_base; + } + btClock* m_clock; + unsigned long* m_value; + unsigned long m_base; + }; +#define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) +#else +#define SPC(_value_) +#endif + +// +// Helpers +// + +// +template +static inline void listappend(T* item,T*& list) +{ +item->links[0]=0; +item->links[1]=list; +if(list) list->links[0]=item; +list=item; +} + +// +template +static inline void listremove(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 +static inline int listcount(T* root) +{ +int n=0; +while(root) { ++n;root=root->links[1]; } +return(n); +} + +// +template +static inline void clear(T& value) +{ +static const struct ZeroDummy : T {} zerodummy; +value=zerodummy; +} + +// +// Colliders +// + +/* Tree collider */ +struct btDbvtTreeCollider : btDbvt::ICollide +{ +btDbvtBroadphase* pbp; +btDbvtProxy* proxy; + btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} +void Process(const btDbvtNode* na,const btDbvtNode* nb) + { + if(na!=nb) + { + btDbvtProxy* pa=(btDbvtProxy*)na->data; + btDbvtProxy* pb=(btDbvtProxy*)nb->data; + #if DBVT_BP_SORTPAIRS + if(pa>pb) btSwap(pa,pb); + #endif + pbp->m_paircache->addOverlappingPair(pa,pb); + ++pbp->m_newpairs; + } + } +void Process(const btDbvtNode* n) + { + Process(n,proxy->leaf); + } +}; + +// +// btDbvtBroadphase +// + +// +btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) +{ +m_deferedcollide = false; +m_needcleanup = true; +m_releasepaircache = (paircache!=0)?false:true; +m_prediction = 1/(btScalar)2; +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(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); +m_gid = 0; +m_pid = 0; +m_cid = 0; +for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } +#if DBVT_BP_PROFILE +clear(m_profiling); +#endif +} + +// +btDbvtBroadphase::~btDbvtBroadphase() +{ +if(m_releasepaircache) +{ + m_paircache->~btOverlappingPairCache(); + btAlignedFree(m_paircache); +} +} + +// +btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin, + const btVector3& aabbMax, + int /*shapeType*/, + void* userPtr, + short int collisionFilterGroup, + short int collisionFilterMask, + btDispatcher* /*dispatcher*/, + void* /*multiSapProxy*/) +{ +btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( userPtr, + collisionFilterGroup, + collisionFilterMask); +proxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); +proxy->stage = m_stageCurrent; +proxy->m_uniqueId = ++m_gid; +proxy->leaf = m_sets[0].insert(proxy->aabb,proxy); +listappend(proxy,m_stageRoots[m_stageCurrent]); +if(!m_deferedcollide) + { + btDbvtTreeCollider collider(this); + collider.proxy=proxy; + btDbvt::collideTV(m_sets[0].m_root,proxy->aabb,collider); + btDbvt::collideTV(m_sets[1].m_root,proxy->aabb,collider); + } +return(proxy); +} + +// +void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, + btDispatcher* dispatcher) +{ +btDbvtProxy* proxy=(btDbvtProxy*)absproxy; +if(proxy->stage==STAGECOUNT) + m_sets[1].remove(proxy->leaf); + else + m_sets[0].remove(proxy->leaf); +listremove(proxy,m_stageRoots[proxy->stage]); +m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); +btAlignedFree(proxy); +m_needcleanup=true; +} + +// +void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, + const btVector3& aabbMin, + const btVector3& aabbMax, + btDispatcher* /*dispatcher*/) +{ +btDbvtProxy* proxy=(btDbvtProxy*)absproxy; +ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); +#if DBVT_BP_PREVENTFALSEUPDATE +if(NotEqual(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(Intersect(proxy->leaf->volume,aabb)) + {/* Moving */ + const btVector3 delta=aabbMin-proxy->aabb.Mins(); + btVector3 velocity(aabb.Extents()*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 DBVT_BP_MARGIN + m_sets[0].update(proxy->leaf,aabb,velocity,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; + } + } + listremove(proxy,m_stageRoots[proxy->stage]); + proxy->aabb = aabb; + proxy->stage = m_stageCurrent; + listappend(proxy,m_stageRoots[m_stageCurrent]); + if(docollide) + { + m_needcleanup=true; + if(!m_deferedcollide) + { + btDbvtTreeCollider collider(this); + btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider); + btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider); + } + } + } +} + +// +void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) +{ +collide(dispatcher); +#if DBVT_BP_PROFILE +if(0==(m_pid%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/DBVT_BP_PROFILING_RATE); + printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); + printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); + printf("total: %uus\r\n",total/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)/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)*DBVT_BP_PROFILING_RATE)); + clear(m_profiling); + m_clock.reset(); + } +#endif +} + +// +void btDbvtBroadphase::collide(btDispatcher* dispatcher) +{ +SPC(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=btMax(0,m_fixedleft-count); + } +/* dynamic -> fixed set */ +m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; +btDbvtProxy* current=m_stageRoots[m_stageCurrent]; +if(current) + { + btDbvtTreeCollider collider(this); + do { + btDbvtProxy* next=current->links[1]; + listremove(current,m_stageRoots[current->stage]); + listappend(current,m_stageRoots[STAGECOUNT]); + #if DBVT_BP_ACCURATESLEEPING + m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); + collider.proxy=current; + btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); + btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); + #endif + m_sets[0].remove(current->leaf); + current->leaf = m_sets[1].insert(current->aabb,current); + current->stage = STAGECOUNT; + current = next; + } while(current); + m_fixedleft=m_sets[1].m_leaves; + m_needcleanup=true; + } +/* collide dynamics */ + { + btDbvtTreeCollider collider(this); + if(m_deferedcollide) + { + SPC(m_profiling.m_fdcollide); + btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider); + } + if(m_deferedcollide) + { + SPC(m_profiling.m_ddcollide); + btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider); + } + } +/* clean up */ +if(m_needcleanup) + { + SPC(m_profiling.m_cleanup); + btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); + if(pairs.size()>0) + { + const int ci=pairs.size(); + int ni=btMin(ci,btMax(m_newpairs,(ci*m_cupdates)/100)); + for(int i=0;ileaf->volume,pb->leaf->volume)) + { + #if DBVT_BP_SORTPAIRS + if(pa>pb) btSwap(pa,pb); + #endif + m_paircache->removeOverlappingPair(pa,pb,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/(btScalar)m_updates_call; } + else + { m_updates_ratio=0; } +m_updates_done/=2; +m_updates_call/=2; +} + +// +void btDbvtBroadphase::optimize() +{ +m_sets[0].optimizeTopDown(); +m_sets[1].optimizeTopDown(); +} + +// +btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() +{ +return(m_paircache); +} + +// +const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const +{ +return(m_paircache); +} + +// +void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const +{ + + ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds; + +if(!m_sets[0].empty()) + if(!m_sets[1].empty()) Merge( 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=btDbvtVolume::FromCR(btVector3(0,0,0),0); +aabbMin=bounds.Mins(); +aabbMax=bounds.Maxs(); +} + +// +void btDbvtBroadphase::printStats() +{} + +// +#if DBVT_BP_ENABLE_BENCHMARK + +struct btBroadphaseBenchmark + { + struct Experiment + { + const char* name; + int object_count; + int update_count; + int spawn_count; + int iterations; + btScalar speed; + btScalar amplitude; + }; + struct Object + { + btVector3 center; + btVector3 extents; + btBroadphaseProxy* proxy; + btScalar time; + void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi) + { + time += speed; + center[0] = btCos(time*(btScalar)2.17)*amplitude+ + btSin(time)*amplitude/2; + center[1] = btCos(time*(btScalar)1.38)*amplitude+ + btSin(time)*amplitude; + center[2] = btSin(time*(btScalar)0.777)*amplitude; + pbi->setAabb(proxy,center-extents,center+extents,0); + } + }; + static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } + static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); } + static void OutputTime(const char* name,btClock& c,unsigned count=0) + { + const unsigned long us=c.getTimeMicroseconds(); + const unsigned long ms=(us+500)/1000; + const btScalar sec=us/(btScalar)(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 btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) +{ +static const btBroadphaseBenchmark::Experiment experiments[]= + { + {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100}, + /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, + {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/ + }; +static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); +btAlignedObjectArray objects; +btClock wallclock; +/* Begin */ +for(int iexp=0;iexpcenter[0]=btBroadphaseBenchmark::UnitRand()*50; + po->center[1]=btBroadphaseBenchmark::UnitRand()*50; + po->center[2]=btBroadphaseBenchmark::UnitRand()*50; + po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; + po->time=btBroadphaseBenchmark::UnitRand()*2000; + po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); + objects.push_back(po); + } + btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); + /* First update */ + wallclock.reset(); + for(int i=0;iupdate(speed,amplitude,pbi); + } + btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); + /* Updates */ + wallclock.reset(); + for(int i=0;iupdate(speed,amplitude,pbi); + } + pbi->calculateOverlappingPairs(0); + } + btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); + /* Clean up */ + wallclock.reset(); + for(int i=0;idestroyProxy(objects[i]->proxy,0); + delete objects[i]; + } + objects.resize(0); + btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); + } + +} +#else +void btDbvtBroadphase::benchmark(btBroadphaseInterface*) +{} +#endif + +#if DBVT_BP_PROFILE +#undef SPC +#endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h new file mode 100644 index 00000000000..1f16043a7a8 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -0,0 +1,116 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btDbvtBroadphase implementation by Nathanael Presson +#ifndef BT_DBVT_BROADPHASE_H +#define BT_DBVT_BROADPHASE_H + +#include "BulletCollision/BroadphaseCollision/btDbvt.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" + +// +// Compile time config +// + +#define DBVT_BP_PROFILE 0 +#define DBVT_BP_SORTPAIRS 1 +#define DBVT_BP_PREVENTFALSEUPDATE 0 +#define DBVT_BP_ACCURATESLEEPING 0 +#define DBVT_BP_ENABLE_BENCHMARK 0 +#define DBVT_BP_MARGIN (btScalar)0.05 + +#if DBVT_BP_PROFILE + #define DBVT_BP_PROFILING_RATE 256 + #include "LinearMath/btQuickprof.h" +#endif + +// +// btDbvtProxy +// +struct btDbvtProxy : btBroadphaseProxy +{ +/* Fields */ +btDbvtAabbMm aabb; +btDbvtNode* leaf; +btDbvtProxy* links[2]; +int stage; +/* ctor */ +btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : + btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask) + { + links[0]=links[1]=0; + } +}; + +typedef btAlignedObjectArray btDbvtProxyArray; + +///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt). +///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 btAxisSweep3 and bt32BitAxisSweep3. +struct btDbvtBroadphase : btBroadphaseInterface +{ +/* Config */ +enum { + DYNAMIC_SET = 0, /* Dynamic set index */ + FIXED_SET = 1, /* Fixed set index */ + STAGECOUNT = 2 /* Number of stages */ + }; +/* Fields */ +btDbvt m_sets[2]; // Dbvt sets +btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list +btOverlappingPairCache* m_paircache; // Pair cache +btScalar 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 +btScalar m_updates_ratio; // m_updates_done/m_updates_call +int m_pid; // Parse id +int m_cid; // Cleanup index +int m_gid; // Gen id +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 DBVT_BP_PROFILE +btClock 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 */ +btDbvtBroadphase(btOverlappingPairCache* paircache=0); +~btDbvtBroadphase(); +void collide(btDispatcher* dispatcher); +void optimize(); +/* btBroadphaseInterface Implementation */ +btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); +void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); +void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); +void calculateOverlappingPairs(btDispatcher* dispatcher); +btOverlappingPairCache* getOverlappingPairCache(); +const btOverlappingPairCache* getOverlappingPairCache() const; +void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; +void printStats(); +static void benchmark(btBroadphaseInterface*); +}; + +#endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp new file mode 100644 index 00000000000..3f866ab7c5f --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp @@ -0,0 +1,466 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btMultiSapBroadphase.h" + +#include "btSimpleBroadphase.h" +#include "LinearMath/btAabbUtil2.h" +#include "btQuantizedBvh.h" + +/// btSapBroadphaseArray m_sapBroadphases; + +/// btOverlappingPairCache* m_overlappingPairs; +extern int gOverlappingPairs; + +/* +class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache +{ +public: + + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + { + return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy); + } +}; + +*/ + +btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache) +:m_overlappingPairs(pairCache), +m_optimizedAabbTree(0), +m_ownsPairCache(false), +m_invalidPair(0) +{ + if (!m_overlappingPairs) + { + m_ownsPairCache = true; + void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16); + m_overlappingPairs = new (mem)btSortedOverlappingPairCache(); + } + + struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback + { + virtual ~btMultiSapOverlapFilterCallback() + {} + // return true when pairs need collision + virtual bool needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const + { + btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy; + btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy; + + bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0; + collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask); + + return collides; + } + }; + + void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16); + m_filterCallback = new (mem)btMultiSapOverlapFilterCallback(); + + m_overlappingPairs->setOverlapFilterCallback(m_filterCallback); +// mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16); +// m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs); +} + +btMultiSapBroadphase::~btMultiSapBroadphase() +{ + if (m_ownsPairCache) + { + m_overlappingPairs->~btOverlappingPairCache(); + btAlignedFree(m_overlappingPairs); + } +} + + +void btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax) +{ + m_optimizedAabbTree = new btQuantizedBvh(); + m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax); + QuantizedNodeArray& nodes = m_optimizedAabbTree->getLeafNodeArray(); + for (int i=0;igetBroadphaseAabb(aabbMin,aabbMax); + m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); + m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); + int partId = 0; + node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i; + nodes.push_back(node); + } + m_optimizedAabbTree->buildInternal(); +} + +btBroadphaseProxy* btMultiSapBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/) +{ + //void* ignoreMe -> we could think of recursive multi-sap, if someone is interested + + void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16); + btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin, aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask); + m_multiSapProxies.push_back(proxy); + + ///this should deal with inserting/removal into child broadphases + setAabb(proxy,aabbMin,aabbMax,dispatcher); + return proxy; +} + +void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) +{ + ///not yet + btAssert(0); + +} + + +void btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase) +{ + void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16); + btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy; + bridgeProxyRef->m_childProxy = childProxy; + bridgeProxyRef->m_childBroadphase = childBroadphase; + parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef); +} + + +bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax); +bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax) +{ +return +amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() && +amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() && +amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ(); +} + + + + + + +//#include + +void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher) +{ + btMultiSapProxy* multiProxy = static_cast(proxy); + multiProxy->m_aabbMin = aabbMin; + multiProxy->m_aabbMax = aabbMax; + + +// bool fullyContained = false; +// bool alreadyInSimple = false; + + + + + struct MyNodeOverlapCallback : public btNodeOverlapCallback + { + btMultiSapBroadphase* m_multiSap; + btMultiSapProxy* m_multiProxy; + btDispatcher* m_dispatcher; + + MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher) + :m_multiSap(multiSap), + m_multiProxy(multiProxy), + m_dispatcher(dispatcher) + { + + } + + virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex) + { + btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex]; + + int containingBroadphaseIndex = -1; + //already found? + for (int i=0;im_bridgeProxies.size();i++) + { + + if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) + { + containingBroadphaseIndex = i; + break; + } + } + if (containingBroadphaseIndex<0) + { + //add it + btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy); + m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase); + + } + } + }; + + MyNodeOverlapCallback myNodeCallback(this,multiProxy,dispatcher); + + + + + m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); + int i; + + for ( i=0;im_bridgeProxies.size();i++) + { + btVector3 worldAabbMin,worldAabbMax; + multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax); + bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); + if (!overlapsBroadphase) + { + //remove it now + btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i]; + + btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; + bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); + + multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1); + multiProxy->m_bridgeProxies.pop_back(); + + } + } + + + /* + + if (1) + { + + //find broadphase that contain this multiProxy + int numChildBroadphases = getBroadphaseArray().size(); + for (int i=0;igetBroadphaseAabb(worldAabbMin,worldAabbMax); + bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); + + // fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); + int containingBroadphaseIndex = -1; + + //if already contains this + + for (int i=0;im_bridgeProxies.size();i++) + { + if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) + { + containingBroadphaseIndex = i; + } + alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase); + } + + if (overlapsBroadphase) + { + if (containingBroadphaseIndex<0) + { + btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); + childProxy->m_multiSapParentProxy = multiProxy; + addToChildBroadphase(multiProxy,childProxy,childBroadphase); + } + } else + { + if (containingBroadphaseIndex>=0) + { + //remove + btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex]; + + btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; + bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); + + multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1); + multiProxy->m_bridgeProxies.pop_back(); + } + } + } + + + ///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force) + ///hopefully we don't end up with many entries here (can assert/provide feedback on stats) + if (0)//!multiProxy->m_bridgeProxies.size()) + { + ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision + ///this is needed to be able to calculate the aabb overlap + btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); + childProxy->m_multiSapParentProxy = multiProxy; + addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); + } + } + + if (!multiProxy->m_bridgeProxies.size()) + { + ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision + ///this is needed to be able to calculate the aabb overlap + btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); + childProxy->m_multiSapParentProxy = multiProxy; + addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); + } +*/ + + + //update + for ( i=0;im_bridgeProxies.size();i++) + { + btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i]; + bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher); + } + +} +bool stopUpdating=false; + + + +class btMultiSapBroadphasePairSortPredicate +{ + public: + + bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) + { + btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0; + btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0; + btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0; + btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0; + + return aProxy0 > bProxy0 || + (aProxy0 == bProxy0 && aProxy1 > bProxy1) || + (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); + } +}; + + + ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb +void btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) +{ + +// m_simpleBroadphase->calculateOverlappingPairs(dispatcher); + + if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval()) + { + + btBroadphasePairArray& overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray(); + + // quicksort(overlappingPairArray,0,overlappingPairArray.size()); + + overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); + + //perform a sort, to find duplicates and to sort 'invalid' pairs to the end + // overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); + + overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; + + + int i; + + btBroadphasePair previousPair; + previousPair.m_pProxy0 = 0; + previousPair.m_pProxy1 = 0; + previousPair.m_algorithm = 0; + + + for (i=0;im_multiSapParentProxy : 0; + btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0; + btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0; + btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0; + + bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1); + + previousPair = pair; + + bool needsRemoval = false; + + if (!isDuplicate) + { + bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); + + if (hasOverlap) + { + needsRemoval = false;//callback->processOverlap(pair); + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + btAssert(!pair.m_algorithm); + } + + if (needsRemoval) + { + getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher); + + // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + // m_overlappingPairArray.pop_back(); + pair.m_pProxy0 = 0; + pair.m_pProxy1 = 0; + m_invalidPair++; + gOverlappingPairs--; + } + + } + + ///if you don't like to skip the invalid pairs in the array, execute following code: + #define CLEAN_INVALID_PAIRS 1 + #ifdef CLEAN_INVALID_PAIRS + + //perform a sort, to sort 'invalid' pairs to the end + //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); + overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); + + overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; + #endif//CLEAN_INVALID_PAIRS + + //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); + } + + +} + + +bool btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) +{ + btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy; + btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy; + + return TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax, + multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax); + +} + + +void btMultiSapBroadphase::printStats() +{ +/* printf("---------------------------------\n"); + + printf("btMultiSapBroadphase.h\n"); + printf("numHandles = %d\n",m_multiSapProxies.size()); + //find broadphase that contain this multiProxy + int numChildBroadphases = getBroadphaseArray().size(); + for (int i=0;iprintStats(); + + } + */ + +} diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h new file mode 100644 index 00000000000..a0c002de856 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h @@ -0,0 +1,144 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef BT_MULTI_SAP_BROADPHASE +#define BT_MULTI_SAP_BROADPHASE + +#include "btBroadphaseInterface.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btOverlappingPairCache.h" + + +class btBroadphaseInterface; +class btSimpleBroadphase; + + +typedef btAlignedObjectArray btSapBroadphaseArray; + +///The btMultiSapBroadphase is a broadphase that contains multiple SAP broadphases. +///The user can add SAP broadphases that cover the world. A btBroadphaseProxy can be in multiple child broadphases at the same time. +///A btQuantizedBvh acceleration structures finds overlapping SAPs for each btBroadphaseProxy. +///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328 +///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329 +class btMultiSapBroadphase :public btBroadphaseInterface +{ + btSapBroadphaseArray m_sapBroadphases; + + btSimpleBroadphase* m_simpleBroadphase; + + btOverlappingPairCache* m_overlappingPairs; + + class btQuantizedBvh* m_optimizedAabbTree; + + + bool m_ownsPairCache; + + btOverlapFilterCallback* m_filterCallback; + + int m_invalidPair; + + struct btBridgeProxy + { + btBroadphaseProxy* m_childProxy; + btBroadphaseInterface* m_childBroadphase; + }; + + +public: + + struct btMultiSapProxy : public btBroadphaseProxy + { + + ///array with all the entries that this proxy belongs to + btAlignedObjectArray m_bridgeProxies; + btVector3 m_aabbMin; + btVector3 m_aabbMax; + + int m_shapeType; + +/* void* m_userPtr; + short int m_collisionFilterGroup; + short int m_collisionFilterMask; +*/ + btMultiSapProxy(const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) + :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax), + m_shapeType(shapeType) + { + m_multiSapParentProxy =this; + } + + + }; + +protected: + + + btAlignedObjectArray m_multiSapProxies; + +public: + + btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0); + + + btSapBroadphaseArray& getBroadphaseArray() + { + return m_sapBroadphases; + } + + const btSapBroadphaseArray& getBroadphaseArray() const + { + return m_sapBroadphases; + } + + virtual ~btMultiSapBroadphase(); + + virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy); + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher); + + void addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase); + + ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); + + bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + + virtual btOverlappingPairCache* getOverlappingPairCache() + { + return m_overlappingPairs; + } + virtual const btOverlappingPairCache* getOverlappingPairCache() const + { + return m_overlappingPairs; + } + + ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame + ///will add some transform later + virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const + { + aabbMin.setValue(-1e30f,-1e30f,-1e30f); + aabbMax.setValue(1e30f,1e30f,1e30f); + } + + void buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax); + + virtual void printStats(); + + void quicksort (btBroadphasePairArray& a, int lo, int hi); + +}; + +#endif //BT_MULTI_SAP_BROADPHASE diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h new file mode 100644 index 00000000000..9c7b6f81367 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h @@ -0,0 +1,40 @@ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef OVERLAPPING_PAIR_CALLBACK_H +#define OVERLAPPING_PAIR_CALLBACK_H + +class btDispatcher; +struct btBroadphasePair; + +///The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache. +class btOverlappingPairCallback +{ +public: + virtual ~btOverlappingPairCallback() + { + + } + + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) = 0; + + virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) = 0; + + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) = 0; + +}; + +#endif //OVERLAPPING_PAIR_CALLBACK_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp new file mode 100644 index 00000000000..a30bd1fd9e1 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -0,0 +1,1025 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btQuantizedBvh.h" + +#include "LinearMath/btAabbUtil2.h" +#include "LinearMath/btIDebugDraw.h" + + +btQuantizedBvh::btQuantizedBvh() : m_useQuantization(false), + //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) + m_traversalMode(TRAVERSAL_STACKLESS) + //m_traversalMode(TRAVERSAL_RECURSIVE) + ,m_subtreeHeaderCount(0) //PCK: add this line +{ + +} + + + + + +void btQuantizedBvh::buildInternal() +{ + ///assumes that caller filled in the m_quantizedLeafNodes + m_useQuantization = true; + int numLeafNodes = 0; + + if (m_useQuantization) + { + //now we have an array of leafnodes in m_leafNodes + numLeafNodes = m_quantizedLeafNodes.size(); + + m_quantizedContiguousNodes.resize(2*numLeafNodes); + + } + + m_curNodeIndex = 0; + + buildTree(0,numLeafNodes); + + ///if the entire tree is small then subtree size, we need to create a header info for the tree + if(m_useQuantization && !m_SubtreeHeaders.size()) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); + subtree.m_rootNodeIndex = 0; + subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex(); + } + + //PCK: update the copy of the size + m_subtreeHeaderCount = m_SubtreeHeaders.size(); + + //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary + m_quantizedLeafNodes.clear(); + m_leafNodes.clear(); +} + + + +///just for debugging, to visualize the individual patches/subtrees +#ifdef DEBUG_PATCH_COLORS +btVector3 color[4]= +{ + btVector3(255,0,0), + btVector3(0,255,0), + btVector3(0,0,255), + btVector3(0,255,255) +}; +#endif //DEBUG_PATCH_COLORS + + + +void btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin) +{ + //enlarge the AABB to avoid division by zero when initializing the quantization values + btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin); + m_bvhAabbMin = bvhAabbMin - clampValue; + m_bvhAabbMax = bvhAabbMax + clampValue; + btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin; + m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize; + m_useQuantization = true; +} + + + + +btQuantizedBvh::~btQuantizedBvh() +{ +} + +#ifdef DEBUG_TREE_BUILDING +int gStackDepth = 0; +int gMaxStackDepth = 0; +#endif //DEBUG_TREE_BUILDING + +void btQuantizedBvh::buildTree (int startIndex,int endIndex) +{ +#ifdef DEBUG_TREE_BUILDING + gStackDepth++; + if (gStackDepth > gMaxStackDepth) + gMaxStackDepth = gStackDepth; +#endif //DEBUG_TREE_BUILDING + + + int splitAxis, splitIndex, i; + int numIndices =endIndex-startIndex; + int curIndex = m_curNodeIndex; + + assert(numIndices>0); + + if (numIndices==1) + { +#ifdef DEBUG_TREE_BUILDING + gStackDepth--; +#endif //DEBUG_TREE_BUILDING + + assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex); + + m_curNodeIndex++; + return; + } + //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. + + splitAxis = calcSplittingAxis(startIndex,endIndex); + + splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis); + + int internalNodeIndex = m_curNodeIndex; + + setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin); + setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax); + + for (i=startIndex;im_escapeIndex; + + int leftChildNodexIndex = m_curNodeIndex; + + //build left child tree + buildTree(startIndex,splitIndex); + + int rightChildNodexIndex = m_curNodeIndex; + //build right child tree + buildTree(splitIndex,endIndex); + +#ifdef DEBUG_TREE_BUILDING + gStackDepth--; +#endif //DEBUG_TREE_BUILDING + + int escapeIndex = m_curNodeIndex - curIndex; + + if (m_useQuantization) + { + //escapeIndex is the number of nodes of this subtree + const int sizeQuantizedNode =sizeof(btQuantizedBvhNode); + const int treeSizeInBytes = escapeIndex * sizeQuantizedNode; + if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES) + { + updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex); + } + } + + setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex); + +} + +void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex) +{ + btAssert(m_useQuantization); + + btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex]; + int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex(); + int leftSubTreeSizeInBytes = leftSubTreeSize * static_cast(sizeof(btQuantizedBvhNode)); + + btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex]; + int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex(); + int rightSubTreeSizeInBytes = rightSubTreeSize * static_cast(sizeof(btQuantizedBvhNode)); + + if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(leftChildNode); + subtree.m_rootNodeIndex = leftChildNodexIndex; + subtree.m_subtreeSize = leftSubTreeSize; + } + + if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(rightChildNode); + subtree.m_rootNodeIndex = rightChildNodexIndex; + subtree.m_subtreeSize = rightSubTreeSize; + } + + //PCK: update the copy of the size + m_subtreeHeaderCount = m_SubtreeHeaders.size(); +} + + +int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis) +{ + int i; + int splitIndex =startIndex; + int numIndices = endIndex - startIndex; + btScalar splitValue; + + btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); + for (i=startIndex;i splitValue) + { + //swap + swapLeafNodes(i,splitIndex); + splitIndex++; + } + } + + //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex + //otherwise the tree-building might fail due to stack-overflows in certain cases. + //unbalanced1 is unsafe: it can cause stack overflows + //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); + + //unbalanced2 should work too: always use center (perfect balanced trees) + //bool unbalanced2 = true; + + //this should be safe too: + int rangeBalancedIndices = numIndices/3; + bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + + if (unbalanced) + { + splitIndex = startIndex+ (numIndices>>1); + } + + bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); + (void)unbal; + btAssert(!unbal); + + return splitIndex; +} + + +int btQuantizedBvh::calcSplittingAxis(int startIndex,int endIndex) +{ + int i; + + btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); + int numIndices = endIndex-startIndex; + + for (i=startIndex;im_aabbMinOrg,rootNode->m_aabbMaxOrg); + isLeafNode = rootNode->m_escapeIndex == -1; + + //PCK: unsigned instead of bool + if (isLeafNode && (aabbOverlap != 0)) + { + nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); + } + + //PCK: unsigned instead of bool + if ((aabbOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->m_escapeIndex; + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + +/* +///this was the original recursive traversal, before we optimized towards stackless traversal +void btQuantizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax); + if (aabbOverlap) + { + isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild); + if (isLeafNode) + { + nodeCallback->processNode(rootNode); + } else + { + walkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax); + walkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax); + } + } + +} +*/ + +void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +{ + btAssert(m_useQuantization); + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned aabbOverlap; + + //PCK: unsigned instead of bool + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax); + isLeafNode = currentNode->isLeafNode(); + + //PCK: unsigned instead of bool + if (aabbOverlap != 0) + { + if (isLeafNode) + { + nodeCallback->processNode(currentNode->getPartId(),currentNode->getTriangleIndex()); + } else + { + //process left and right children + const btQuantizedBvhNode* leftChildNode = currentNode+1; + walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + + const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex(); + walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + } + } +} + + + + + +void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const +{ + btAssert(m_useQuantization); + + int curIndex = startNodeIndex; + int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + (void)subTreeSize; + + const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; + int escapeIndex; + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned boxBoxOverlap = 0; + unsigned rayBoxOverlap = 0; + + btScalar lambda_max = 1.0; +#define RAYAABB2 +#ifdef RAYAABB2 + btVector3 rayFrom = raySource; + btVector3 rayDirection = (rayTarget-raySource); + rayDirection.normalize (); + lambda_max = rayDirection.dot(rayTarget-raySource); + ///what about division by zero? --> just set rayDirection[i] to 1.0 + rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[0]; + rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[1]; + rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[2]; + unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; +#endif + + /* Quick pruning by quantized box */ + btVector3 rayAabbMin = raySource; + btVector3 rayAabbMax = raySource; + rayAabbMin.setMin(rayTarget); + rayAabbMax.setMax(rayTarget); + + /* Add box cast extents to bounding box */ + rayAabbMin += aabbMin; + rayAabbMax += aabbMax; + + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin,0); + quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax,1); + + while (curIndex < endNodeIndex) + { + +//#define VISUALLY_ANALYZE_BVH 1 +#ifdef VISUALLY_ANALYZE_BVH + //some code snippet to debugDraw aabb, to visually analyze bvh structure + static int drawPatch = 0; + //need some global access to a debugDrawer + extern btIDebugDraw* debugDrawerPtr; + if (curIndex==drawPatch) + { + btVector3 aabbMin,aabbMax; + aabbMin = unQuantize(rootNode->m_quantizedAabbMin); + aabbMax = unQuantize(rootNode->m_quantizedAabbMax); + btVector3 color(1,0,0); + debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + } +#endif//VISUALLY_ANALYZE_BVH + + //catch bugs in tree data + assert (walkIterations < subTreeSize); + + walkIterations++; + //PCK: unsigned instead of bool + // only interested if this is closer than any previous hit + btScalar param = 1.0; + rayBoxOverlap = 0; + boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + isLeafNode = rootNode->isLeafNode(); + if (boxBoxOverlap) + { + btVector3 bounds[2]; + bounds[0] = unQuantize(rootNode->m_quantizedAabbMin); + bounds[1] = unQuantize(rootNode->m_quantizedAabbMax); + /* Add box cast extents */ + bounds[0] += aabbMin; + bounds[1] += aabbMax; + btVector3 normal; +#if 0 + bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); + bool ra = btRayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal); + if (ra2 != ra) + { + printf("functions don't match\n"); + } +#endif +#ifdef RAYAABB2 + ///careful with this check: need to check division by zero (above) and fix the unQuantize method + ///thanks Joerg/hiker for the reproduction case! + ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 + + rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max); +#else + rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); +#endif + } + + if (isLeafNode && rayBoxOverlap) + { + nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); + } + + //PCK: unsigned instead of bool + if ((rayBoxOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->getEscapeIndex(); + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + +void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const +{ + btAssert(m_useQuantization); + + int curIndex = startNodeIndex; + int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + (void)subTreeSize; + + const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; + int escapeIndex; + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned aabbOverlap; + + while (curIndex < endNodeIndex) + { + +//#define VISUALLY_ANALYZE_BVH 1 +#ifdef VISUALLY_ANALYZE_BVH + //some code snippet to debugDraw aabb, to visually analyze bvh structure + static int drawPatch = 0; + //need some global access to a debugDrawer + extern btIDebugDraw* debugDrawerPtr; + if (curIndex==drawPatch) + { + btVector3 aabbMin,aabbMax; + aabbMin = unQuantize(rootNode->m_quantizedAabbMin); + aabbMax = unQuantize(rootNode->m_quantizedAabbMax); + btVector3 color(1,0,0); + debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + } +#endif//VISUALLY_ANALYZE_BVH + + //catch bugs in tree data + assert (walkIterations < subTreeSize); + + walkIterations++; + //PCK: unsigned instead of bool + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + isLeafNode = rootNode->isLeafNode(); + + if (isLeafNode && aabbOverlap) + { + nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); + } + + //PCK: unsigned instead of bool + if ((aabbOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->getEscapeIndex(); + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + +//This traversal can be called from Playstation 3 SPU +void btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +{ + btAssert(m_useQuantization); + + int i; + + + for (i=0;im_SubtreeHeaders.size();i++) + { + const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; + + //PCK: unsigned instead of bool + unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap != 0) + { + walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, + subtree.m_rootNodeIndex, + subtree.m_rootNodeIndex+subtree.m_subtreeSize); + } + } +} + + +void btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const +{ + bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; + if (fast_path) + { + walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0), 0, m_curNodeIndex); + } else { + /* Otherwise fallback to AABB overlap test */ + btVector3 aabbMin = raySource; + btVector3 aabbMax = raySource; + aabbMin.setMin(rayTarget); + aabbMax.setMax(rayTarget); + reportAabbOverlappingNodex(nodeCallback,aabbMin,aabbMax); + } +} + + +void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const +{ + bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; + if (fast_path) + { + walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); + } else { + /* Slow path: + Construct the bounding box for the entire box cast and send that down the tree */ + btVector3 qaabbMin = raySource; + btVector3 qaabbMax = raySource; + qaabbMin.setMin(rayTarget); + qaabbMax.setMax(rayTarget); + qaabbMin += aabbMin; + qaabbMax += aabbMax; + reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax); + } +} + + +void btQuantizedBvh::swapLeafNodes(int i,int splitIndex) +{ + if (m_useQuantization) + { + btQuantizedBvhNode tmp = m_quantizedLeafNodes[i]; + m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex]; + m_quantizedLeafNodes[splitIndex] = tmp; + } else + { + btOptimizedBvhNode tmp = m_leafNodes[i]; + m_leafNodes[i] = m_leafNodes[splitIndex]; + m_leafNodes[splitIndex] = tmp; + } +} + +void btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex) +{ + if (m_useQuantization) + { + m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex]; + } else + { + m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex]; + } +} + +//PCK: include +#include + +//PCK: consts +static const unsigned BVH_ALIGNMENT = 16; +static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1; + +static const unsigned BVH_ALIGNMENT_BLOCKS = 2; + + + +unsigned int btQuantizedBvh::getAlignmentSerializationPadding() +{ + return BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; +} + +unsigned btQuantizedBvh::calculateSerializeBufferSize() +{ + unsigned baseSize = sizeof(btQuantizedBvh) + getAlignmentSerializationPadding(); + baseSize += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; + if (m_useQuantization) + { + return baseSize + m_curNodeIndex * sizeof(btQuantizedBvhNode); + } + return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode); +} + +bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) +{ + assert(m_subtreeHeaderCount == m_SubtreeHeaders.size()); + m_subtreeHeaderCount = m_SubtreeHeaders.size(); + +/* if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) + { + ///check alignedment for buffer? + btAssert(0); + return false; + } +*/ + + btQuantizedBvh *targetBvh = (btQuantizedBvh *)o_alignedDataBuffer; + + // construct the class so the virtual function table, etc will be set up + // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor + new (targetBvh) btQuantizedBvh; + + if (i_swapEndian) + { + targetBvh->m_curNodeIndex = static_cast(btSwapEndian(m_curNodeIndex)); + + + btSwapVector3Endian(m_bvhAabbMin,targetBvh->m_bvhAabbMin); + btSwapVector3Endian(m_bvhAabbMax,targetBvh->m_bvhAabbMax); + btSwapVector3Endian(m_bvhQuantization,targetBvh->m_bvhQuantization); + + targetBvh->m_traversalMode = (btTraversalMode)btSwapEndian(m_traversalMode); + targetBvh->m_subtreeHeaderCount = static_cast(btSwapEndian(m_subtreeHeaderCount)); + } + else + { + targetBvh->m_curNodeIndex = m_curNodeIndex; + targetBvh->m_bvhAabbMin = m_bvhAabbMin; + targetBvh->m_bvhAabbMax = m_bvhAabbMax; + targetBvh->m_bvhQuantization = m_bvhQuantization; + targetBvh->m_traversalMode = m_traversalMode; + targetBvh->m_subtreeHeaderCount = m_subtreeHeaderCount; + } + + targetBvh->m_useQuantization = m_useQuantization; + + unsigned char *nodeData = (unsigned char *)targetBvh; + nodeData += sizeof(btQuantizedBvh); + + unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + nodeData += sizeToAdd; + + int nodeCount = m_curNodeIndex; + + if (m_useQuantization) + { + targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]); + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]); + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast(btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex)); + } + } + else + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]; + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]; + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]; + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]; + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]; + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]; + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex; + + + } + } + nodeData += sizeof(btQuantizedBvhNode) * nodeCount; + } + else + { + targetBvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMinOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); + btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMaxOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); + + targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast(btSwapEndian(m_contiguousNodes[nodeIndex].m_escapeIndex)); + targetBvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast(btSwapEndian(m_contiguousNodes[nodeIndex].m_subPart)); + targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast(btSwapEndian(m_contiguousNodes[nodeIndex].m_triangleIndex)); + } + } + else + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg = m_contiguousNodes[nodeIndex].m_aabbMinOrg; + targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg = m_contiguousNodes[nodeIndex].m_aabbMaxOrg; + + targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = m_contiguousNodes[nodeIndex].m_escapeIndex; + targetBvh->m_contiguousNodes[nodeIndex].m_subPart = m_contiguousNodes[nodeIndex].m_subPart; + targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = m_contiguousNodes[nodeIndex].m_triangleIndex; + } + } + nodeData += sizeof(btOptimizedBvhNode) * nodeCount; + } + + sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + nodeData += sizeToAdd; + + // Now serialize the subtree headers + targetBvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, m_subtreeHeaderCount, m_subtreeHeaderCount); + if (i_swapEndian) + { + for (int i = 0; i < m_subtreeHeaderCount; i++) + { + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[2]); + + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[2]); + + targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast(btSwapEndian(m_SubtreeHeaders[i].m_rootNodeIndex)); + targetBvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast(btSwapEndian(m_SubtreeHeaders[i].m_subtreeSize)); + } + } + else + { + for (int i = 0; i < m_subtreeHeaderCount; i++) + { + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = (m_SubtreeHeaders[i].m_quantizedAabbMin[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = (m_SubtreeHeaders[i].m_quantizedAabbMin[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = (m_SubtreeHeaders[i].m_quantizedAabbMin[2]); + + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = (m_SubtreeHeaders[i].m_quantizedAabbMax[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = (m_SubtreeHeaders[i].m_quantizedAabbMax[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = (m_SubtreeHeaders[i].m_quantizedAabbMax[2]); + + targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = (m_SubtreeHeaders[i].m_rootNodeIndex); + targetBvh->m_SubtreeHeaders[i].m_subtreeSize = (m_SubtreeHeaders[i].m_subtreeSize); + targetBvh->m_SubtreeHeaders[i] = m_SubtreeHeaders[i]; + } + } + + nodeData += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; + + return true; +} + +btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) +{ + + if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) + { + return NULL; + } + btQuantizedBvh *bvh = (btQuantizedBvh *)i_alignedDataBuffer; + + if (i_swapEndian) + { + bvh->m_curNodeIndex = static_cast(btSwapEndian(bvh->m_curNodeIndex)); + + btUnSwapVector3Endian(bvh->m_bvhAabbMin); + btUnSwapVector3Endian(bvh->m_bvhAabbMax); + btUnSwapVector3Endian(bvh->m_bvhQuantization); + + bvh->m_traversalMode = (btTraversalMode)btSwapEndian(bvh->m_traversalMode); + bvh->m_subtreeHeaderCount = static_cast(btSwapEndian(bvh->m_subtreeHeaderCount)); + } + + unsigned int calculatedBufSize = bvh->calculateSerializeBufferSize(); + btAssert(calculatedBufSize <= i_dataBufferSize); + + if (calculatedBufSize > i_dataBufferSize) + { + return NULL; + } + + unsigned char *nodeData = (unsigned char *)bvh; + nodeData += sizeof(btQuantizedBvh); + + unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + nodeData += sizeToAdd; + + int nodeCount = bvh->m_curNodeIndex; + + // Must call placement new to fill in virtual function table, etc, but we don't want to overwrite most data, so call a special version of the constructor + // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor + new (bvh) btQuantizedBvh(*bvh, false); + + if (bvh->m_useQuantization) + { + bvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]); + + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]); + + bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast(btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex)); + } + } + nodeData += sizeof(btQuantizedBvhNode) * nodeCount; + } + else + { + bvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); + btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); + + bvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_escapeIndex)); + bvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_subPart)); + bvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_triangleIndex)); + } + } + nodeData += sizeof(btOptimizedBvhNode) * nodeCount; + } + + sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + nodeData += sizeToAdd; + + // Now serialize the subtree headers + bvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, bvh->m_subtreeHeaderCount, bvh->m_subtreeHeaderCount); + if (i_swapEndian) + { + for (int i = 0; i < bvh->m_subtreeHeaderCount; i++) + { + bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2]); + + bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2]); + + bvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast(btSwapEndian(bvh->m_SubtreeHeaders[i].m_rootNodeIndex)); + bvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast(btSwapEndian(bvh->m_SubtreeHeaders[i].m_subtreeSize)); + } + } + + return bvh; +} + +// Constructor that prevents btVector3's default constructor from being called +btQuantizedBvh::btQuantizedBvh(btQuantizedBvh &self, bool /* ownsMemory */) : +m_bvhAabbMin(self.m_bvhAabbMin), +m_bvhAabbMax(self.m_bvhAabbMax), +m_bvhQuantization(self.m_bvhQuantization) +{ + + +} + + + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h new file mode 100644 index 00000000000..8a149b533fa --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @@ -0,0 +1,486 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef QUANTIZED_BVH_H +#define QUANTIZED_BVH_H + +//#define DEBUG_CHECK_DEQUANTIZATION 1 +#ifdef DEBUG_CHECK_DEQUANTIZATION +#ifdef __SPU__ +#define printf spu_printf +#endif //__SPU__ + +#include +#include +#endif //DEBUG_CHECK_DEQUANTIZATION + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedAllocator.h" + + +//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp + + +//Note: currently we have 16 bytes per quantized node +#define MAX_SUBTREE_SIZE_IN_BYTES 2048 + +// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one +// actually) triangles each (since the sign bit is reserved +#define MAX_NUM_PARTS_IN_BITS 10 + +///btQuantizedBvhNode 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). +ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes + int m_escapeIndexOrTriangleIndex; + + bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrTriangleIndex >= 0); + } + int getEscapeIndex() const + { + btAssert(!isLeafNode()); + return -m_escapeIndexOrTriangleIndex; + } + int getTriangleIndex() const + { + btAssert(isLeafNode()); + // Get only the lower bits where the triangle index is stored + return (m_escapeIndexOrTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS))); + } + int getPartId() const + { + btAssert(isLeafNode()); + // Get only the highest bits where the part index is stored + return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); + } +} +; + +/// btOptimizedBvhNode contains both internal and leaf node information. +/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes. +ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + //32 bytes + btVector3 m_aabbMinOrg; + btVector3 m_aabbMaxOrg; + + //4 + int m_escapeIndex; + + //8 + //for child nodes + int m_subPart; + int m_triangleIndex; + int m_padding[5];//bad, due to alignment + + +}; + + +///btBvhSubtreeInfo provides info to gather a subtree of limited size +ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo +{ +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + //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]; + + btBvhSubtreeInfo() + { + //memset(&m_padding[0], 0, sizeof(m_padding)); + } + + + void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode) + { + m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0]; + m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1]; + m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2]; + m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0]; + m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1]; + m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2]; + } +} +; + + +class btNodeOverlapCallback +{ +public: + virtual ~btNodeOverlapCallback() {}; + + virtual void processNode(int subPart, int triangleIndex) = 0; +}; + +#include "LinearMath/btAlignedAllocator.h" +#include "LinearMath/btAlignedObjectArray.h" + + + +///for code readability: +typedef btAlignedObjectArray NodeArray; +typedef btAlignedObjectArray QuantizedNodeArray; +typedef btAlignedObjectArray BvhSubtreeInfoArray; + + +///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU. +///It is used by the btBvhTriangleMeshShape as midphase, and by the btMultiSapBroadphase. +///It is recommended to use quantization for better performance and lower memory requirements. +ATTRIBUTE_ALIGNED16(class) btQuantizedBvh +{ +protected: + + NodeArray m_leafNodes; + NodeArray m_contiguousNodes; + + QuantizedNodeArray m_quantizedLeafNodes; + + QuantizedNodeArray m_quantizedContiguousNodes; + + int m_curNodeIndex; + + + //quantization data + bool m_useQuantization; + btVector3 m_bvhAabbMin; + btVector3 m_bvhAabbMax; + btVector3 m_bvhQuantization; +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + enum btTraversalMode + { + TRAVERSAL_STACKLESS = 0, + TRAVERSAL_STACKLESS_CACHE_FRIENDLY, + TRAVERSAL_RECURSIVE + }; +protected: + + btTraversalMode m_traversalMode; + + BvhSubtreeInfoArray m_SubtreeHeaders; + + //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray + int m_subtreeHeaderCount; + + + ///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!) + ///this might be refactored into a virtual, it is usually not calculated at run-time + void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin) + { + if (m_useQuantization) + { + quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0); + } else + { + m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin; + + } + } + void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax) + { + if (m_useQuantization) + { + quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1); + } else + { + m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax; + } + } + + btVector3 getAabbMin(int nodeIndex) const + { + if (m_useQuantization) + { + return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]); + } + //non-quantized + return m_leafNodes[nodeIndex].m_aabbMinOrg; + + } + btVector3 getAabbMax(int nodeIndex) const + { + if (m_useQuantization) + { + return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]); + } + //non-quantized + return m_leafNodes[nodeIndex].m_aabbMaxOrg; + + } + + + void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) + { + if (m_useQuantization) + { + m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex; + } + else + { + m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex; + } + + } + + void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax) + { + if (m_useQuantization) + { + unsigned short int quantizedAabbMin[3]; + unsigned short int quantizedAabbMax[3]; + quantize(quantizedAabbMin,newAabbMin,0); + quantize(quantizedAabbMax,newAabbMax,1); + for (int i=0;i<3;i++) + { + if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i]) + m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i]; + + if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i]) + m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i]; + + } + } else + { + //non-quantized + m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin); + m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax); + } + } + + void swapLeafNodes(int firstIndex,int secondIndex); + + void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex); + +protected: + + + + void buildTree (int startIndex,int endIndex); + + int calcSplittingAxis(int startIndex,int endIndex); + + int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis); + + void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; + void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const; + + ///tree traversal designed for small-memory processors like PS3 SPU + void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; + + ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal + void walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; + + ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal + void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const; + + +#define USE_BANCHLESS 1 +#ifdef USE_BANCHLESS + //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360) + SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const + { + return static_cast(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) + & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) + & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), + 1, 0)); + } +#else + SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const + { + bool overlap = true; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + return overlap; + } +#endif //USE_BANCHLESS + + void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); + +public: + btQuantizedBvh(); + + virtual ~btQuantizedBvh(); + + + ///***************************************** expert/internal use only ************************* + void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0)); + QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; } + ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized + void buildInternal(); + ///***************************************** expert/internal use only ************************* + + void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const; + void reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const; + + SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const + { + + btAssert(m_useQuantization); + + btAssert(point.getX() <= m_bvhAabbMax.getX()); + btAssert(point.getY() <= m_bvhAabbMax.getY()); + btAssert(point.getZ() <= m_bvhAabbMax.getZ()); + + btAssert(point.getX() >= m_bvhAabbMin.getX()); + btAssert(point.getY() >= m_bvhAabbMin.getY()); + btAssert(point.getZ() >= m_bvhAabbMin.getZ()); + + btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization; + ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative + ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly) + ///todo: double-check this + if (isMax) + { + out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1)); + out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1)); + out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1)); + } else + { + out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe)); + out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe)); + out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe)); + } + + +#ifdef DEBUG_CHECK_DEQUANTIZATION + btVector3 newPoint = unQuantize(out); + if (isMax) + { + if (newPoint.getX() < point.getX()) + { + printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX()); + } + if (newPoint.getY() < point.getY()) + { + printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY()); + } + if (newPoint.getZ() < point.getZ()) + { + + printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ()); + } + } else + { + if (newPoint.getX() > point.getX()) + { + printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX()); + } + if (newPoint.getY() > point.getY()) + { + printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY()); + } + if (newPoint.getZ() > point.getZ()) + { + printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ()); + } + } +#endif //DEBUG_CHECK_DEQUANTIZATION + + } + + + SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const + { + + btAssert(m_useQuantization); + + btVector3 clampedPoint(point2); + clampedPoint.setMax(m_bvhAabbMin); + clampedPoint.setMin(m_bvhAabbMax); + + quantize(out,clampedPoint,isMax); + + } + + SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const + { + btVector3 vecOut; + vecOut.setValue( + (btScalar)(vecIn[0]) / (m_bvhQuantization.getX()), + (btScalar)(vecIn[1]) / (m_bvhQuantization.getY()), + (btScalar)(vecIn[2]) / (m_bvhQuantization.getZ())); + vecOut += m_bvhAabbMin; + return vecOut; + } + + ///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees. + void setTraversalMode(btTraversalMode traversalMode) + { + m_traversalMode = traversalMode; + } + + + SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray() + { + return m_quantizedContiguousNodes; + } + + + SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray() + { + return m_SubtreeHeaders; + } + + + /////Calculate space needed to store BVH for serialization + unsigned calculateSerializeBufferSize(); + + /// Data buffer MUST be 16 byte aligned + virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian); + + ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' + static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); + + static unsigned int getAlignmentSerializationPadding(); + + SIMD_FORCE_INLINE bool isQuantized() + { + return m_useQuantization; + } + +private: + // Special "copy" constructor that allows for in-place deserialization + // Prevents btVector3's default constructor from being called, but doesn't inialize much else + // ownsMemory should most likely be false if deserializing, and if you are not, don't call this (it also changes the function signature, which we need) + btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory); + +} +; + + +#endif //QUANTIZED_BVH_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp new file mode 100644 index 00000000000..cd0c028012c --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp @@ -0,0 +1,85 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btBoxBoxCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "btBoxBoxDetector.h" + +#define USE_PERSISTENT_CONTACTS 1 + +btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1) +: btCollisionAlgorithm(ci), +m_ownManifold(false), +m_manifoldPtr(mf) +{ + if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1); + m_ownManifold = true; + } +} + +btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + if (!m_manifoldPtr) + return; + + btCollisionObject* col0 = body0; + btCollisionObject* col1 = body1; + btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape(); + btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape(); + + + + /// report a contact. internally this will be kept persistent, and contact reduction is done + resultOut->setPersistentManifold(m_manifoldPtr); +#ifndef USE_PERSISTENT_CONTACTS + m_manifoldPtr->clearManifold(); +#endif //USE_PERSISTENT_CONTACTS + + btDiscreteCollisionDetectorInterface::ClosestPointInput input; + input.m_maximumDistanceSquared = 1e30f; + input.m_transformA = body0->getWorldTransform(); + input.m_transformB = body1->getWorldTransform(); + + btBoxBoxDetector detector(box0,box1); + detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + +#ifdef USE_PERSISTENT_CONTACTS + // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } +#endif //USE_PERSISTENT_CONTACTS + +} + +btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +{ + //not yet + return 1.f; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h new file mode 100644 index 00000000000..35afaf175a1 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BOX_BOX__COLLISION_ALGORITHM_H +#define BOX_BOX__COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" + +class btPersistentManifold; + +///box-box collision detection +class btBoxBoxCollisionAlgorithm : public btCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + +public: + btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btCollisionAlgorithm(ci) {} + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); + + virtual ~btBoxBoxCollisionAlgorithm(); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + int bbsize = sizeof(btBoxBoxCollisionAlgorithm); + void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); + return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0,body1); + } + }; + +}; + +#endif //BOX_BOX__COLLISION_ALGORITHM_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp new file mode 100644 index 00000000000..45ebff5dc45 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -0,0 +1,683 @@ + +/* + * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith + * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. + * All rights reserved. Email: russ@q12.org Web: www.q12.org + Bullet Continuous Collision Detection and Physics Library + Bullet is Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///ODE box-box collision detection is adapted to work with Bullet + +#include "btBoxBoxDetector.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" + +#include +#include + +btBoxBoxDetector::btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2) +: m_box1(box1), +m_box2(box2) +{ + +} + + +// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and +// generate contact points. this returns 0 if there is no contact otherwise +// it returns the number of contacts generated. +// `normal' returns the contact normal. +// `depth' returns the maximum penetration depth along that normal. +// `return_code' returns a number indicating the type of contact that was +// detected: +// 1,2,3 = box 2 intersects with a face of box 1 +// 4,5,6 = box 1 intersects with a face of box 2 +// 7..15 = edge-edge contact +// `maxc' is the maximum number of contacts allowed to be generated, i.e. +// the size of the `contact' array. +// `contact' and `skip' are the contact array information provided to the +// collision functions. this function only fills in the position and depth +// fields. +struct dContactGeom; +#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) +#define dInfinity FLT_MAX + + +/*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } +PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); } +PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); } +PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); } +*/ +static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } +static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); } +static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); } +static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); } +#define dMULTIPLYOP1_331(A,op,B,C) \ +{\ + (A)[0] op dDOT41((B),(C)); \ + (A)[1] op dDOT41((B+1),(C)); \ + (A)[2] op dDOT41((B+2),(C)); \ +} + +#define dMULTIPLYOP0_331(A,op,B,C) \ +{ \ + (A)[0] op dDOT((B),(C)); \ + (A)[1] op dDOT((B+4),(C)); \ + (A)[2] op dDOT((B+8),(C)); \ +} + +#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C) +#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) + +typedef btScalar dMatrix3[4*3]; + +void dLineClosestApproach (const btVector3& pa, const btVector3& ua, + const btVector3& pb, const btVector3& ub, + btScalar *alpha, btScalar *beta); +void dLineClosestApproach (const btVector3& pa, const btVector3& ua, + const btVector3& pb, const btVector3& ub, + btScalar *alpha, btScalar *beta) +{ + btVector3 p; + p[0] = pb[0] - pa[0]; + p[1] = pb[1] - pa[1]; + p[2] = pb[2] - pa[2]; + btScalar uaub = dDOT(ua,ub); + btScalar q1 = dDOT(ua,p); + btScalar q2 = -dDOT(ub,p); + btScalar d = 1-uaub*uaub; + if (d <= btScalar(0.0001f)) { + // @@@ this needs to be made more robust + *alpha = 0; + *beta = 0; + } + else { + d = 1.f/d; + *alpha = (q1 + uaub*q2)*d; + *beta = (uaub*q1 + q2)*d; + } +} + + + +// find all the intersection points between the 2D rectangle with vertices +// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), +// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). +// +// the intersection points are returned as x,y pairs in the 'ret' array. +// the number of intersection points is returned by the function (this will +// be in the range 0 to 8). + +static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16]) +{ + // q (and r) contain nq (and nr) coordinate points for the current (and + // chopped) polygons + int nq=4,nr=0; + btScalar buffer[16]; + btScalar *q = p; + btScalar *r = ret; + for (int dir=0; dir <= 1; dir++) { + // direction notation: xy[0] = x axis, xy[1] = y axis + for (int sign=-1; sign <= 1; sign += 2) { + // chop q along the line xy[dir] = sign*h[dir] + btScalar *pq = q; + btScalar *pr = r; + nr = 0; + for (int i=nq; i > 0; i--) { + // go through all points in q and all lines between adjacent points + if (sign*pq[dir] < h[dir]) { + // this point is inside the chopping line + pr[0] = pq[0]; + pr[1] = pq[1]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + btScalar *nextq = (i > 1) ? pq+2 : q; + if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { + // this line crosses the chopping line + pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / + (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); + pr[dir] = sign*h[dir]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + pq += 2; + } + q = r; + r = (q==ret) ? buffer : ret; + nq = nr; + } + } + done: + if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar)); + return nr; +} + + +#define M__PI 3.14159265f + +// given n points in the plane (array p, of size 2*n), generate m points that +// best represent the whole set. the definition of 'best' here is not +// predetermined - the idea is to select points that give good box-box +// collision detection behavior. the chosen point indexes are returned in the +// array iret (of size m). 'i0' is always the first entry in the array. +// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be +// in the range [0..n-1]. + +void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]); +void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) +{ + // compute the centroid of the polygon in cx,cy + int i,j; + btScalar a,cx,cy,q; + if (n==1) { + cx = p[0]; + cy = p[1]; + } + else if (n==2) { + cx = btScalar(0.5)*(p[0] + p[2]); + cy = btScalar(0.5)*(p[1] + p[3]); + } + else { + a = 0; + cx = 0; + cy = 0; + for (i=0; i<(n-1); i++) { + q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; + a += q; + cx += q*(p[i*2]+p[i*2+2]); + cy += q*(p[i*2+1]+p[i*2+3]); + } + q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; + a = 1.f/(btScalar(3.0)*(a+q)); + cx = a*(cx + q*(p[n*2-2]+p[0])); + cy = a*(cy + q*(p[n*2-1]+p[1])); + } + + // compute the angle of each point w.r.t. the centroid + btScalar A[8]; + for (i=0; i M__PI) a -= 2*M__PI; + btScalar maxdiff=1e9,diff; +#if defined(DEBUG) || defined (_DEBUG) + *iret = i0; // iret is not allowed to keep this value +#endif + for (i=0; i M__PI) diff = 2*M__PI - diff; + if (diff < maxdiff) { + maxdiff = diff; + *iret = i; + } + } + } +#if defined(DEBUG) || defined (_DEBUG) + btAssert (*iret != i0); // ensure iret got set +#endif + avail[*iret] = 0; + iret++; + } +} + + + +int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, + const btVector3& side1, const btVector3& p2, + const dMatrix3 R2, const btVector3& side2, + btVector3& normal, btScalar *depth, int *return_code, + int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output); +int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, + const btVector3& side1, const btVector3& p2, + const dMatrix3 R2, const btVector3& side2, + btVector3& normal, btScalar *depth, int *return_code, + int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output) +{ + const btScalar fudge_factor = btScalar(1.05); + btVector3 p,pp,normalC; + const btScalar *normalR = 0; + btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, + Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; + int i,j,invert_normal,code; + + // get vector from centers of box 1 to box 2, relative to box 1 + p = p2 - p1; + dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 + + // get side lengths / 2 + A[0] = side1[0]*btScalar(0.5); + A[1] = side1[1]*btScalar(0.5); + A[2] = side1[2]*btScalar(0.5); + B[0] = side2[0]*btScalar(0.5); + B[1] = side2[1]*btScalar(0.5); + B[2] = side2[2]*btScalar(0.5); + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 + R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); + R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); + R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); + + Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13); + Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23); + Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33); + + // for all 15 possible separating axes: + // * see if the axis separates the boxes. if so, return 0. + // * find the depth of the penetration along the separating axis (s2) + // * if this is the largest depth so far, record it. + // the normal vector will be set to the separating axis with the smallest + // depth. note: normalR is set to point to a column of R1 or R2 if that is + // the smallest depth normal so far. otherwise normalR is 0 and normalC is + // set to a vector relative to body 1. invert_normal is 1 if the sign of + // the normal should be flipped. + +#define TST(expr1,expr2,norm,cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } + + s = -dInfinity; + invert_normal = 0; + code = 0; + + // separating axis = u1,u2,u3 + TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); + TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); + TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); + + // separating axis = v1,v2,v3 + TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); + TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); + TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); + + // note: cross product axes need to be scaled when s is computed. + // normal (n1,n2,n3) is relative to box 1. +#undef TST +#define TST(expr1,expr2,n1,n2,n3,cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ + if (l > 0) { \ + s2 /= l; \ + if (s2*fudge_factor > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } \ + } + + // separating axis = u1 x (v1,v2,v3) + TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); + TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); + TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); + + // separating axis = u2 x (v1,v2,v3) + TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); + TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); + TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); + + // separating axis = u3 x (v1,v2,v3) + TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); + TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); + TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); + +#undef TST + + if (!code) return 0; + + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + dMULTIPLY0_331 (normal,R1,normalC); + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + *depth = -s; + + // compute contact point(s) + + if (code > 6) { + // an edge from box 1 touches an edge from box 2. + // find a point pa on the intersecting edge of box 1 + btVector3 pa; + btScalar sign; + for (i=0; i<3; i++) pa[i] = p1[i]; + for (j=0; j<3; j++) { + sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0); + for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; + } + + // find a point pb on the intersecting edge of box 2 + btVector3 pb; + for (i=0; i<3; i++) pb[i] = p2[i]; + for (j=0; j<3; j++) { + sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0); + for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; + } + + btScalar alpha,beta; + btVector3 ua,ub; + for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; + for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; + + dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); + for (i=0; i<3; i++) pa[i] += ua[i]*alpha; + for (i=0; i<3; i++) pb[i] += ub[i]*beta; + + { + + //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]); + //contact[0].depth = *depth; + btVector3 pointInWorld; + +#ifdef USE_CENTER_POINT + for (i=0; i<3; i++) + pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5); + output.addContactPoint(-normal,pointInWorld,-*depth); +#else + output.addContactPoint(-normal,pb,-*depth); +#endif // + *return_code = code; + } + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). define face 'a' to be the reference + // face (i.e. the normal vector is perpendicular to this) and face 'b' to be + // the incident face (the closest face of the other box). + + const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb; + if (code <= 3) { + Ra = R1; + Rb = R2; + pa = p1; + pb = p2; + Sa = A; + Sb = B; + } + else { + Ra = R2; + Rb = R1; + pa = p2; + pb = p1; + Sa = B; + Sb = A; + } + + // nr = normal vector of reference face dotted with axes of incident box. + // anr = absolute values of nr. + btVector3 normal2,nr,anr; + if (code <= 3) { + normal2[0] = normal[0]; + normal2[1] = normal[1]; + normal2[2] = normal[2]; + } + else { + normal2[0] = -normal[0]; + normal2[1] = -normal[1]; + normal2[2] = -normal[2]; + } + dMULTIPLY1_331 (nr,Rb,normal2); + anr[0] = btFabs (nr[0]); + anr[1] = btFabs (nr[1]); + anr[2] = btFabs (nr[2]); + + // find the largest compontent of anr: this corresponds to the normal + // for the indident face. the other axis numbers of the indicent face + // are stored in a1,a2. + int lanr,a1,a2; + if (anr[1] > anr[0]) { + if (anr[1] > anr[2]) { + a1 = 0; + lanr = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + else { + if (anr[0] > anr[2]) { + lanr = 0; + a1 = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + + // compute center point of incident face, in reference-face coordinates + btVector3 center; + if (nr[lanr] < 0) { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; + } + else { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; + } + + // find the normal and non-normal axis numbers of the reference box + int codeN,code1,code2; + if (code <= 3) codeN = code-1; else codeN = code-4; + if (codeN==0) { + code1 = 1; + code2 = 2; + } + else if (codeN==1) { + code1 = 0; + code2 = 2; + } + else { + code1 = 0; + code2 = 1; + } + + // find the four corners of the incident face, in reference-face coordinates + btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) + btScalar c1,c2,m11,m12,m21,m22; + c1 = dDOT14 (center,Ra+code1); + c2 = dDOT14 (center,Ra+code2); + // optimize this? - we have already computed this data above, but it is not + // stored in an easy-to-index format. for now it's quicker just to recompute + // the four dot products. + m11 = dDOT44 (Ra+code1,Rb+a1); + m12 = dDOT44 (Ra+code1,Rb+a2); + m21 = dDOT44 (Ra+code2,Rb+a1); + m22 = dDOT44 (Ra+code2,Rb+a2); + { + btScalar k1 = m11*Sb[a1]; + btScalar k2 = m21*Sb[a1]; + btScalar k3 = m12*Sb[a2]; + btScalar k4 = m22*Sb[a2]; + quad[0] = c1 - k1 - k3; + quad[1] = c2 - k2 - k4; + quad[2] = c1 - k1 + k3; + quad[3] = c2 - k2 + k4; + quad[4] = c1 + k1 + k3; + quad[5] = c2 + k2 + k4; + quad[6] = c1 + k1 - k3; + quad[7] = c2 + k2 - k4; + } + + // find the size of the reference face + btScalar rect[2]; + rect[0] = Sa[code1]; + rect[1] = Sa[code2]; + + // intersect the incident and reference faces + btScalar ret[16]; + int n = intersectRectQuad2 (rect,quad,ret); + if (n < 1) return 0; // this should never happen + + // convert the intersection points into reference-face coordinates, + // and compute the contact position and depth for each point. only keep + // those points that have a positive (penetrating) depth. delete points in + // the 'ret' array as necessary so that 'point' and 'ret' correspond. + btScalar point[3*8]; // penetrating contact points + btScalar dep[8]; // depths for those points + btScalar det1 = 1.f/(m11*m22 - m12*m21); + m11 *= det1; + m12 *= det1; + m21 *= det1; + m22 *= det1; + int cnum = 0; // number of penetrating contact points found + for (j=0; j < n; j++) { + btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); + btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); + for (i=0; i<3; i++) point[cnum*3+i] = + center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; + dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); + if (dep[cnum] >= 0) { + ret[cnum*2] = ret[j*2]; + ret[cnum*2+1] = ret[j*2+1]; + cnum++; + } + } + if (cnum < 1) return 0; // this should never happen + + // we can't generate more contacts than we actually have + if (maxc > cnum) maxc = cnum; + if (maxc < 1) maxc = 1; + + if (cnum <= maxc) { + // we have less contacts than we need, so we use them all + for (j=0; j < cnum; j++) { + + //AddContactPoint... + + //dContactGeom *con = CONTACT(contact,skip*j); + //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i]; + //con->depth = dep[j]; + + btVector3 pointInWorld; + for (i=0; i<3; i++) + pointInWorld[i] = point[j*3+i] + pa[i]; + output.addContactPoint(-normal,pointInWorld,-dep[j]); + + } + } + else { + // we have more contacts than are wanted, some of them must be culled. + // find the deepest point, it is always the first contact. + int i1 = 0; + btScalar maxdepth = dep[0]; + for (i=1; i maxdepth) { + maxdepth = dep[i]; + i1 = i; + } + } + + int iret[8]; + cullPoints2 (cnum,ret,maxc,i1,iret); + + for (j=0; j < maxc; j++) { +// dContactGeom *con = CONTACT(contact,skip*j); + // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; + // con->depth = dep[iret[j]]; + + btVector3 posInWorld; + for (i=0; i<3; i++) + posInWorld[i] = point[iret[j]*3+i] + pa[i]; + output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); + } + cnum = maxc; + } + + *return_code = code; + return cnum; +} + +void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/) +{ + + const btTransform& transformA = input.m_transformA; + const btTransform& transformB = input.m_transformB; + + int skip = 0; + dContactGeom *contact = 0; + + dMatrix3 R1; + dMatrix3 R2; + + for (int j=0;j<3;j++) + { + R1[0+4*j] = transformA.getBasis()[j].x(); + R2[0+4*j] = transformB.getBasis()[j].x(); + + R1[1+4*j] = transformA.getBasis()[j].y(); + R2[1+4*j] = transformB.getBasis()[j].y(); + + + R1[2+4*j] = transformA.getBasis()[j].z(); + R2[2+4*j] = transformB.getBasis()[j].z(); + + } + + + + btVector3 normal; + btScalar depth; + int return_code; + int maxc = 4; + + + dBoxBox2 (transformA.getOrigin(), + R1, + 2.f*m_box1->getHalfExtentsWithMargin(), + transformB.getOrigin(), + R2, + 2.f*m_box2->getHalfExtentsWithMargin(), + normal, &depth, &return_code, + maxc, contact, skip, + output + ); + +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h new file mode 100644 index 00000000000..605294d47bd --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h @@ -0,0 +1,44 @@ +/* + * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith + * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. + * All rights reserved. Email: russ@q12.org Web: www.q12.org + +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef BOX_BOX_DETECTOR_H +#define BOX_BOX_DETECTOR_H + + +class btBoxShape; +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" + + +/// btBoxBoxDetector wraps the ODE box-box collision detector +/// re-distributed under the Zlib license with permission from Russell L. Smith +struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface +{ + btBoxShape* m_box1; + btBoxShape* m_box2; + +public: + + btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2); + + virtual ~btBoxBoxDetector() {}; + + virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + +}; + +#endif //BT_BOX_BOX_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h new file mode 100644 index 00000000000..fad770ac26d --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -0,0 +1,47 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_COLLISION_CONFIGURATION +#define BT_COLLISION_CONFIGURATION +struct btCollisionAlgorithmCreateFunc; + +class btStackAlloc; +class btPoolAllocator; + +///btCollisionConfiguration allows to configure Bullet collision detection +///stack allocator size, default collision algorithms and persistent manifold pool size +///todo: describe the meaning +class btCollisionConfiguration +{ + +public: + + virtual ~btCollisionConfiguration() + { + } + + ///memory pools + virtual btPoolAllocator* getPersistentManifoldPool() = 0; + + virtual btPoolAllocator* getCollisionAlgorithmPool() = 0; + + virtual btStackAlloc* getStackAllocator() = 0; + + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0; + +}; + +#endif //BT_COLLISION_CONFIGURATION + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp new file mode 100644 index 00000000000..391cf6c7bc4 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -0,0 +1,108 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btConvexPlaneCollisionAlgorithm.h" + +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" + +//#include + +btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) +: btCollisionAlgorithm(ci), +m_ownManifold(false), +m_manifoldPtr(mf), +m_isSwapped(isSwapped) +{ + btCollisionObject* convexObj = m_isSwapped? col1 : col0; + btCollisionObject* planeObj = m_isSwapped? col0 : col1; + + if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj); + m_ownManifold = true; + } +} + + +btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + + + +void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + (void)resultOut; + if (!m_manifoldPtr) + return; + + btCollisionObject* convexObj = m_isSwapped? body1 : body0; + btCollisionObject* planeObj = m_isSwapped? body0: body1; + + btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); + + bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + btTransform planeInConvex; + planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform(); + btTransform convexInPlaneTrans; + convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform(); + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + btVector3 vtxInPlane = convexInPlaneTrans(vtx); + btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); + + btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected; + + hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); + resultOut->setPersistentManifold(m_manifoldPtr); + if (hasCollision) + { + /// report a contact. internally this will be kept persistent, and contact reduction is done + btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal; + btVector3 pOnB = vtxInPlaneWorld; + resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + } + if (m_ownManifold) + { + if (m_manifoldPtr->getNumContacts()) + { + resultOut->refreshContactPoints(); + } + } +} + +btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + + //not yet + return btScalar(1.); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h new file mode 100644 index 00000000000..7b258554171 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -0,0 +1,71 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H +#define CONVEX_PLANE_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "btCollisionDispatcher.h" + +#include "LinearMath/btVector3.h" + +/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; + +public: + + btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); + + virtual ~btConvexPlaneCollisionAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); + if (!m_swapped) + { + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false); + } else + { + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true); + } + } + }; + +}; + +#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp new file mode 100644 index 00000000000..1c317080544 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -0,0 +1,291 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btDefaultCollisionConfiguration.h" + +#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM +#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM +#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" + + + +#include "LinearMath/btStackAlloc.h" +#include "LinearMath/btPoolAllocator.h" + + + + + +btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) +//btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* stackAlloc,btPoolAllocator* persistentManifoldPool,btPoolAllocator* collisionAlgorithmPool) +{ + + void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16); + m_simplexSolver = new (mem)btVoronoiSimplexSolver(); + +#define USE_EPA 1 +#ifdef USE_EPA + mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16); + m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver; +#else + mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16); + m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver; +#endif//USE_EPA + + + //default CreationFunctions, filling the m_doubleDispatch table + mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16); + m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver); + mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); + m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); + m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16); + m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16); + m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc; + mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16); + m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16); + m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc; +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); + m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); + m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc; + m_boxSphereCF->m_swapped = true; +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); + m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); + m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; + m_triangleSphereCF->m_swapped = true; + + mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16); + m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc; + + //convex versus plane + mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; + m_planeConvexCF->m_swapped = true; + + ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool + int maxSize = sizeof(btConvexConvexAlgorithm); + int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); + int maxSize3 = sizeof(btCompoundCollisionAlgorithm); + int maxSize4 = sizeof(btEmptyAlgorithm); + + int collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4); + + if (constructionInfo.m_stackAlloc) + { + m_ownsStackAllocator = false; + this->m_stackAlloc = constructionInfo.m_stackAlloc; + } else + { + m_ownsStackAllocator = true; + void* mem = btAlignedAlloc(sizeof(btStackAlloc),16); + m_stackAlloc = new(mem)btStackAlloc(constructionInfo.m_defaultStackAllocatorSize); + } + + if (constructionInfo.m_persistentManifoldPool) + { + m_ownsPersistentManifoldPool = false; + m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool; + } else + { + m_ownsPersistentManifoldPool = true; + void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); + m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize); + } + + if (constructionInfo.m_collisionAlgorithmPool) + { + m_ownsCollisionAlgorithmPool = false; + m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool; + } else + { + m_ownsCollisionAlgorithmPool = true; + void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); + m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); + } + + +} + +btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() +{ + if (m_ownsStackAllocator) + { + m_stackAlloc->destroy(); + m_stackAlloc->~btStackAlloc(); + btAlignedFree(m_stackAlloc); + } + if (m_ownsCollisionAlgorithmPool) + { + m_collisionAlgorithmPool->~btPoolAllocator(); + btAlignedFree(m_collisionAlgorithmPool); + } + if (m_ownsPersistentManifoldPool) + { + m_persistentManifoldPool->~btPoolAllocator(); + btAlignedFree(m_persistentManifoldPool); + } + + m_convexConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_convexConvexCreateFunc); + + m_convexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_convexConcaveCreateFunc); + m_swappedConvexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_swappedConvexConcaveCreateFunc); + + m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_compoundCreateFunc); + + m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_swappedCompoundCreateFunc); + + m_emptyCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_emptyCreateFunc); + + m_sphereSphereCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_sphereSphereCF); + +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + m_sphereBoxCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_sphereBoxCF); + m_boxSphereCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_boxSphereCF); +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_sphereTriangleCF); + m_triangleSphereCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_triangleSphereCF); + m_boxBoxCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_boxBoxCF); + + m_convexPlaneCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_convexPlaneCF); + m_planeConvexCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_planeConvexCF); + + m_simplexSolver->~btVoronoiSimplexSolver(); + btAlignedFree(m_simplexSolver); + + m_pdSolver->~btConvexPenetrationDepthSolver(); + + btAlignedFree(m_pdSolver); + + +} + + +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) +{ + + + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_sphereSphereCF; + } +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE)) + { + return m_sphereBoxCF; + } + + if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_boxSphereCF; + } +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE)) + { + return m_sphereTriangleCF; + } + + if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_triangleSphereCF; + } + + if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) + { + return m_boxBoxCF; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) + { + return m_convexPlaneCF; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE)) + { + return m_planeConvexCF; + } + + + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) + { + return m_convexConvexCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1)) + { + return m_convexConcaveCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0)) + { + return m_swappedConvexConcaveCreateFunc; + } + + if (btBroadphaseProxy::isCompound(proxyType0)) + { + return m_compoundCreateFunc; + } else + { + if (btBroadphaseProxy::isCompound(proxyType1)) + { + return m_swappedCompoundCreateFunc; + } + } + + //failed to find an algorithm + return m_emptyCreateFunc; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h new file mode 100644 index 00000000000..df48ee95a18 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -0,0 +1,118 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_DEFAULT_COLLISION_CONFIGURATION +#define BT_DEFAULT_COLLISION_CONFIGURATION + +#include "btCollisionConfiguration.h" +class btVoronoiSimplexSolver; +class btConvexPenetrationDepthSolver; + +struct btDefaultCollisionConstructionInfo +{ + btStackAlloc* m_stackAlloc; + btPoolAllocator* m_persistentManifoldPool; + btPoolAllocator* m_collisionAlgorithmPool; + int m_defaultMaxPersistentManifoldPoolSize; + int m_defaultMaxCollisionAlgorithmPoolSize; + int m_defaultStackAllocatorSize; + + btDefaultCollisionConstructionInfo() + :m_stackAlloc(0), + m_persistentManifoldPool(0), + m_collisionAlgorithmPool(0), + m_defaultMaxPersistentManifoldPoolSize(65535), + m_defaultMaxCollisionAlgorithmPoolSize(65535), + m_defaultStackAllocatorSize(5*1024*1024) + { + } +}; + + + +///btCollisionConfiguration allows to configure Bullet collision detection +///stack allocator, pool memory allocators +///todo: describe the meaning +class btDefaultCollisionConfiguration : public btCollisionConfiguration +{ + +protected: + + int m_persistentManifoldPoolSize; + + btStackAlloc* m_stackAlloc; + bool m_ownsStackAllocator; + + btPoolAllocator* m_persistentManifoldPool; + bool m_ownsPersistentManifoldPool; + + + btPoolAllocator* m_collisionAlgorithmPool; + bool m_ownsCollisionAlgorithmPool; + + //default simplex/penetration depth solvers + btVoronoiSimplexSolver* m_simplexSolver; + btConvexPenetrationDepthSolver* m_pdSolver; + + //default CreationFunctions, filling the m_doubleDispatch table + btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc; + btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; + btCollisionAlgorithmCreateFunc* m_sphereSphereCF; +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + btCollisionAlgorithmCreateFunc* m_sphereBoxCF; + btCollisionAlgorithmCreateFunc* m_boxSphereCF; +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + btCollisionAlgorithmCreateFunc* m_boxBoxCF; + btCollisionAlgorithmCreateFunc* m_sphereTriangleCF; + btCollisionAlgorithmCreateFunc* m_triangleSphereCF; + btCollisionAlgorithmCreateFunc* m_planeConvexCF; + btCollisionAlgorithmCreateFunc* m_convexPlaneCF; + +public: + + + btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); + + virtual ~btDefaultCollisionConfiguration(); + + ///memory pools + virtual btPoolAllocator* getPersistentManifoldPool() + { + return m_persistentManifoldPool; + } + + virtual btPoolAllocator* getCollisionAlgorithmPool() + { + return m_collisionAlgorithmPool; + } + + virtual btStackAlloc* getStackAllocator() + { + return m_stackAlloc; + } + + + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + + +}; + +#endif //BT_DEFAULT_COLLISION_CONFIGURATION + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp new file mode 100644 index 00000000000..fb81c8a5bde --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp @@ -0,0 +1,78 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btConvexInternalShape.h" + + +btConvexInternalShape::btConvexInternalShape() +: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), +m_collisionMargin(CONVEX_DISTANCE_MARGIN) +{ +} + + +void btConvexInternalShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling.absolute(); +} + + + +void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const +{ + + btScalar margin = getMargin(); + for (int i=0;i<3;i++) + { + btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + vec[i] = btScalar(1.); + + btVector3 sv = localGetSupportingVertex(vec*trans.getBasis()); + + btVector3 tmp = trans(sv); + maxAabb[i] = tmp[i]+margin; + vec[i] = btScalar(-1.); + tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); + minAabb[i] = tmp[i]-margin; + } +}; + + +btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const +{ +#ifndef __SPU__ + + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); + + if ( getMargin()!=btScalar(0.) ) + { + btVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); + } + vecnorm.normalize(); + supVertex+= getMargin() * vecnorm; + } + return supVertex; + +#else + return btVector3(0,0,0); +#endif //__SPU__ + + } + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h new file mode 100644 index 00000000000..9388ccf31f2 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h @@ -0,0 +1,98 @@ + +#ifndef BT_CONVEX_INTERNAL_SHAPE_H +#define BT_CONVEX_INTERNAL_SHAPE_H + +#include "btConvexShape.h" + +///The btConvexInternalShape is an internal base class, shared by most convex shape implementations. +class btConvexInternalShape : public btConvexShape +{ + + protected: + + //local scaling. collisionMargin is not scaled ! + btVector3 m_localScaling; + + btVector3 m_implicitShapeDimensions; + + btScalar m_collisionMargin; + + btScalar m_padding; + +public: + + btConvexInternalShape(); + + virtual ~btConvexInternalShape() + { + + } + + + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; +#ifndef __SPU__ + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0; + + //notice that the vectors should be unit length + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; +#endif //#ifndef __SPU__ + + const btVector3& getImplicitShapeDimensions() const + { + return m_implicitShapeDimensions; + } + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + { + getAabbSlow(t,aabbMin,aabbMax); + } + + + + virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const + { + return m_localScaling; + } + + const btVector3& getLocalScalingNV() const + { + return m_localScaling; + } + + virtual void setMargin(btScalar margin) + { + m_collisionMargin = margin; + } + virtual btScalar getMargin() const + { + return m_collisionMargin; + } + + btScalar getMarginNV() const + { + return m_collisionMargin; + } + + virtual int getNumPreferredPenetrationDirections() const + { + return 0; + } + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + { + (void)penetrationVector; + (void)index; + btAssert(0); + } + + + +}; + + +#endif //BT_CONVEX_INTERNAL_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h new file mode 100644 index 00000000000..7cb6d5ab6fe --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h @@ -0,0 +1,34 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/// This file was created by Alex Silverman + +#ifndef MATERIAL_H +#define MATERIAL_H + +// Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties +class btMaterial +{ + // public members so that materials can change due to world events +public: + btScalar m_friction; + btScalar m_restitution; + int pad[2]; + + btMaterial(){} + btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; } +}; + +#endif // MATERIAL_H \ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp new file mode 100644 index 00000000000..fc47e86411d --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp @@ -0,0 +1,45 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/// This file was created by Alex Silverman + +#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h" +//#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" + + +///Obtains the material for a specific triangle +const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex) +{ + const unsigned char * materialBase = 0; + int numMaterials; + PHY_ScalarType materialType; + int materialStride; + const unsigned char * triangleMaterialBase = 0; + int numTriangles; + int triangleMaterialStride; + PHY_ScalarType triangleType; + + ((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride, + &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID); + + // return the pointer to the place with the friction for the triangle + // TODO: This depends on whether it's a moving mesh or not + // BUG IN GIMPACT + //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]); + int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)])); + btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride])); + return (matVal); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h new file mode 100644 index 00000000000..2f108020cdf --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h @@ -0,0 +1,124 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/// This file was created by Alex Silverman + +#ifndef BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H +#define BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H + +#include "btBvhTriangleMeshShape.h" +#include "btMaterial.h" + +///The BvhTriangleMaterialMeshShape extends the btBvhTriangleMeshShape. Its main contribution is the interface into a material array, which allows per-triangle friction and restitution. +ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape +{ + btAlignedObjectArray m_materialList; + int ** m_triangleMaterials; + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {} + btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true): + btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) + { + btVector3 m_triangle[3]; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); + + for(int i = 0; i < meshInterface->getNumSubParts(); i++) + { + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + i); + //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16)); + } + } + + ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb + btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true): + btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh) + { + btVector3 m_triangle[3]; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); + + for(int i = 0; i < meshInterface->getNumSubParts(); i++) + { + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + i); + //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16)); + } + } + + virtual ~btMultimaterialTriangleMeshShape() + { +/* + for(int i = 0; i < m_meshInterface->getNumSubParts(); i++) + { + btAlignedFree(m_materialValues[i]); + m_materialLookup[i] = NULL; + } + btAlignedFree(m_materialValues); + m_materialLookup = NULL; +*/ + } + virtual int getShapeType() const + { + return MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; + } + + //debugging + virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";} + + ///Obtains the material for a specific triangle + const btMaterial * getMaterialProperties(int partID, int triIndex); + +} +; + +#endif //BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp new file mode 100644 index 00000000000..2b81a02b557 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp @@ -0,0 +1,121 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btScaledBvhTriangleMeshShape.h" + +btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling) +:m_bvhTriMeshShape(childShape), +m_localScaling(localScaling) +{ + +} + +btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape() +{ +} + + +class btScaledTriangleCallback : public btTriangleCallback +{ + btTriangleCallback* m_originalCallback; + + btVector3 m_localScaling; + +public: + + btScaledTriangleCallback(btTriangleCallback* originalCallback,btVector3 localScaling) + :m_originalCallback(originalCallback), + m_localScaling(localScaling) + { + } + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + btVector3 newTriangle[3]; + newTriangle[0] = triangle[0]*m_localScaling; + newTriangle[1] = triangle[1]*m_localScaling; + newTriangle[2] = triangle[2]*m_localScaling; + m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex); + } +}; + +void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + btScaledTriangleCallback scaledCallback(callback,m_localScaling); + + btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ()); + btVector3 scaledAabbMin,scaledAabbMax; + + ///support negative scaling + scaledAabbMin[0] = m_localScaling.getX() >= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; + scaledAabbMin[1] = m_localScaling.getY() >= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; + scaledAabbMin[2] = m_localScaling.getZ() >= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; + + scaledAabbMax[0] = m_localScaling.getX() <= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; + scaledAabbMax[1] = m_localScaling.getY() <= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; + scaledAabbMax[2] = m_localScaling.getZ() <= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; + + + m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax); +} + + +void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +{ + btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin(); + btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax(); + + btVector3 tmpLocalAabbMin = localAabbMin * m_localScaling; + btVector3 tmpLocalAabbMax = localAabbMax * m_localScaling; + + localAabbMin[0] = (m_localScaling.getX() >= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0]; + localAabbMin[1] = (m_localScaling.getY() >= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; + localAabbMin[2] = (m_localScaling.getZ() >= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; + localAabbMax[0] = (m_localScaling.getX() <= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0]; + localAabbMax[1] = (m_localScaling.getY() <= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; + localAabbMax[2] = (m_localScaling.getZ() <= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; + + btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin); + btScalar margin = m_bvhTriMeshShape->getMargin(); + localHalfExtents += btVector3(margin,margin,margin); + btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin); + + btMatrix3x3 abs_b = trans.getBasis().absolute(); + + btPoint3 center = trans(localCenter); + + btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + aabbMin = center - extent; + aabbMax = center + extent; + +} + +void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling; +} + +const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const +{ + return m_localScaling; +} + +void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + ///don't make this a movable object! +// btAssert(0); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h new file mode 100644 index 00000000000..0a1ef8ce5cb --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h @@ -0,0 +1,67 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SCALED_BVH_TRIANGLE_MESH_SHAPE_H +#define SCALED_BVH_TRIANGLE_MESH_SHAPE_H + +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" + + +///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape. +///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling' +ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape +{ + + + btVector3 m_localScaling; + + btBvhTriangleMeshShape* m_bvhTriMeshShape; + +public: + + + btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling); + + virtual ~btScaledBvhTriangleMeshShape(); + + virtual int getShapeType() const + { + //use un-used 'FAST_CONCAVE_MESH_PROXYTYPE' for now, later add SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE to btBroadphaseProxy.h + return SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; + } + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + btBvhTriangleMeshShape* getChildShape() + { + return m_bvhTriMeshShape; + } + + const btBvhTriangleMeshShape* getChildShape() const + { + return m_bvhTriMeshShape; + } + + //debugging + virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";} + +}; + +#endif //BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp new file mode 100644 index 00000000000..a87b87f1a6f --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -0,0 +1,164 @@ +/* +btbtShapeHull implemented by John McCutchan. + +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +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 "btShapeHull.h" +#include "LinearMath/btConvexHull.h" + +#define NUM_UNITSPHERE_POINTS 42 + +static btVector3 btUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = +{ + btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), + btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), + btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), + btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), + btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), + btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), + btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), + btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), + btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), + btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), + btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), + btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), + btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), + btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), + btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), + btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), + btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), + btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), + btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), + btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), + btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), + btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), + btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), + btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), + btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), + btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), + btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), + btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), + btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), + btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), + btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), + btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), + btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), + btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), + btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), + btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), + btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), + btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), + btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), + btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), + btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), + btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) +}; + +btShapeHull::btShapeHull (const btConvexShape* shape) +{ + m_shape = shape; + m_vertices.clear (); + m_indices.clear(); + m_numIndices = 0; +} + +btShapeHull::~btShapeHull () +{ + m_indices.clear(); + m_vertices.clear (); +} + +bool +btShapeHull::buildHull (btScalar /*margin*/) +{ + int numSampleDirections = NUM_UNITSPHERE_POINTS; + { + int numPDA = m_shape->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int i=0;igetPreferredPenetrationDirection(i,norm); + btUnitSpherePoints[numSampleDirections] = norm; + numSampleDirections++; + } + } + } + + btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + int i; + for (i = 0; i < numSampleDirections; i++) + { + supportPoints[i] = m_shape->localGetSupportingVertex(btUnitSpherePoints[i]); + } + + HullDesc hd; + hd.mFlags = QF_TRIANGLES; + hd.mVcount = static_cast(numSampleDirections); + +#ifdef BT_USE_DOUBLE_PRECISION + hd.mVertices = &supportPoints[0]; + hd.mVertexStride = sizeof(btVector3); +#else + hd.mVertices = &supportPoints[0]; + hd.mVertexStride = sizeof (btVector3); +#endif + + HullLibrary hl; + HullResult hr; + if (hl.CreateConvexHull (hd, hr) == QE_FAIL) + { + return false; + } + + m_vertices.resize (static_cast(hr.mNumOutputVertices)); + + + for (i = 0; i < static_cast(hr.mNumOutputVertices); i++) + { + m_vertices[i] = hr.m_OutputVertices[i]; + } + m_numIndices = hr.mNumIndices; + m_indices.resize(static_cast(m_numIndices)); + for (i = 0; i < static_cast(m_numIndices); i++) + { + m_indices[i] = hr.m_Indices[i]; + } + + // free temporary hull result that we just copied + hl.ReleaseResult (hr); + + return true; +} + +int +btShapeHull::numTriangles () const +{ + return static_cast(m_numIndices / 3); +} + +int +btShapeHull::numVertices () const +{ + return m_vertices.size (); +} + +int +btShapeHull::numIndices () const +{ + return static_cast(m_numIndices); +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h new file mode 100644 index 00000000000..583c6b99eb6 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h @@ -0,0 +1,56 @@ +/* +btShapeHull implemented by John McCutchan. + +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef _SHAPE_HULL_H +#define _SHAPE_HULL_H + +#include "LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" + + +///The btShapeHull class takes a btConvexShape, builds a simplified convex hull using btConvexHull and provides triangle indices and vertices. +///It can be useful for to simplify a complex convex object and for visualization of a non-polyhedral convex object. +///It approximates the convex hull using the supporting vertex of 42 directions. +class btShapeHull +{ +public: + btShapeHull (const btConvexShape* shape); + ~btShapeHull (); + + bool buildHull (btScalar margin); + + int numTriangles () const; + int numVertices () const; + int numIndices () const; + + const btVector3* getVertexPointer() const + { + return &m_vertices[0]; + } + const unsigned int* getIndexPointer() const + { + return &m_indices[0]; + } + +protected: + btAlignedObjectArray m_vertices; + btAlignedObjectArray m_indices; + unsigned int m_numIndices; + const btConvexShape* m_shape; +}; + +#endif //_SHAPE_HULL_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp new file mode 100644 index 00000000000..492854ff646 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp @@ -0,0 +1,86 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was created by Alex Silverman + +#include "btTriangleIndexVertexMaterialArray.h" + +btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, + int numVertices,btScalar* vertexBase,int vertexStride, + int numMaterials, unsigned char* materialBase, int materialStride, + int* triangleMaterialsBase, int materialIndexStride) : +btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride) +{ + btMaterialProperties mat; + + mat.m_numMaterials = numMaterials; + mat.m_materialBase = materialBase; + mat.m_materialStride = materialStride; +#ifdef BT_USE_DOUBLE_PRECISION + mat.m_materialType = PHY_DOUBLE; +#else + mat.m_materialType = PHY_FLOAT; +#endif + + mat.m_numTriangles = numTriangles; + mat.m_triangleMaterialsBase = (unsigned char *)triangleMaterialsBase; + mat.m_triangleMaterialStride = materialIndexStride; + mat.m_triangleType = PHY_INTEGER; + + addMaterialProperties(mat); +} + + +void btTriangleIndexVertexMaterialArray::getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) +{ + btAssert(subpart< getNumSubParts() ); + + btMaterialProperties& mats = m_materials[subpart]; + + numMaterials = mats.m_numMaterials; + (*materialBase) = (unsigned char *) mats.m_materialBase; +#ifdef BT_USE_DOUBLE_PRECISION + materialType = PHY_DOUBLE; +#else + materialType = PHY_FLOAT; +#endif + materialStride = mats.m_materialStride; + + numTriangles = mats.m_numTriangles; + (*triangleMaterialBase) = (unsigned char *)mats.m_triangleMaterialsBase; + triangleMaterialStride = mats.m_triangleMaterialStride; + triangleType = mats.m_triangleType; +} + +void btTriangleIndexVertexMaterialArray::getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) +{ + btMaterialProperties& mats = m_materials[subpart]; + + numMaterials = mats.m_numMaterials; + (*materialBase) = (const unsigned char *) mats.m_materialBase; +#ifdef BT_USE_DOUBLE_PRECISION + materialType = PHY_DOUBLE; +#else + materialType = PHY_FLOAT; +#endif + materialStride = mats.m_materialStride; + + numTriangles = mats.m_numTriangles; + (*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase; + triangleMaterialStride = mats.m_triangleMaterialStride; + triangleType = mats.m_triangleType; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h new file mode 100644 index 00000000000..69a2e631458 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h @@ -0,0 +1,84 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was created by Alex Silverman + +#ifndef BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H +#define BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H + +#include "btTriangleIndexVertexArray.h" + + +ATTRIBUTE_ALIGNED16( struct) btMaterialProperties +{ + ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution + int m_numMaterials; + const unsigned char * m_materialBase; + int m_materialStride; + PHY_ScalarType m_materialType; + ///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're + /// padding the structure, it can be reproduced at no real cost + ///m_triangleMaterials =====> 1 integer value makes up one entry + /// eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5 + int m_numTriangles; + const unsigned char * m_triangleMaterialsBase; + int m_triangleMaterialStride; + ///m_triangleType <========== Automatically set in addMaterialProperties + PHY_ScalarType m_triangleType; +}; + +typedef btAlignedObjectArray MaterialArray; + +///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray +///The addition of a material array allows for the utilization of the partID and +///triangleIndex that are returned in the ContactAddedCallback. As with +///TriangleIndexVertexArray, no duplicate is made of the material data, so it +///is the users responsibility to maintain the array during the lifetime of the +///TriangleIndexVertexMaterialArray. +ATTRIBUTE_ALIGNED16(class) btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray +{ +protected: + MaterialArray m_materials; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btTriangleIndexVertexMaterialArray() + { + } + + btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, + int numVertices,btScalar* vertexBase,int vertexStride, + int numMaterials, unsigned char* materialBase, int materialStride, + int* triangleMaterialsBase, int materialIndexStride); + + virtual ~btTriangleIndexVertexMaterialArray() {} + + void addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER) + { + m_materials.push_back(mat); + m_materials[m_materials.size()-1].m_triangleType = triangleType; + } + + virtual void getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType ,int subpart = 0); + + virtual void getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0); + +} +; + +#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp new file mode 100644 index 00000000000..ef340286cb0 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp @@ -0,0 +1,114 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btUniformScalingShape.h" + +btUniformScalingShape::btUniformScalingShape( btConvexShape* convexChildShape,btScalar uniformScalingFactor): +m_childConvexShape(convexChildShape), +m_uniformScalingFactor(uniformScalingFactor) +{ +} + +btUniformScalingShape::~btUniformScalingShape() +{ +} + + +btVector3 btUniformScalingShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +{ + btVector3 tmpVertex; + tmpVertex = m_childConvexShape->localGetSupportingVertexWithoutMargin(vec); + return tmpVertex*m_uniformScalingFactor; +} + +void btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors); + int i; + for (i=0;ilocalGetSupportingVertex(vec); + return tmpVertex*m_uniformScalingFactor; +} + + +void btUniformScalingShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + + ///this linear upscaling is not realistic, but we don't deal with large mass ratios... + btVector3 tmpInertia; + m_childConvexShape->calculateLocalInertia(mass,tmpInertia); + inertia = tmpInertia * m_uniformScalingFactor; +} + + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btUniformScalingShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + m_childConvexShape->getAabb(t,aabbMin,aabbMax); + btVector3 aabbCenter = (aabbMax+aabbMin)*btScalar(0.5); + btVector3 scaledAabbHalfExtends = (aabbMax-aabbMin)*btScalar(0.5)*m_uniformScalingFactor; + + aabbMin = aabbCenter - scaledAabbHalfExtends; + aabbMax = aabbCenter + scaledAabbHalfExtends; + +} + +void btUniformScalingShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax); + btVector3 aabbCenter = (aabbMax+aabbMin)*btScalar(0.5); + btVector3 scaledAabbHalfExtends = (aabbMax-aabbMin)*btScalar(0.5)*m_uniformScalingFactor; + + aabbMin = aabbCenter - scaledAabbHalfExtends; + aabbMax = aabbCenter + scaledAabbHalfExtends; +} + +void btUniformScalingShape::setLocalScaling(const btVector3& scaling) +{ + m_childConvexShape->setLocalScaling(scaling); +} + +const btVector3& btUniformScalingShape::getLocalScaling() const +{ + return m_childConvexShape->getLocalScaling(); +} + +void btUniformScalingShape::setMargin(btScalar margin) +{ + m_childConvexShape->setMargin(margin); +} +btScalar btUniformScalingShape::getMargin() const +{ + return m_childConvexShape->getMargin() * m_uniformScalingFactor; +} + +int btUniformScalingShape::getNumPreferredPenetrationDirections() const +{ + return m_childConvexShape->getNumPreferredPenetrationDirections(); +} + +void btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const +{ + m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h new file mode 100644 index 00000000000..1e17fc8e198 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h @@ -0,0 +1,88 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_UNIFORM_SCALING_SHAPE_H +#define BT_UNIFORM_SCALING_SHAPE_H + +#include "btConvexShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types + +///The btUniformScalingShape allows to re-use uniform scaled instances of btConvexShape in a memory efficient way. +///Istead of using btUniformScalingShape, it is better to use the non-uniform setLocalScaling method on convex shapes that implement it. +class btUniformScalingShape : public btConvexShape +{ + btConvexShape* m_childConvexShape; + + btScalar m_uniformScalingFactor; + + public: + + btUniformScalingShape( btConvexShape* convexChildShape, btScalar uniformScalingFactor); + + virtual ~btUniformScalingShape(); + + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + btScalar getUniformScalingFactor() const + { + return m_uniformScalingFactor; + } + + btConvexShape* getChildShape() + { + return m_childConvexShape; + } + + const btConvexShape* getChildShape() const + { + return m_childConvexShape; + } + + virtual const char* getName()const + { + return "UniformScalingShape"; + } + + virtual int getShapeType() const { return UNIFORM_SCALING_SHAPE_PROXYTYPE; } + + + /////////////////////////// + + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + virtual void setLocalScaling(const btVector3& scaling) ; + virtual const btVector3& getLocalScaling() const ; + + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + + virtual int getNumPreferredPenetrationDirections() const; + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; + + +}; + +#endif //BT_UNIFORM_SCALING_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp new file mode 100644 index 00000000000..ccfc22ee673 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -0,0 +1,943 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the +use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not +claim that you wrote the original software. If you use this software in a +product, an acknowledgment in the product documentation would be appreciated +but is not required. +2. Altered source versions must be plainly marked as such, and must not be +misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson, 2008 +*/ +#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "btGjkEpa2.h" + +#if defined(DEBUG) || defined (_DEBUG) +#include //for debug printf +#ifdef __SPU__ +#include +#define printf spu_printf +#endif //__SPU__ +#endif + +namespace gjkepa2_impl +{ + +// Config + + /* GJK */ +#define GJK_MAX_ITERATIONS 128 +#define GJK_ACCURARY ((btScalar)0.0001) +#define GJK_MIN_DISTANCE ((btScalar)0.0001) +#define GJK_DUPLICATED_EPS ((btScalar)0.0001) +#define GJK_SIMPLEX2_EPS ((btScalar)0.0) +#define GJK_SIMPLEX3_EPS ((btScalar)0.0) +#define GJK_SIMPLEX4_EPS ((btScalar)0.0) + + /* EPA */ +#define EPA_MAX_VERTICES 64 +#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) +#define EPA_MAX_ITERATIONS 255 +#define EPA_ACCURACY ((btScalar)0.0001) +#define EPA_FALLBACK (10*EPA_ACCURACY) +#define EPA_PLANE_EPS ((btScalar)0.00001) +#define EPA_INSIDE_EPS ((btScalar)0.01) + + +// Shorthands +typedef unsigned int U; +typedef unsigned char U1; + +// MinkowskiDiff +struct MinkowskiDiff + { + const btConvexShape* m_shapes[2]; + btMatrix3x3 m_toshape1; + btTransform m_toshape0; + btVector3 (btConvexShape::*Ls)(const btVector3&) const; + void EnableMargin(bool enable) + { + if(enable) + Ls=&btConvexShape::localGetSupportingVertex; + else + Ls=&btConvexShape::localGetSupportingVertexWithoutMargin; + } + inline btVector3 Support0(const btVector3& d) const + { + return(((m_shapes[0])->*(Ls))(d)); + } + inline btVector3 Support1(const btVector3& d) const + { + return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); + } + inline btVector3 Support(const btVector3& d) const + { + return(Support0(d)-Support1(-d)); + } + btVector3 Support(const btVector3& d,U index) const + { + if(index) + return(Support1(d)); + else + return(Support0(d)); + } + }; + +typedef MinkowskiDiff tShape; + + +// GJK +struct GJK +{ +/* Types */ +struct sSV + { + btVector3 d,w; + }; +struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; +struct eStatus { enum _ { + Valid, + Inside, + Failed };}; +/* Fields */ +tShape m_shape; +btVector3 m_ray; +btScalar m_distance; +sSimplex m_simplices[2]; +sSV m_store[4]; +sSV* m_free[4]; +U m_nfree; +U m_current; +sSimplex* m_simplex; +eStatus::_ m_status; +/* Methods */ + GJK() + { + Initialize(); + } +void Initialize() + { + m_ray = btVector3(0,0,0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; + } +eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) + { + U iterations=0; + btScalar sqdist=0; + btScalar alpha=0; + btVector3 lastw[4]; + U clastw=0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eStatus::Valid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const btScalar sqrl= m_ray.length2(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const U next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const btScalar rl=m_ray.length(); + if(rlw; + bool found=false; + for(U i=0;i<4;++i) + { + if((w-lastw[i]).length2()w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; + } + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = btVector3(0,0,0); + m_current = next; + for(U i=0,ni=cs.rank;iw*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eStatus::Inside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)rank) + { + case 1: + { + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + appendvertice(*m_simplex, axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + const btVector3 p=cross(d,axis); + if(p.length2()>0) + { + appendvertice(*m_simplex, p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + m_simplex->c[2]->w-m_simplex->c[0]->w); + if(n.length2()>0) + { + appendvertice(*m_simplex,n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 4: + { + if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, + m_simplex->c[1]->w-m_simplex->c[3]->w, + m_simplex->c[2]->w-m_simplex->c[3]->w))>0) + return(true); + } + break; + } + return(false); + } +/* Internals */ +void getsupport(const btVector3& d,sSV& sv) const + { + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d); + } +void removevertice(sSimplex& simplex) + { + m_free[m_nfree++]=simplex.c[--simplex.rank]; + } +void appendvertice(sSimplex& simplex,const btVector3& v) + { + simplex.p[simplex.rank]=0; + simplex.c[simplex.rank]=m_free[--m_nfree]; + getsupport(v,*simplex.c[simplex.rank++]); + } +static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) + { + return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- + a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ + a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); + } +static btScalar projectorigin( const btVector3& a, + const btVector3& b, + btScalar* w,U& m) + { + const btVector3 d=b-a; + const btScalar l=d.length2(); + if(l>GJK_SIMPLEX2_EPS) + { + const btScalar t(l>0?-dot(a,d)/l:0); + if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } + else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } + else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } + } + return(-1); + } +static btScalar projectorigin( const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w,U& m) + { + static const U imd3[]={1,2,0}; + const btVector3* vt[]={&a,&b,&c}; + const btVector3 dl[]={a-b,b-c,c-a}; + const btVector3 n=cross(dl[0],dl[1]); + const btScalar l=n.length2(); + if(l>GJK_SIMPLEX3_EPS) + { + btScalar mindist=-1; + btScalar subw[2]; + U subm; + for(U i=0;i<3;++i) + { + if(dot(*vt[i],cross(dl[i],n))>0) + { + const U j=imd3[i]; + const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subd(((subm&1)?1<GJK_SIMPLEX4_EPS)) + { + btScalar mindist=-1; + btScalar subw[3]; + U subm; + for(U i=0;i<3;++i) + { + const U j=imd3[i]; + const btScalar s=vl*dot(d,cross(dl[i],dl[j])); + if(s>0) + { + const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd((subm&1?1<e[ea]=(U1)eb;fa->f[ea]=fb; + fb->e[eb]=(U1)ea;fb->f[eb]=fa; + } +static inline void append(sList& list,sFace* face) + { + face->l[0] = 0; + face->l[1] = list.root; + if(list.root) list.root->l[0]=face; + list.root = face; + ++list.count; + } +static inline void remove(sList& list,sFace* face) + { + if(face->l[1]) face->l[1]->l[0]=face->l[0]; + if(face->l[0]) face->l[0]->l[1]=face->l[1]; + if(face==list.root) list.root=face->l[1]; + --list.count; + } + + +void Initialize() + { + m_status = eStatus::Failed; + m_normal = btVector3(0,0,0); + m_depth = 0; + m_nextsv = 0; + for(U i=0;i1)&&gjk.EncloseOrigin()) + { + + /* Clean up */ + while(m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull,f); + append(m_stock,f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + btSwap(simplex.c[0],simplex.c[1]); + btSwap(simplex.p[0],simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), + newface(simplex.c[1],simplex.c[0],simplex.c[3],true), + newface(simplex.c[2],simplex.c[1],simplex.c[3],true), + newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; + if(m_hull.count==4) + { + sFace* best=findbest(); + sFace outer=*best; + U pass=0; + U iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eStatus::Valid; + for(;iterationspass = (U1)(++pass); + gjk.getsupport(best->n,*w); + const btScalar wdist=dot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(U j=0;(j<3)&&valid;++j) + { + valid&=expand( pass,w, + best->f[j],best->e[j], + horizon); + } + if(valid&&(horizon.nf>=3)) + { + bind(horizon.cf,1,horizon.ff,2); + remove(m_hull,best); + append(m_stock,best); + best=findbest(); + if(best->p>=outer.p) outer=*best; + } else { m_status=eStatus::InvalidHull;break; } + } else { m_status=eStatus::AccuraryReached;break; } + } else { m_status=eStatus::OutOfVertices;break; } + } + const btVector3 projection=outer.n*outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = cross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = cross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = cross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return(m_status); + } + } + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const btScalar nl=m_normal.length(); + if(nl>0) + m_normal = m_normal/nl; + else + m_normal = btVector3(1,0,0); + m_depth = 0; + m_result.rank=1; + m_result.c[0]=simplex.c[0]; + m_result.p[0]=1; + return(m_status); + } +sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) + { + if(m_stock.root) + { + sFace* face=m_stock.root; + remove(m_stock,face); + append(m_hull,face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = cross(b->w-a->w,c->w-a->w); + const btScalar l=face->n.length(); + const bool v=l>EPA_ACCURACY; + face->p = btMin(btMin( + dot(a->w,cross(face->n,a->w-b->w)), + dot(b->w,cross(face->n,b->w-c->w))), + dot(c->w,cross(face->n,c->w-a->w))) / + (v?l:1); + face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; + if(v) + { + face->d = dot(a->w,face->n)/l; + face->n /= l; + if(forced||(face->d>=-EPA_PLANE_EPS)) + { + return(face); + } else m_status=eStatus::NonConvex; + } else m_status=eStatus::Degenerated; + remove(m_hull,face); + append(m_stock,face); + return(0); + } + m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; + return(0); + } +sFace* findbest() + { + sFace* minf=m_hull.root; + btScalar mind=minf->d*minf->d; + btScalar maxp=minf->p; + for(sFace* f=minf->l[1];f;f=f->l[1]) + { + const btScalar sqd=f->d*f->d; + if((f->p>=maxp)&&(sqdp; + } + } + return(minf); + } +bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) + { + static const U i1m3[]={1,2,0}; + static const U i2m3[]={2,0,1}; + if(f->pass!=pass) + { + const U e1=i1m3[e]; + if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + { + sFace* nf=newface(f->c[e1],f->c[e],w,false); + if(nf) + { + bind(nf,0,f,e); + if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; + horizon.cf=nf; + ++horizon.nf; + return(true); + } + } + else + { + const U e2=i2m3[e]; + f->pass = (U1)pass; + if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& + expand(pass,w,f->f[e2],f->e[e2],horizon)) + { + remove(m_hull,f); + append(m_stock,f); + return(true); + } + } + } + return(false); + } + +}; + +// +static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + btGjkEpaSolver2::sResults& results, + tShape& shape, + bool withmargins) +{ +/* Results */ +results.witnesses[0] = +results.witnesses[1] = btVector3(0,0,0); +results.status = btGjkEpaSolver2::sResults::Separated; +/* Shape */ +shape.m_shapes[0] = shape0; +shape.m_shapes[1] = shape1; +shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); +shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); +shape.EnableMargin(withmargins); +} + +} + +// +// Api +// + +using namespace gjkepa2_impl; + +// +int btGjkEpaSolver2::StackSizeRequirement() +{ +return(sizeof(GJK)+sizeof(EPA)); +} + +// +bool btGjkEpaSolver2::Distance( const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) +{ +tShape shape; +Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); +GJK gjk; +GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); +if(gjk_status==GJK::eStatus::Valid) + { + btVector3 w0=btVector3(0,0,0); + btVector3 w1=btVector3(0,0,0); + for(U i=0;irank;++i) + { + const btScalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*w1; + results.normal = w0-w1; + results.distance = results.normal.length(); + results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; + return(true); + } + else + { + results.status = gjk_status==GJK::eStatus::Inside? + sResults::Penetrating : + sResults::GJK_Failed ; + return(false); + } +} + +// +bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results, + bool usemargins) +{ +tShape shape; +Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins); +GJK gjk; +GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); +switch(gjk_status) + { + case GJK::eStatus::Inside: + { + EPA epa; + EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=EPA::eStatus::Failed) + { + btVector3 w0=btVector3(0,0,0); + for(U i=0;id,0)*epa.m_result.p[i]; + } + results.status = sResults::Penetrating; + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return(true); + } else results.status=sResults::EPA_Failed; + } + break; + case GJK::eStatus::Failed: + results.status=sResults::GJK_Failed; + break; + } +return(false); +} + +// +btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, + btScalar margin, + const btConvexShape* shape0, + const btTransform& wtrs0, + sResults& results) +{ +tShape shape; +btSphereShape shape1(margin); +btTransform wtrs1(btQuaternion(0,0,0,1),position); +Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); +GJK gjk; +GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); +if(gjk_status==GJK::eStatus::Valid) + { + btVector3 w0=btVector3(0,0,0); + btVector3 w1=btVector3(0,0,0); + for(U i=0;irank;++i) + { + const btScalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*w1; + const btVector3 delta= results.witnesses[1]- + results.witnesses[0]; + const btScalar margin= shape0->getMargin()+ + shape1.getMargin(); + const btScalar length= delta.length(); + results.normal = delta/length; + results.witnesses[0] += results.normal*margin; + return(length-margin); + } + else + { + if(gjk_status==GJK::eStatus::Inside) + { + if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) + { + const btVector3 delta= results.witnesses[0]- + results.witnesses[1]; + const btScalar length= delta.length(); + if (length >= SIMD_EPSILON) + results.normal = delta/length; + return(-length); + } + } + } +return(SIMD_INFINITY); +} + +// +bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) +{ +if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) + return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); + else + return(true); +} + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURARY +#undef GJK_MIN_DISTANCE +#undef GJK_DUPLICATED_EPS +#undef GJK_SIMPLEX2_EPS +#undef GJK_SIMPLEX3_EPS +#undef GJK_SIMPLEX4_EPS + +#undef EPA_MAX_VERTICES +#undef EPA_MAX_FACES +#undef EPA_MAX_ITERATIONS +#undef EPA_ACCURACY +#undef EPA_FALLBACK +#undef EPA_PLANE_EPS +#undef EPA_INSIDE_EPS diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h new file mode 100644 index 00000000000..a55214203d3 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h @@ -0,0 +1,71 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the +use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not +claim that you wrote the original software. If you use this software in a +product, an acknowledgment in the product documentation would be appreciated +but is not required. +2. Altered source versions must be plainly marked as such, and must not be +misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson, 2008 +*/ +#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ +#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ +#include "BulletCollision/CollisionShapes/btConvexShape.h" + +///btGjkEpaSolver contributed under zlib by Nathanael Presson +struct btGjkEpaSolver2 +{ +struct sResults + { + enum eStatus + { + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + btVector3 witnesses[2]; + btVector3 normal; + btScalar distance; + }; + +static int StackSizeRequirement(); + +static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + const btVector3& guess, + sResults& results); + +static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + const btVector3& guess, + sResults& results, + bool usemargins=true); + +static btScalar SignedDistance( const btVector3& position, + btScalar margin, + const btConvexShape* shape, + const btTransform& wtrs, + sResults& results); + +static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + const btVector3& guess, + sResults& results); +}; + +#endif diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp new file mode 100644 index 00000000000..4128f504bf1 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -0,0 +1,415 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 +*/ + +//----------------------------------------------------------------------------- + +#include "btSliderConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include + +//----------------------------------------------------------------------------- + +void btSliderConstraint::initParams() +{ + m_lowerLinLimit = btScalar(1.0); + m_upperLinLimit = btScalar(-1.0); + m_lowerAngLimit = btScalar(0.); + m_upperAngLimit = btScalar(0.); + m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingDirLin = btScalar(0.); + m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingDirAng = btScalar(0.); + m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; + + m_poweredLinMotor = false; + m_targetLinMotorVelocity = btScalar(0.); + m_maxLinMotorForce = btScalar(0.); + m_accumulatedLinMotorImpulse = btScalar(0.0); + + m_poweredAngMotor = false; + m_targetAngMotorVelocity = btScalar(0.); + m_maxAngMotorForce = btScalar(0.); + m_accumulatedAngMotorImpulse = btScalar(0.0); + +} // btSliderConstraint::initParams() + +//----------------------------------------------------------------------------- + +btSliderConstraint::btSliderConstraint() + :btTypedConstraint(SLIDER_CONSTRAINT_TYPE), + m_useLinearReferenceFrameA(true) +{ + initParams(); +} // btSliderConstraint::btSliderConstraint() + +//----------------------------------------------------------------------------- + +btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) + : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB) + , m_frameInA(frameInA) + , m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) +{ + initParams(); +} // btSliderConstraint::btSliderConstraint() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::buildJacobian() +{ + if(m_useLinearReferenceFrameA) + { + buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB); + } + else + { + buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA); + } +} // btSliderConstraint::buildJacobian() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) +{ + //calculate transforms + m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA; + m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB; + m_realPivotAInW = m_calculatedTransformA.getOrigin(); + m_realPivotBInW = m_calculatedTransformB.getOrigin(); + m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X + m_delta = m_realPivotBInW - m_realPivotAInW; + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition(); + m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition(); + btVector3 normalWorld; + int i; + //linear part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + new (&m_jacLin[i]) btJacobianEntry( + rbA.getCenterOfMassTransform().getBasis().transpose(), + rbB.getCenterOfMassTransform().getBasis().transpose(), + m_relPosA, + m_relPosB, + normalWorld, + rbA.getInvInertiaDiagLocal(), + rbA.getInvMass(), + rbB.getInvInertiaDiagLocal(), + rbB.getInvMass() + ); + m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal(); + m_depth[i] = m_delta.dot(normalWorld); + } + testLinLimits(); + // angular part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + new (&m_jacAng[i]) btJacobianEntry( + normalWorld, + rbA.getCenterOfMassTransform().getBasis().transpose(), + rbB.getCenterOfMassTransform().getBasis().transpose(), + rbA.getInvInertiaDiagLocal(), + rbB.getInvInertiaDiagLocal() + ); + } + testAngLimits(); + btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); + m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA)); + // clear accumulator for motors + m_accumulatedLinMotorImpulse = btScalar(0.0); + m_accumulatedAngMotorImpulse = btScalar(0.0); +} // btSliderConstraint::buildJacobianInt() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::solveConstraint(btScalar timeStep) +{ + m_timeStep = timeStep; + if(m_useLinearReferenceFrameA) + { + solveConstraintInt(m_rbA, m_rbB); + } + else + { + solveConstraintInt(m_rbB, m_rbA); + } +} // btSliderConstraint::solveConstraint() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) +{ + int i; + // linear + btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA); + btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB); + btVector3 vel = velA - velB; + for(i = 0; i < 3; i++) + { + const btVector3& normal = m_jacLin[i].m_linearJointAxis; + btScalar rel_vel = normal.dot(vel); + // calculate positional error + btScalar depth = m_depth[i]; + // get parameters + btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); + btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); + btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); + // calcutate and apply impulse + btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; + btVector3 impulse_vector = normal * normalImpulse; + rbA.applyImpulse( impulse_vector, m_relPosA); + rbB.applyImpulse(-impulse_vector, m_relPosB); + if(m_poweredLinMotor && (!i)) + { // apply linear motor + if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) + { + btScalar desiredMotorVel = m_targetLinMotorVelocity; + btScalar motor_relvel = desiredMotorVel + rel_vel; + normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; + // clamp accumulated impulse + btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse); + if(new_acc > m_maxLinMotorForce) + { + new_acc = m_maxLinMotorForce; + } + btScalar del = new_acc - m_accumulatedLinMotorImpulse; + if(normalImpulse < btScalar(0.0)) + { + normalImpulse = -del; + } + else + { + normalImpulse = del; + } + m_accumulatedLinMotorImpulse = new_acc; + // apply clamped impulse + impulse_vector = normal * normalImpulse; + rbA.applyImpulse( impulse_vector, m_relPosA); + rbB.applyImpulse(-impulse_vector, m_relPosB); + } + } + } + // angular + // get axes in world space + btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); + btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); + + const btVector3& angVelA = rbA.getAngularVelocity(); + const btVector3& angVelB = rbB.getAngularVelocity(); + + btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); + btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); + + btVector3 angAorthog = angVelA - angVelAroundAxisA; + btVector3 angBorthog = angVelB - angVelAroundAxisB; + btVector3 velrelOrthog = angAorthog-angBorthog; + //solve orthogonal angular velocity correction + btScalar len = velrelOrthog.length(); + if (len > btScalar(0.00001)) + { + btVector3 normal = velrelOrthog.normalized(); + btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); + velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + } + //solve angular positional correction + btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); + btScalar len2 = angularError.length(); + if (len2>btScalar(0.00001)) + { + btVector3 normal2 = angularError.normalized(); + btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); + angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + } + // apply impulse + rbA.applyTorqueImpulse(-velrelOrthog+angularError); + rbB.applyTorqueImpulse(velrelOrthog-angularError); + btScalar impulseMag; + //solve angular limits + if(m_solveAngLim) + { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep; + impulseMag *= m_kAngle * m_softnessLimAng; + } + else + { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep; + impulseMag *= m_kAngle * m_softnessDirAng; + } + btVector3 impulse = axisA * impulseMag; + rbA.applyTorqueImpulse(impulse); + rbB.applyTorqueImpulse(-impulse); + //apply angular motor + if(m_poweredAngMotor) + { + if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce) + { + btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB; + btScalar projRelVel = velrel.dot(axisA); + + btScalar desiredMotorVel = m_targetAngMotorVelocity; + btScalar motor_relvel = desiredMotorVel - projRelVel; + + btScalar angImpulse = m_kAngle * motor_relvel; + // clamp accumulated impulse + btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse); + if(new_acc > m_maxAngMotorForce) + { + new_acc = m_maxAngMotorForce; + } + btScalar del = new_acc - m_accumulatedAngMotorImpulse; + if(angImpulse < btScalar(0.0)) + { + angImpulse = -del; + } + else + { + angImpulse = del; + } + m_accumulatedAngMotorImpulse = new_acc; + // apply clamped impulse + btVector3 motorImp = angImpulse * axisA; + m_rbA.applyTorqueImpulse(motorImp); + m_rbB.applyTorqueImpulse(-motorImp); + } + } +} // btSliderConstraint::solveConstraint() + +//----------------------------------------------------------------------------- + +//----------------------------------------------------------------------------- + +void btSliderConstraint::calculateTransforms(void){ + if(m_useLinearReferenceFrameA) + { + m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; + m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; + } + else + { + m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB; + m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA; + } + m_realPivotAInW = m_calculatedTransformA.getOrigin(); + m_realPivotBInW = m_calculatedTransformB.getOrigin(); + m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X + m_delta = m_realPivotBInW - m_realPivotAInW; + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + btVector3 normalWorld; + int i; + //linear part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + m_depth[i] = m_delta.dot(normalWorld); + } +} // btSliderConstraint::calculateTransforms() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::testLinLimits(void) +{ + m_solveLinLim = false; + m_linPos = m_depth[0]; + if(m_lowerLinLimit <= m_upperLinLimit) + { + if(m_depth[0] > m_upperLinLimit) + { + m_depth[0] -= m_upperLinLimit; + m_solveLinLim = true; + } + else if(m_depth[0] < m_lowerLinLimit) + { + m_depth[0] -= m_lowerLinLimit; + m_solveLinLim = true; + } + else + { + m_depth[0] = btScalar(0.); + } + } + else + { + m_depth[0] = btScalar(0.); + } +} // btSliderConstraint::testLinLimits() + +//----------------------------------------------------------------------------- + + +void btSliderConstraint::testAngLimits(void) +{ + m_angDepth = btScalar(0.); + m_solveAngLim = false; + if(m_lowerAngLimit <= m_upperAngLimit) + { + const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1); + const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); + const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); + btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); + if(rot < m_lowerAngLimit) + { + m_angDepth = rot - m_lowerAngLimit; + m_solveAngLim = true; + } + else if(rot > m_upperAngLimit) + { + m_angDepth = rot - m_upperAngLimit; + m_solveAngLim = true; + } + } +} // btSliderConstraint::testAngLimits() + + +//----------------------------------------------------------------------------- + + + +btVector3 btSliderConstraint::getAncorInA(void) +{ + btVector3 ancorInA; + ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis; + ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA; + return ancorInA; +} // btSliderConstraint::getAncorInA() + +//----------------------------------------------------------------------------- + +btVector3 btSliderConstraint::getAncorInB(void) +{ + btVector3 ancorInB; + ancorInB = m_frameInB.getOrigin(); + return ancorInB; +} // btSliderConstraint::getAncorInB(); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h new file mode 100644 index 00000000000..580dfa1178d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -0,0 +1,218 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 + +TODO: + - add clamping od accumulated impulse to improve stability + - add conversion for ODE constraint solver +*/ + +#ifndef SLIDER_CONSTRAINT_H +#define SLIDER_CONSTRAINT_H + +//----------------------------------------------------------------------------- + +#include "LinearMath/btVector3.h" +#include "btJacobianEntry.h" +#include "btTypedConstraint.h" + +//----------------------------------------------------------------------------- + +class btRigidBody; + +//----------------------------------------------------------------------------- + +#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0)) +#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0)) +#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7)) + +//----------------------------------------------------------------------------- + +class btSliderConstraint : public btTypedConstraint +{ +protected: + btTransform m_frameInA; + btTransform m_frameInB; + // use frameA fo define limits, if true + bool m_useLinearReferenceFrameA; + // linear limits + btScalar m_lowerLinLimit; + btScalar m_upperLinLimit; + // angular limits + btScalar m_lowerAngLimit; + btScalar m_upperAngLimit; + // softness, restitution and damping for different cases + // DirLin - moving inside linear limits + // LimLin - hitting linear limit + // DirAng - moving inside angular limits + // LimAng - hitting angular limit + // OrthoLin, OrthoAng - against constraint axis + btScalar m_softnessDirLin; + btScalar m_restitutionDirLin; + btScalar m_dampingDirLin; + btScalar m_softnessDirAng; + btScalar m_restitutionDirAng; + btScalar m_dampingDirAng; + btScalar m_softnessLimLin; + btScalar m_restitutionLimLin; + btScalar m_dampingLimLin; + btScalar m_softnessLimAng; + btScalar m_restitutionLimAng; + btScalar m_dampingLimAng; + btScalar m_softnessOrthoLin; + btScalar m_restitutionOrthoLin; + btScalar m_dampingOrthoLin; + btScalar m_softnessOrthoAng; + btScalar m_restitutionOrthoAng; + btScalar m_dampingOrthoAng; + + // for interlal use + bool m_solveLinLim; + bool m_solveAngLim; + + btJacobianEntry m_jacLin[3]; + btScalar m_jacLinDiagABInv[3]; + + btJacobianEntry m_jacAng[3]; + + btScalar m_timeStep; + btTransform m_calculatedTransformA; + btTransform m_calculatedTransformB; + + btVector3 m_sliderAxis; + btVector3 m_realPivotAInW; + btVector3 m_realPivotBInW; + btVector3 m_projPivotInW; + btVector3 m_delta; + btVector3 m_depth; + btVector3 m_relPosA; + btVector3 m_relPosB; + + btScalar m_linPos; + + btScalar m_angDepth; + btScalar m_kAngle; + + bool m_poweredLinMotor; + btScalar m_targetLinMotorVelocity; + btScalar m_maxLinMotorForce; + btScalar m_accumulatedLinMotorImpulse; + + bool m_poweredAngMotor; + btScalar m_targetAngMotorVelocity; + btScalar m_maxAngMotorForce; + btScalar m_accumulatedAngMotorImpulse; + + //------------------------ + void initParams(); +public: + // constructors + btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); + btSliderConstraint(); + // overrides + virtual void buildJacobian(); + virtual void solveConstraint(btScalar timeStep); + // access + const btRigidBody& getRigidBodyA() const { return m_rbA; } + const btRigidBody& getRigidBodyB() const { return m_rbB; } + const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } + const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; } + const btTransform & getFrameOffsetA() const { return m_frameInA; } + const btTransform & getFrameOffsetB() const { return m_frameInB; } + btTransform & getFrameOffsetA() { return m_frameInA; } + btTransform & getFrameOffsetB() { return m_frameInB; } + btScalar getLowerLinLimit() { return m_lowerLinLimit; } + void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; } + btScalar getUpperLinLimit() { return m_upperLinLimit; } + void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } + btScalar getLowerAngLimit() { return m_lowerAngLimit; } + void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; } + btScalar getUpperAngLimit() { return m_upperAngLimit; } + void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; } + bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } + btScalar getSoftnessDirLin() { return m_softnessDirLin; } + btScalar getRestitutionDirLin() { return m_restitutionDirLin; } + btScalar getDampingDirLin() { return m_dampingDirLin ; } + btScalar getSoftnessDirAng() { return m_softnessDirAng; } + btScalar getRestitutionDirAng() { return m_restitutionDirAng; } + btScalar getDampingDirAng() { return m_dampingDirAng; } + btScalar getSoftnessLimLin() { return m_softnessLimLin; } + btScalar getRestitutionLimLin() { return m_restitutionLimLin; } + btScalar getDampingLimLin() { return m_dampingLimLin; } + btScalar getSoftnessLimAng() { return m_softnessLimAng; } + btScalar getRestitutionLimAng() { return m_restitutionLimAng; } + btScalar getDampingLimAng() { return m_dampingLimAng; } + btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; } + btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; } + btScalar getDampingOrthoLin() { return m_dampingOrthoLin; } + btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; } + btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; } + btScalar getDampingOrthoAng() { return m_dampingOrthoAng; } + void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; } + void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } + void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; } + void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; } + void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } + void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; } + void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; } + void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } + void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; } + void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; } + void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } + void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; } + void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } + void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } + void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } + void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } + void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } + void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } + void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } + bool getPoweredLinMotor() { return m_poweredLinMotor; } + void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } + btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } + void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } + btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; } + void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } + bool getPoweredAngMotor() { return m_poweredAngMotor; } + void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } + btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } + void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } + btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } + btScalar getLinearPos() { return m_linPos; } + + // access for ODE solver + bool getSolveLinLimit() { return m_solveLinLim; } + btScalar getLinDepth() { return m_depth[0]; } + bool getSolveAngLimit() { return m_solveAngLim; } + btScalar getAngDepth() { return m_angDepth; } + // internal + void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB); + void solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB); + // shared code used by ODE solver + void calculateTransforms(void); + void testLinLimits(void); + void testAngLimits(void); + // access for PE Solver + btVector3 getAncorInA(void); + btVector3 getAncorInB(void); +}; + +//----------------------------------------------------------------------------- + +#endif //SLIDER_CONSTRAINT_H + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp new file mode 100644 index 00000000000..19443adc723 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp @@ -0,0 +1,193 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btContinuousDynamicsWorld.h" +#include "LinearMath/btQuickprof.h" + +//collision detection +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" + +//rigidbody & constraints +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" + + + +#include + +btContinuousDynamicsWorld::btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) +:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) +{ +} + +btContinuousDynamicsWorld::~btContinuousDynamicsWorld() +{ +} + + +void btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) +{ + + startProfiling(timeStep); + + + ///update aabbs information + updateAabbs(); + //static int frame=0; +// printf("frame %d\n",frame++); + + ///apply gravity, predict motion + predictUnconstraintMotion(timeStep); + + btDispatcherInfo& dispatchInfo = getDispatchInfo(); + + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = getDebugDrawer(); + + ///perform collision detection + performDiscreteCollisionDetection(); + + calculateSimulationIslands(); + + + getSolverInfo().m_timeStep = timeStep; + + + + ///solve contact and other joint constraints + solveConstraints(getSolverInfo()); + + ///CallbackTriggers(); + calculateTimeOfImpacts(timeStep); + + btScalar toi = dispatchInfo.m_timeOfImpact; +// if (toi < 1.f) +// printf("toi = %f\n",toi); + if (toi < 0.f) + printf("toi = %f\n",toi); + + + ///integrate transforms + integrateTransforms(timeStep * toi); + + ///update vehicle simulation + updateVehicles(timeStep); + + + updateActivationState( timeStep ); + + if(0 != m_internalTickCallback) { + (*m_internalTickCallback)(this, timeStep); + } +} + +void btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep) +{ + ///these should be 'temporal' aabbs! + updateTemporalAabbs(timeStep); + + ///'toi' is the global smallest time of impact. However, we just calculate the time of impact for each object individually. + ///so we handle the case moving versus static properly, and we cheat for moving versus moving + btScalar toi = 1.f; + + + btDispatcherInfo& dispatchInfo = getDispatchInfo(); + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_timeOfImpact = 1.f; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS; + + ///calculate time of impact for overlapping pairs + + + btDispatcher* dispatcher = getDispatcher(); + if (dispatcher) + dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); + + toi = dispatchInfo.m_timeOfImpact; + + dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_DISCRETE; + +} + +void btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep) +{ + + btVector3 temporalAabbMin,temporalAabbMax; + + for ( int i=0;igetCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),temporalAabbMin,temporalAabbMax); + const btVector3& linvel = body->getLinearVelocity(); + + //make the AABB temporal + btScalar temporalAabbMaxx = temporalAabbMax.getX(); + btScalar temporalAabbMaxy = temporalAabbMax.getY(); + btScalar temporalAabbMaxz = temporalAabbMax.getZ(); + btScalar temporalAabbMinx = temporalAabbMin.getX(); + btScalar temporalAabbMiny = temporalAabbMin.getY(); + btScalar temporalAabbMinz = temporalAabbMin.getZ(); + + // add linear motion + btVector3 linMotion = linvel*timeStep; + + if (linMotion.x() > 0.f) + temporalAabbMaxx += linMotion.x(); + else + temporalAabbMinx += linMotion.x(); + if (linMotion.y() > 0.f) + temporalAabbMaxy += linMotion.y(); + else + temporalAabbMiny += linMotion.y(); + if (linMotion.z() > 0.f) + temporalAabbMaxz += linMotion.z(); + else + temporalAabbMinz += linMotion.z(); + + //add conservative angular motion + btScalar angularMotion(0);// = angvel.length() * GetAngularMotionDisc() * timeStep; + btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion); + temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); + temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); + + temporalAabbMin -= angularMotion3d; + temporalAabbMax += angularMotion3d; + + m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax,m_dispatcher1); + } + } + + //update aabb (of all moved objects) + + m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); + + + +} + + + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h new file mode 100644 index 00000000000..61c8dea03eb --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h @@ -0,0 +1,46 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_CONTINUOUS_DYNAMICS_WORLD_H +#define BT_CONTINUOUS_DYNAMICS_WORLD_H + +#include "btDiscreteDynamicsWorld.h" + +///btContinuousDynamicsWorld adds optional (per object) continuous collision detection for fast moving objects to the btDiscreteDynamicsWorld. +///This copes with fast moving objects that otherwise would tunnel/miss collisions. +///Under construction, don't use yet! Please use btDiscreteDynamicsWorld instead. +class btContinuousDynamicsWorld : public btDiscreteDynamicsWorld +{ + + void updateTemporalAabbs(btScalar timeStep); + + public: + + btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + virtual ~btContinuousDynamicsWorld(); + + ///time stepping with calculation of time of impact for selected fast moving objects + virtual void internalSingleStepSimulation( btScalar timeStep); + + virtual void calculateTimeOfImpacts(btScalar timeStep); + + virtual btDynamicsWorldType getWorldType() const + { + return BT_CONTINUOUS_DYNAMICS_WORLD; + } + +}; + +#endif //BT_CONTINUOUS_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/LinearMath/btConvexHull.cpp b/extern/bullet2/src/LinearMath/btConvexHull.cpp new file mode 100644 index 00000000000..b8929a86808 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btConvexHull.cpp @@ -0,0 +1,1174 @@ +/* +Stan Melax Convex Hull Computation +Copyright (c) 2003-2006 Stan Melax http://www.melax.com/ + +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 + +#include "btConvexHull.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btMinMax.h" +#include "LinearMath/btVector3.h" + + + +template +void Swap(T &a,T &b) +{ + T tmp = a; + a=b; + b=tmp; +} + + +//---------------------------------- + +class int3 +{ +public: + int x,y,z; + int3(){}; + int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;} + const int& operator[](int i) const {return (&x)[i];} + int& operator[](int i) {return (&x)[i];} +}; + + +//------- btPlane ---------- + + +inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);} +inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); } +inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); } + + +//--------- Utility Functions ------ + +btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1); +btVector3 PlaneProject(const btPlane &plane, const btVector3 &point); + +btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2); +btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2) +{ + btVector3 N1 = p0.normal; + btVector3 N2 = p1.normal; + btVector3 N3 = p2.normal; + + btVector3 n2n3; n2n3 = N2.cross(N3); + btVector3 n3n1; n3n1 = N3.cross(N1); + btVector3 n1n2; n1n2 = N1.cross(N2); + + btScalar quotient = (N1.dot(n2n3)); + + btAssert(btFabs(quotient) > btScalar(0.000001)); + + quotient = btScalar(-1.) / quotient; + n2n3 *= p0.dist; + n3n1 *= p1.dist; + n1n2 *= p2.dist; + btVector3 potentialVertex = n2n3; + potentialVertex += n3n1; + potentialVertex += n1n2; + potentialVertex *= quotient; + + btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ()); + return result; + +} + +btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL); +btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2); +btVector3 NormalOf(const btVector3 *vert, const int n); + + +btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1) +{ + // returns the point where the line p0-p1 intersects the plane n&d + static btVector3 dif; + dif = p1-p0; + btScalar dn= dot(plane.normal,dif); + btScalar t = -(plane.dist+dot(plane.normal,p0) )/dn; + return p0 + (dif*t); +} + +btVector3 PlaneProject(const btPlane &plane, const btVector3 &point) +{ + return point - plane.normal * (dot(point,plane.normal)+plane.dist); +} + +btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) +{ + // return the normal of the triangle + // inscribed by v0, v1, and v2 + btVector3 cp=cross(v1-v0,v2-v1); + btScalar m=cp.length(); + if(m==0) return btVector3(1,0,0); + return cp*(btScalar(1.0)/m); +} + + +btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint) +{ + static btVector3 cp; + cp = cross(udir,vdir).normalized(); + + btScalar distu = -dot(cp,ustart); + btScalar distv = -dot(cp,vstart); + btScalar dist = (btScalar)fabs(distu-distv); + if(upoint) + { + btPlane plane; + plane.normal = cross(vdir,cp).normalized(); + plane.dist = -dot(plane.normal,vstart); + *upoint = PlaneLineIntersection(plane,ustart,ustart+udir); + } + if(vpoint) + { + btPlane plane; + plane.normal = cross(udir,cp).normalized(); + plane.dist = -dot(plane.normal,ustart); + *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); + } + return dist; +} + + + + + + + +#define COPLANAR (0) +#define UNDER (1) +#define OVER (2) +#define SPLIT (OVER|UNDER) +#define PAPERWIDTH (btScalar(0.001)) + +btScalar planetestepsilon = PAPERWIDTH; + + + +typedef ConvexH::HalfEdge HalfEdge; + +ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size) +{ + vertices.resize(vertices_size); + edges.resize(edges_size); + facets.resize(facets_size); +} + + +int PlaneTest(const btPlane &p, const btVector3 &v); +int PlaneTest(const btPlane &p, const btVector3 &v) { + btScalar a = dot(v,p.normal)+p.dist; + int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR); + return flag; +} + +int SplitTest(ConvexH &convex,const btPlane &plane); +int SplitTest(ConvexH &convex,const btPlane &plane) { + int flag=0; + for(int i=0;i +int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray &allow) +{ + btAssert(count); + int m=-1; + for(int i=0;idot(p[m],dir)) + m=i; + } + btAssert(m!=-1); + return m; +} + +btVector3 orth(const btVector3 &v); +btVector3 orth(const btVector3 &v) +{ + btVector3 a=cross(v,btVector3(0,0,1)); + btVector3 b=cross(v,btVector3(0,1,0)); + if (a.length() > b.length()) + { + return a.normalized(); + } else { + return b.normalized(); + } +} + + +template +int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray &allow) +{ + int m=-1; + while(m==-1) + { + m = maxdirfiltered(p,count,dir,allow); + if(allow[m]==3) return m; + T u = orth(dir); + T v = cross(u,dir); + int ma=-1; + for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0)) + { + btScalar s = sinf(SIMD_RADS_PER_DEG*(x)); + btScalar c = cosf(SIMD_RADS_PER_DEG*(x)); + int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); + if(ma==m && mb==m) + { + allow[m]=3; + return m; + } + if(ma!=-1 && ma!=mb) // Yuck - this is really ugly + { + int mc = ma; + for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0)) + { + btScalar s = sinf(SIMD_RADS_PER_DEG*(xx)); + btScalar c = cosf(SIMD_RADS_PER_DEG*(xx)); + int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); + if(mc==m && md==m) + { + allow[m]=3; + return m; + } + mc=md; + } + } + ma=mb; + } + allow[m]=0; + m=-1; + } + btAssert(0); + return m; +} + + + + +int operator ==(const int3 &a,const int3 &b); +int operator ==(const int3 &a,const int3 &b) +{ + for(int i=0;i<3;i++) + { + if(a[i]!=b[i]) return 0; + } + return 1; +} + + +int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon); +int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) +{ + btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); + return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? +} +int hasedge(const int3 &t, int a,int b); +int hasedge(const int3 &t, int a,int b) +{ + for(int i=0;i<3;i++) + { + int i1= (i+1)%3; + if(t[i]==a && t[i1]==b) return 1; + } + return 0; +} +int hasvert(const int3 &t, int v); +int hasvert(const int3 &t, int v) +{ + return (t[0]==v || t[1]==v || t[2]==v) ; +} +int shareedge(const int3 &a,const int3 &b); +int shareedge(const int3 &a,const int3 &b) +{ + int i; + for(i=0;i<3;i++) + { + int i1= (i+1)%3; + if(hasedge(a,b[i1],b[i])) return 1; + } + return 0; +} + +class Tri; + + + +class Tri : public int3 +{ +public: + int3 n; + int id; + int vmax; + btScalar rise; + Tri(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) + { + vmax=-1; + rise = btScalar(0.0); + } + ~Tri() + { + } + int &neib(int a,int b); +}; + + +int &Tri::neib(int a,int b) +{ + static int er=-1; + int i; + for(i=0;i<3;i++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + if((*this)[i]==a && (*this)[i1]==b) return n[i2]; + if((*this)[i]==b && (*this)[i1]==a) return n[i2]; + } + btAssert(0); + return er; +} +void HullLibrary::b2bfix(Tri* s,Tri*t) +{ + int i; + for(i=0;i<3;i++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + int a = (*s)[i1]; + int b = (*s)[i2]; + btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id); + btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id); + m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a); + m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b); + } +} + +void HullLibrary::removeb2b(Tri* s,Tri*t) +{ + b2bfix(s,t); + deAllocateTriangle(s); + + deAllocateTriangle(t); +} + +void HullLibrary::checkit(Tri *t) +{ + (void)t; + + int i; + btAssert(m_tris[t->id]==t); + for(i=0;i<3;i++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + int a = (*t)[i1]; + int b = (*t)[i2]; + + // release compile fix + (void)i1; + (void)i2; + (void)a; + (void)b; + + btAssert(a!=b); + btAssert( m_tris[t->n[i]]->neib(b,a) == t->id); + } +} + +Tri* HullLibrary::allocateTriangle(int a,int b,int c) +{ + void* mem = btAlignedAlloc(sizeof(Tri),16); + Tri* tr = new (mem)Tri(a,b,c); + tr->id = m_tris.size(); + m_tris.push_back(tr); + + return tr; +} + +void HullLibrary::deAllocateTriangle(Tri* tri) +{ + btAssert(m_tris[tri->id]==tri); + m_tris[tri->id]=NULL; + tri->~Tri(); + btAlignedFree(tri); +} + + +void HullLibrary::extrude(Tri *t0,int v) +{ + int3 t= *t0; + int n = m_tris.size(); + Tri* ta = allocateTriangle(v,t[1],t[2]); + ta->n = int3(t0->n[0],n+1,n+2); + m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0; + Tri* tb = allocateTriangle(v,t[2],t[0]); + tb->n = int3(t0->n[1],n+2,n+0); + m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1; + Tri* tc = allocateTriangle(v,t[0],t[1]); + tc->n = int3(t0->n[2],n+0,n+1); + m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2; + checkit(ta); + checkit(tb); + checkit(tc); + if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]); + if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]); + if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]); + deAllocateTriangle(t0); + +} + +Tri* HullLibrary::extrudable(btScalar epsilon) +{ + int i; + Tri *t=NULL; + for(i=0;iriserise)) + { + t = m_tris[i]; + } + } + return (t->rise >epsilon)?t:NULL ; +} + + + + +int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow) +{ + btVector3 basis[3]; + basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) ); + int p0 = maxdirsterid(verts,verts_count, basis[0],allow); + int p1 = maxdirsterid(verts,verts_count,-basis[0],allow); + basis[0] = verts[p0]-verts[p1]; + if(p0==p1 || basis[0]==btVector3(0,0,0)) + return int4(-1,-1,-1,-1); + basis[1] = cross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]); + basis[2] = cross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]); + if (basis[1].length() > basis[2].length()) + { + basis[1].normalize(); + } else { + basis[1] = basis[2]; + basis[1].normalize (); + } + int p2 = maxdirsterid(verts,verts_count,basis[1],allow); + if(p2 == p0 || p2 == p1) + { + p2 = maxdirsterid(verts,verts_count,-basis[1],allow); + } + if(p2 == p0 || p2 == p1) + return int4(-1,-1,-1,-1); + basis[1] = verts[p2] - verts[p0]; + basis[2] = cross(basis[1],basis[0]).normalized(); + int p3 = maxdirsterid(verts,verts_count,basis[2],allow); + if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow); + if(p3==p0||p3==p1||p3==p2) + return int4(-1,-1,-1,-1); + btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3)); + if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);} + return int4(p0,p1,p2,p3); +} + +int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) +{ + if(verts_count <4) return 0; + if(vlimit==0) vlimit=1000000000; + int j; + btVector3 bmin(*verts),bmax(*verts); + btAlignedObjectArray isextreme; + isextreme.reserve(verts_count); + btAlignedObjectArray allow; + allow.reserve(verts_count); + + for(j=0;jn=int3(2,3,1); + Tri *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); + Tri *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); + Tri *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); + isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; + checkit(t0);checkit(t1);checkit(t2);checkit(t3); + + for(j=0;jvmax<0); + btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); + t->vmax = maxdirsterid(verts,verts_count,n,allow); + t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); + } + Tri *te; + vlimit-=4; + while(vlimit >0 && ((te=extrudable(epsilon)) != 0)) + { + int3 ti=*te; + int v=te->vmax; + btAssert(v != -1); + btAssert(!isextreme[v]); // wtf we've already done this vertex + isextreme[v]=1; + //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already + j=m_tris.size(); + while(j--) { + if(!m_tris[j]) continue; + int3 t=*m_tris[j]; + if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) + { + extrude(m_tris[j],v); + } + } + // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle + j=m_tris.size(); + while(j--) + { + if(!m_tris[j]) continue; + if(!hasvert(*m_tris[j],v)) break; + int3 nt=*m_tris[j]; + if(above(verts,nt,center,btScalar(0.01)*epsilon) || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) ) + { + Tri *nb = m_tris[m_tris[j]->n[0]]; + btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->idvmax>=0) break; + btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); + t->vmax = maxdirsterid(verts,verts_count,n,allow); + if(isextreme[t->vmax]) + { + t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate. + } + else + { + t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); + } + } + vlimit --; + } + return 1; +} + +int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit) +{ + int rc=calchullgen(verts,verts_count, vlimit) ; + if(!rc) return 0; + btAlignedObjectArray ts; + int i; + + for(i=0;i(ts[i]); + } + m_tris.resize(0); + + return 1; +} + + + + + +bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit) +{ + + int tris_count; + int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast(vlimit) ); + if(!ret) return false; + result.mIndexCount = (unsigned int) (tris_count*3); + result.mFaceCount = (unsigned int) tris_count; + result.mVertices = (btVector3*) vertices; + result.mVcount = (unsigned int) vcount; + return true; + +} + + +void ReleaseHull(PHullResult &result); +void ReleaseHull(PHullResult &result) +{ + if ( result.m_Indices.size() ) + { + result.m_Indices.clear(); + } + + result.mVcount = 0; + result.mIndexCount = 0; + result.mVertices = 0; +} + + +//********************************************************************* +//********************************************************************* +//******** HullLib header +//********************************************************************* +//********************************************************************* + +//********************************************************************* +//********************************************************************* +//******** HullLib implementation +//********************************************************************* +//********************************************************************* + +HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request + HullResult &result) // contains the resulst +{ + HullError ret = QE_FAIL; + + + PHullResult hr; + + unsigned int vcount = desc.mVcount; + if ( vcount < 8 ) vcount = 8; + + btAlignedObjectArray vertexSource; + vertexSource.resize(static_cast(vcount)); + + btVector3 scale; + + unsigned int ovcount; + + bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates! + + if ( ok ) + { + + +// if ( 1 ) // scale vertices back to their original size. + { + for (unsigned int i=0; i(i)]; + v[0]*=scale[0]; + v[1]*=scale[1]; + v[2]*=scale[2]; + } + } + + ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices); + + if ( ok ) + { + + // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table. + btAlignedObjectArray vertexScratch; + vertexScratch.resize(static_cast(hr.mVcount)); + + BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount ); + + ret = QE_OK; + + if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle! + { + result.mPolygons = false; + result.mNumOutputVertices = ovcount; + result.m_OutputVertices.resize(static_cast(ovcount)); + result.mNumFaces = hr.mFaceCount; + result.mNumIndices = hr.mIndexCount; + + result.m_Indices.resize(static_cast(hr.mIndexCount)); + + memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); + + if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) + { + + const unsigned int *source = &hr.m_Indices[0]; + unsigned int *dest = &result.m_Indices[0]; + + for (unsigned int i=0; i(ovcount)); + result.mNumFaces = hr.mFaceCount; + result.mNumIndices = hr.mIndexCount+hr.mFaceCount; + result.m_Indices.resize(static_cast(result.mNumIndices)); + memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); + +// if ( 1 ) + { + const unsigned int *source = &hr.m_Indices[0]; + unsigned int *dest = &result.m_Indices[0]; + for (unsigned int i=0; i bmax[j] ) bmax[j] = p[j]; + } + } + } + + btScalar dx = bmax[0] - bmin[0]; + btScalar dy = bmax[1] - bmin[1]; + btScalar dz = bmax[2] - bmin[2]; + + btVector3 center; + + center[0] = dx*btScalar(0.5) + bmin[0]; + center[1] = dy*btScalar(0.5) + bmin[1]; + center[2] = dz*btScalar(0.5) + bmin[2]; + + if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 ) + { + + btScalar len = FLT_MAX; + + if ( dx > EPSILON && dx < len ) len = dx; + if ( dy > EPSILON && dy < len ) len = dy; + if ( dz > EPSILON && dz < len ) len = dz; + + if ( len == FLT_MAX ) + { + dx = dy = dz = btScalar(0.01); // one centimeter + } + else + { + if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. + if ( dy < EPSILON ) dy = len * btScalar(0.05); + if ( dz < EPSILON ) dz = len * btScalar(0.05); + } + + btScalar x1 = center[0] - dx; + btScalar x2 = center[0] + dx; + + btScalar y1 = center[1] - dy; + btScalar y2 = center[1] + dy; + + btScalar z1 = center[2] - dz; + btScalar z2 = center[2] + dz; + + addPoint(vcount,vertices,x1,y1,z1); + addPoint(vcount,vertices,x2,y1,z1); + addPoint(vcount,vertices,x2,y2,z1); + addPoint(vcount,vertices,x1,y2,z1); + addPoint(vcount,vertices,x1,y1,z2); + addPoint(vcount,vertices,x2,y1,z2); + addPoint(vcount,vertices,x2,y2,z2); + addPoint(vcount,vertices,x1,y2,z2); + + return true; // return cube + + + } + else + { + if ( scale ) + { + scale[0] = dx; + scale[1] = dy; + scale[2] = dz; + + recip[0] = 1 / dx; + recip[1] = 1 / dy; + recip[2] = 1 / dz; + + center[0]*=recip[0]; + center[1]*=recip[1]; + center[2]*=recip[2]; + + } + + } + + + + vtx = (const char *) svertices; + + for (unsigned int i=0; igetX(); + btScalar py = p->getY(); + btScalar pz = p->getZ(); + + if ( scale ) + { + px = px*recip[0]; // normalize + py = py*recip[1]; // normalize + pz = pz*recip[2]; // normalize + } + +// if ( 1 ) + { + unsigned int j; + + for (j=0; j dist2 ) + { + v[0] = px; + v[1] = py; + v[2] = pz; + + } + + break; + } + } + + if ( j == vcount ) + { + btVector3& dest = vertices[vcount]; + dest[0] = px; + dest[1] = py; + dest[2] = pz; + vcount++; + } + m_vertexIndexMapping.push_back(j); + } + } + + // ok..now make sure we didn't prune so many vertices it is now invalid. +// if ( 1 ) + { + btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; + btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; + + for (unsigned int i=0; i bmax[j] ) bmax[j] = p[j]; + } + } + + btScalar dx = bmax[0] - bmin[0]; + btScalar dy = bmax[1] - bmin[1]; + btScalar dz = bmax[2] - bmin[2]; + + if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3) + { + btScalar cx = dx*btScalar(0.5) + bmin[0]; + btScalar cy = dy*btScalar(0.5) + bmin[1]; + btScalar cz = dz*btScalar(0.5) + bmin[2]; + + btScalar len = FLT_MAX; + + if ( dx >= EPSILON && dx < len ) len = dx; + if ( dy >= EPSILON && dy < len ) len = dy; + if ( dz >= EPSILON && dz < len ) len = dz; + + if ( len == FLT_MAX ) + { + dx = dy = dz = btScalar(0.01); // one centimeter + } + else + { + if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. + if ( dy < EPSILON ) dy = len * btScalar(0.05); + if ( dz < EPSILON ) dz = len * btScalar(0.05); + } + + btScalar x1 = cx - dx; + btScalar x2 = cx + dx; + + btScalar y1 = cy - dy; + btScalar y2 = cy + dy; + + btScalar z1 = cz - dz; + btScalar z2 = cz + dz; + + vcount = 0; // add box + + addPoint(vcount,vertices,x1,y1,z1); + addPoint(vcount,vertices,x2,y1,z1); + addPoint(vcount,vertices,x2,y2,z1); + addPoint(vcount,vertices,x1,y2,z1); + addPoint(vcount,vertices,x1,y1,z2); + addPoint(vcount,vertices,x2,y1,z2); + addPoint(vcount,vertices,x2,y2,z2); + addPoint(vcount,vertices,x1,y2,z2); + + return true; + } + } + + return true; +} + +void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount) +{ + btAlignedObjectArraytmpIndices; + tmpIndices.resize(m_vertexIndexMapping.size()); + int i; + + for (i=0;i(vcount)); + memset(&usedIndices[0],0,sizeof(unsigned int)*vcount); + + ocount = 0; + + for (i=0; i= 0 && v < vcount ); + + if ( usedIndices[static_cast(v)] ) // if already remapped + { + indices[i] = usedIndices[static_cast(v)]-1; // index to new array + } + else + { + + indices[i] = ocount; // new index mapping + + overts[ocount][0] = verts[v][0]; // copy old vert to new vert array + overts[ocount][1] = verts[v][1]; + overts[ocount][2] = verts[v][2]; + + for (int k=0;k=0 && ocount <= vcount ); + + usedIndices[static_cast(v)] = ocount; // assign new index remapping + + + } + } + + +} diff --git a/extern/bullet2/src/LinearMath/btConvexHull.h b/extern/bullet2/src/LinearMath/btConvexHull.h new file mode 100644 index 00000000000..8c36307dfd6 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btConvexHull.h @@ -0,0 +1,245 @@ + +/* +Stan Melax Convex Hull Computation +Copyright (c) 2008 Stan Melax http://www.melax.com/ + +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. +*/ + +///includes modifications/improvements by John Ratcliff, see BringOutYourDead below. + +#ifndef CD_HULL_H +#define CD_HULL_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +typedef btAlignedObjectArray TUIntArray; + +class HullResult +{ +public: + HullResult(void) + { + mPolygons = true; + mNumOutputVertices = 0; + mNumFaces = 0; + mNumIndices = 0; + } + bool mPolygons; // true if indices represents polygons, false indices are triangles + unsigned int mNumOutputVertices; // number of vertices in the output hull + btAlignedObjectArray m_OutputVertices; // array of vertices + unsigned int mNumFaces; // the number of faces produced + unsigned int mNumIndices; // the total number of indices + btAlignedObjectArray m_Indices; // pointer to indices. + +// If triangles, then indices are array indexes into the vertex list. +// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. +}; + +enum HullFlag +{ + QF_TRIANGLES = (1<<0), // report results as triangles, not polygons. + QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices. + QF_DEFAULT = QF_TRIANGLES +}; + + +class HullDesc +{ +public: + HullDesc(void) + { + mFlags = QF_DEFAULT; + mVcount = 0; + mVertices = 0; + mVertexStride = sizeof(btVector3); + mNormalEpsilon = 0.001f; + mMaxVertices = 4096; // maximum number of points to be considered for a convex hull. + mMaxFaces = 4096; + }; + + HullDesc(HullFlag flag, + unsigned int vcount, + const btVector3 *vertices, + unsigned int stride = sizeof(btVector3)) + { + mFlags = flag; + mVcount = vcount; + mVertices = vertices; + mVertexStride = stride; + mNormalEpsilon = btScalar(0.001); + mMaxVertices = 4096; + } + + bool HasHullFlag(HullFlag flag) const + { + if ( mFlags & flag ) return true; + return false; + } + + void SetHullFlag(HullFlag flag) + { + mFlags|=flag; + } + + void ClearHullFlag(HullFlag flag) + { + mFlags&=~flag; + } + + unsigned int mFlags; // flags to use when generating the convex hull. + unsigned int mVcount; // number of vertices in the input point cloud + const btVector3 *mVertices; // the array of vertices. + unsigned int mVertexStride; // the stride of each vertex, in bytes. + btScalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. + unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull! + unsigned int mMaxFaces; +}; + +enum HullError +{ + QE_OK, // success! + QE_FAIL // failed. +}; + +class btPlane +{ + public: + btVector3 normal; + btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 + btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){} + btPlane():normal(),dist(0){} + +}; + + + +class ConvexH +{ + public: + class HalfEdge + { + public: + short ea; // the other half of the edge (index into edges list) + unsigned char v; // the vertex at the start of this edge (index into vertices list) + unsigned char p; // the facet on which this edge lies (index into facets list) + HalfEdge(){} + HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){} + }; + ConvexH() + { + int i; + i=0; + } + ~ConvexH() + { + int i; + i=0; + } + btAlignedObjectArray vertices; + btAlignedObjectArray edges; + btAlignedObjectArray facets; + ConvexH(int vertices_size,int edges_size,int facets_size); +}; + + +class int4 +{ +public: + int x,y,z,w; + int4(){}; + int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;} + const int& operator[](int i) const {return (&x)[i];} + int& operator[](int i) {return (&x)[i];} +}; + +class PHullResult +{ +public: + + PHullResult(void) + { + mVcount = 0; + mIndexCount = 0; + mFaceCount = 0; + mVertices = 0; + } + + unsigned int mVcount; + unsigned int mIndexCount; + unsigned int mFaceCount; + btVector3* mVertices; + TUIntArray m_Indices; +}; + + + +///The HullLibrary class can create a convex hull from a collection of vertices, using the ComputeHull method. +///The btShapeHull class uses this HullLibrary to create a approximate convex mesh given a general (non-polyhedral) convex shape. +class HullLibrary +{ + + btAlignedObjectArray m_tris; + +public: + + btAlignedObjectArray m_vertexIndexMapping; + + + HullError CreateConvexHull(const HullDesc& desc, // describes the input request + HullResult& result); // contains the resulst + HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it. + +private: + + bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit); + + class Tri* allocateTriangle(int a,int b,int c); + void deAllocateTriangle(Tri*); + void b2bfix(Tri* s,Tri*t); + + void removeb2b(Tri* s,Tri*t); + + void checkit(Tri *t); + + Tri* extrudable(btScalar epsilon); + + int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit); + + int calchullgen(btVector3 *verts,int verts_count, int vlimit); + + int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow); + + class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice); + + void extrude(class Tri* t0,int v); + + ConvexH* test_cube(); + + //BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'. + //After the hull is generated it give you back a set of polygon faces which index the *original* point cloud. + //The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull. + //The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation. + void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount); + + bool CleanupVertices(unsigned int svcount, + const btVector3* svertices, + unsigned int stride, + unsigned int &vcount, // output number of vertices + btVector3* vertices, // location to store the results. + btScalar normalepsilon, + btVector3& scale); +}; + + +#endif + diff --git a/extern/bullet2/src/LinearMath/btHashMap.h b/extern/bullet2/src/LinearMath/btHashMap.h new file mode 100644 index 00000000000..f883e0e489a --- /dev/null +++ b/extern/bullet2/src/LinearMath/btHashMap.h @@ -0,0 +1,303 @@ +#ifndef BT_HASH_MAP_H +#define BT_HASH_MAP_H + +#include "btAlignedObjectArray.h" + +const int BT_HASH_NULL=0xffffffff; + +template +class btHashKey +{ + int m_uid; +public: + + btHashKey(int uid) + :m_uid(uid) + { + } + + int getUid() const + { + return m_uid; + } + + //to our success + SIMD_FORCE_INLINE unsigned int getHash()const + { + int key = m_uid; + // Thomas Wang's hash + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return key; + } + + btHashKey getKey(const Value& value) const + { + return btHashKey(value.getUid()); + } +}; + + +template +class btHashKeyPtr +{ + int m_uid; +public: + + btHashKeyPtr(int uid) + :m_uid(uid) + { + } + + int getUid() const + { + return m_uid; + } + + //to our success + SIMD_FORCE_INLINE unsigned int getHash()const + { + int key = m_uid; + // Thomas Wang's hash + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return key; + } + + btHashKeyPtr getKey(const Value& value) const + { + return btHashKeyPtr(value->getUid()); + } +}; + +///The btHashMap template class implements a generic and lightweight hashmap. +///A basic sample of how to use btHashMap is located in Demos\BasicDemo\main.cpp +template +class btHashMap +{ + + btAlignedObjectArray m_hashTable; + btAlignedObjectArray m_next; + btAlignedObjectArray m_valueArray; + + + + void growTables(const Key& key) + { + int newCapacity = m_valueArray.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] = BT_HASH_NULL; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = BT_HASH_NULL; + } + + for(i=0;i= m_hashTable.size()) + { + return BT_HASH_NULL; + } + + int index = m_hashTable[hash]; + while ((index != BT_HASH_NULL) && (key.getUid() == key.getKey(m_valueArray[index]).getUid()) == false) + { + index = m_next[index]; + } + return index; + } + + void clear() + { + m_hashTable.clear(); + m_next.clear(); + m_valueArray.clear(); + } + +}; + +#endif //BT_HASH_MAP_H diff --git a/extern/bullet2/src/LinearMath/btPoolAllocator.h b/extern/bullet2/src/LinearMath/btPoolAllocator.h new file mode 100644 index 00000000000..39d2559c747 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btPoolAllocator.h @@ -0,0 +1,102 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef _BT_POOL_ALLOCATOR_H +#define _BT_POOL_ALLOCATOR_H + +#include "btScalar.h" +#include "btAlignedAllocator.h" + +///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately. +class btPoolAllocator +{ + int m_elemSize; + int m_maxElements; + int m_freeCount; + void* m_firstFree; + unsigned char* m_pool; + +public: + + btPoolAllocator(int elemSize, int maxElements) + :m_elemSize(elemSize), + m_maxElements(maxElements) + { + m_pool = (unsigned char*) btAlignedAlloc( static_cast(m_elemSize*m_maxElements),16); + + unsigned char* p = m_pool; + m_firstFree = p; + m_freeCount = m_maxElements; + int count = m_maxElements; + while (--count) { + *(void**)p = (p + m_elemSize); + p += m_elemSize; + } + *(void**)p = 0; + } + + ~btPoolAllocator() + { + btAlignedFree( m_pool); + } + + int getFreeCount() const + { + return m_freeCount; + } + + void* allocate(int size) + { + // release mode fix + (void)size; + btAssert(!size || size<=m_elemSize); + btAssert(m_freeCount>0); + void* result = m_firstFree; + m_firstFree = *(void**)m_firstFree; + --m_freeCount; + return result; + } + + bool validPtr(void* ptr) + { + if (ptr) { + if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize)) + { + return true; + } + } + return false; + } + + void freeMemory(void* ptr) + { + if (ptr) { + btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize); + + *(void**)ptr = m_firstFree; + m_firstFree = ptr; + ++m_freeCount; + } + } + + int getElementSize() const + { + return m_elemSize; + } + + +}; + +#endif //_BT_POOL_ALLOCATOR_H diff --git a/intern/elbeem/intern/controlparticles.cpp b/intern/elbeem/intern/controlparticles.cpp new file mode 100644 index 00000000000..4b496e4a3a2 --- /dev/null +++ b/intern/elbeem/intern/controlparticles.cpp @@ -0,0 +1,1461 @@ +// -------------------------------------------------------------------------- +// +// El'Beem - the visual lattice boltzmann freesurface simulator +// All code distributed as part of El'Beem is covered by the version 2 of the +// GNU General Public License. See the file COPYING for details. +// +// Copyright 2008 Nils Thuerey , Richard Keiser, Mark Pauly, Ulrich Ruede +// +// implementation of control particle handling +// +// -------------------------------------------------------------------------- + +// indicator for LBM inclusion +#include "ntl_geometrymodel.h" +#include "ntl_world.h" +#include "solver_class.h" +#include "controlparticles.h" +#include "mvmcoords.h" +#include + +#ifndef sqrtf +#define sqrtf sqrt +#endif + +// brute force circle test init in initTimeArray +// replaced by mDebugInit +//#define CP_FORCECIRCLEINIT 0 + + +void ControlParticles::initBlenderTest() { + mPartSets.clear(); + + ControlParticleSet cps; + mPartSets.push_back(cps); + int setCnt = mPartSets.size()-1; + ControlParticle p; + + // set for time zero + mPartSets[setCnt].time = 0.; + + // add single particle + p.reset(); + p.pos = LbmVec(0.5, 0.5, -0.5); + mPartSets[setCnt].particles.push_back(p); + + // add second set for animation + mPartSets.push_back(cps); + setCnt = mPartSets.size()-1; + mPartSets[setCnt].time = 0.15; + + // insert new position + p.reset(); + p.pos = LbmVec(-0.5, -0.5, 0.5); + mPartSets[setCnt].particles.push_back(p); + + // applyTrafos(); + initTime(0. , 1.); +} + +// blender control object gets converted to mvm flui control object +int ControlParticles::initFromObject(ntlGeometryObjModel *model) { + vector triangles; + vector vertices; + vector normals; + + /* + model->loadBobjModel(string(infile)); + + model->setLoaded(true); + + model->setGeoInitId(gid); + + + printf("a animated? %d\n", model->getIsAnimated()); + printf("b animated? %d\n", model->getMeshAnimated()); + */ + + model->setGeoInitType(FGI_FLUID); + + model->getTriangles(mCPSTimeStart, &triangles, &vertices, &normals, 1 ); + // model->applyTransformation(mCPSTimeStart, &vertices, &normals, 0, vertices.size(), true); + + // valid mesh? + if(triangles.size() <= 0) { + return 0; + } + + ntlRenderGlobals *glob = new ntlRenderGlobals; + ntlScene *genscene = new ntlScene( glob, false ); + genscene->addGeoClass(model); + genscene->addGeoObject(model); + genscene->buildScene(0., false); + char treeFlag = (1<<(4+model->getGeoInitId())); + + ntlTree *tree = new ntlTree( + 15, 8, // TREEwarning - fixed values for depth & maxtriangles here... + genscene, treeFlag ); + + // TODO? use params + ntlVec3Gfx start,end; + model->getExtends(start,end); + /* + printf("start - x: %f, y: %f, z: %f\n", start[0], start[1], start[2]); + printf("end - x: %f, y: %f, z: %f\n", end[0], end[1], end[2]); + printf("mCPSWidth: %f\n"); +*/ + LbmFloat width = mCPSWidth; + if(width<=LBM_EPSILON) { errMsg("ControlParticles::initFromMVMCMesh","Invalid mCPSWidth! "< inspos; + + // printf("distance: %f, width: %f\n", distance, width); + + while(org[2] ninspos; + mvm.transfer(vertices, ninspos); + + // init first set, check dist + ControlParticleSet firstcps; //T + mPartSets.push_back(firstcps); + mPartSets[mPartSets.size()-1].time = mCPSTimeStart; + vector useCP; + + for(int i=0; i<(int)inspos.size(); i++) { + ControlParticle p; p.reset(); + p.pos = vec2L(inspos[i]); + + bool usecpv = true; + + mPartSets[mPartSets.size()-1].particles.push_back(p); + useCP.push_back(usecpv); + } + + // init further sets, temporal mesh sampling + double tsampling = mCPSTimestep; + // printf("tsampling: %f, ninspos.size(): %d, mCPSTimeEnd: %f\n", tsampling, ninspos.size(), mCPSTimeEnd); + + int tcnt=0; + for(double t=mCPSTimeStart+tsampling; ((t0.)); t+=tsampling) { + ControlParticleSet nextcps; //T + mPartSets.push_back(nextcps); + mPartSets[mPartSets.size()-1].time = (gfxReal)t; + + vertices.clear(); triangles.clear(); normals.clear(); + model->getTriangles(t, &triangles, &vertices, &normals, 1 ); + mvm.transfer(vertices, ninspos); + + tcnt++; + for(size_t i=0; i < ninspos.size(); i++) { + + if(useCP[i]) { + ControlParticle p; p.reset(); + p.pos = vec2L(ninspos[i]); + mPartSets[mPartSets.size()-1].particles.push_back(p); + } + } + } + + model->setGeoInitType(FGI_CONTROL); + + delete tree; + delete genscene; + delete glob; + + // do reverse here + if(model->getGeoPartSlipValue()) + { + mirrorTime(); + } + + return 1; +} + + +// init all zero / defaults for a single particle +void ControlParticle::reset() { + pos = LbmVec(0.,0.,0.); + vel = LbmVec(0.,0.,0.); + influence = 1.; + size = 1.; +#ifndef LBMDIM +#ifdef MAIN_2D + rotaxis = LbmVec(0.,1.,0.); // SPH xz +#else // MAIN_2D + // 3d - roate in xy plane, vortex + rotaxis = LbmVec(0.,0.,1.); + // 3d - rotate for wave + //rotaxis = LbmVec(0.,1.,0.); +#endif // MAIN_2D +#else // LBMDIM + rotaxis = LbmVec(0.,1.,0.); // LBM xy , is swapped afterwards +#endif // LBMDIM + + density = 0.; + densityWeight = 0.; + avgVelAcc = avgVel = LbmVec(0.); + avgVelWeight = 0.; +} + + +// default preset/empty init +ControlParticles::ControlParticles() : + _influenceTangential(0.f), + _influenceAttraction(0.f), + _influenceVelocity(0.f), + _influenceMaxdist(0.f), + _radiusAtt(1.0f), + _radiusVel(1.0f), + _radiusMinMaxd(2.0f), + _radiusMaxd(3.0f), + _currTime(-1.0), _currTimestep(1.), + _initTimeScale(1.), + _initPartOffset(0.), _initPartScale(1.), + _initLastPartOffset(0.), _initLastPartScale(1.), + _initMirror(""), + _fluidSpacing(1.), _kernelWeight(-1.), + _charLength(1.), _charLengthInv(1.), + mvCPSStart(-10000.), mvCPSEnd(10000.), + mCPSWidth(0.1), mCPSTimestep(0.02), // was 0.05 + mCPSTimeStart(0.), mCPSTimeEnd(0.5), mCPSWeightFac(1.), + mDebugInit(0) +{ + _radiusAtt = 0.15f; + _radiusVel = 0.15f; + _radiusMinMaxd = 0.16f; + _radiusMaxd = 0.3; + + _influenceAttraction = 0.f; + _influenceTangential = 0.f; + _influenceVelocity = 0.f; + // 3d tests */ +} + + + +ControlParticles::~ControlParticles() { + // nothing to do... +} + +LbmFloat ControlParticles::getControlTimStart() { + if(mPartSets.size()>0) { return mPartSets[0].time; } + return -1000.; +} +LbmFloat ControlParticles::getControlTimEnd() { + if(mPartSets.size()>0) { return mPartSets[mPartSets.size()-1].time; } + return -1000.; +} + +// calculate for delta t +void ControlParticles::setInfluenceVelocity(LbmFloat set, LbmFloat dt) { + const LbmFloat dtInter = 0.01; + LbmFloat facFv = 1.-set; //cparts->getInfluenceVelocity(); + // mLevel[mMaxRefine].timestep + LbmFloat facNv = (LbmFloat)( 1.-pow( (double)facFv, (double)(dt/dtInter)) ); + //errMsg("vwcalc","ts:"<1) && (line[0]=='/' && line[1]=='/')) continue; + + // debug remove newline + if((len>=1)&&(line[len-1]=='\n')) line[len-1]='\0'; + + switch(line[0]) { + + case 'N': { // total number of particles, more for debugging... + noParts = atoi(line+2); + if(noParts<=0) { + errMsg("ControlParticles::initFromTextFile","file '"<=noParts) { + if(debugRead) printf("CPDEBUG%d partset done \n",lineCnt); + haveTime = false; + } else { + ControlParticle p; p.reset(); + mPartSets[setCnt].particles.push_back(p); + } + } + // only new part, or new with pos? + if(line[0] == 'n') break; + + // particle properties + + case 'p': { // new particle set at time T + if((!haveTime)||(setCnt<0)||(mPartSets[setCnt].particles.size()<1)) { fprintf(stdout,"ControlParticles::initFromTextFile - line %d ,error|p: particle missing!\n",lineCnt); abortParse=true; break; } + float px=0.,py=0.,pz=0.; + if( sscanf(line+2,"%f %f %f",&px,&py,&pz) != 3) { + fprintf(stdout,"CPDEBUG%d, unable to parse position!\n",lineCnt); abortParse=true; break; + } + if(!(finite(px)&&finite(py)&&finite(pz))) { px=py=pz=0.; } + LASTCP.pos[0] = px; + LASTCP.pos[1] = py; + LASTCP.pos[2] = pz; + if(debugRead) printf("CPDEBUG%d part%d,%d: position %f,%f,%f \n",lineCnt,setCnt,partCnt, px,py,pz); + } break; + + case 's': { // particle size + if((!haveTime)||(setCnt<0)||(mPartSets[setCnt].particles.size()<1)) { fprintf(stdout,"ControlParticles::initFromTextFile - line %d ,error|s: particle missing!\n",lineCnt); abortParse=true; break; } + float ps=1.; + if( sscanf(line+2,"%f",&ps) != 1) { + fprintf(stdout,"CPDEBUG%d, unable to parse size!\n",lineCnt); abortParse=true; break; + } + if(!(finite(ps))) { ps=0.; } + LASTCP.size = ps; + if(debugRead) printf("CPDEBUG%d part%d,%d: size %f \n",lineCnt,setCnt,partCnt, ps); + } break; + + case 'i': { // particle influence + if((!haveTime)||(setCnt<0)||(mPartSets[setCnt].particles.size()<1)) { fprintf(stdout,"ControlParticles::initFromTextFile - line %d ,error|i: particle missing!\n",lineCnt); abortParse=true; break; } + float pinf=1.; + if( sscanf(line+2,"%f",&pinf) != 1) { + fprintf(stdout,"CPDEBUG%d, unable to parse size!\n",lineCnt); abortParse=true; break; + } + if(!(finite(pinf))) { pinf=0.; } + LASTCP.influence = pinf; + if(debugRead) printf("CPDEBUG%d part%d,%d: influence %f \n",lineCnt,setCnt,partCnt, pinf); + } break; + + case 'a': { // rotation axis + if((!haveTime)||(setCnt<0)||(mPartSets[setCnt].particles.size()<1)) { fprintf(stdout,"ControlParticles::initFromTextFile - line %d ,error|a: particle missing!\n",lineCnt); abortParse=true; break; } + float px=0.,py=0.,pz=0.; + if( sscanf(line+2,"%f %f %f",&px,&py,&pz) != 3) { + fprintf(stdout,"CPDEBUG%d, unable to parse rotaxis!\n",lineCnt); abortParse=true; break; + } + if(!(finite(px)&&finite(py)&&finite(pz))) { px=py=pz=0.; } + LASTCP.rotaxis[0] = px; + LASTCP.rotaxis[1] = py; + LASTCP.rotaxis[2] = pz; + if(debugRead) printf("CPDEBUG%d part%d,%d: rotaxis %f,%f,%f \n",lineCnt,setCnt,partCnt, px,py,pz); + } break; + + + default: + if(debugRead) printf("CPDEBUG%d ignored: '%s'\n",lineCnt, line ); + break; + } + } + if(debugRead && abortParse) printf("CPDEBUG aborted parsing after set... %d\n",(int)mPartSets.size() ); + + // sanity check + for(int i=0; i<(int)mPartSets.size(); i++) { + if( (int)mPartSets[i].particles.size()!=noParts) { + fprintf(stdout,"ControlParticles::initFromTextFile (%s) - invalid no of particles in set %d, is:%d, shouldbe:%d \n",filename.c_str() ,i,(int)mPartSets[i].particles.size(), noParts); + mPartSets.clear(); + fclose(infile); + return 0; + } + } + + // print stats + printf("ControlParticles::initFromTextFile (%s): Read %d sets, each %d particles\n",filename.c_str() , + (int)mPartSets.size(), noParts ); + if(mPartSets.size()>0) { + printf("ControlParticles::initFromTextFile (%s): Time: %f,%f\n",filename.c_str() ,mPartSets[0].time, mPartSets[mPartSets.size()-1].time ); + } + + // done... + fclose(infile); + applyTrafos(); + */ + return 1; +} + + +int ControlParticles::initFromTextFileOld(string filename) +{ + /* + const bool debugRead = false; + char line[LINE_LEN]; + line[LINE_LEN-1] = '\0'; + mPartSets.clear(); + if(filename.size()<1) return 0; + + FILE *infile = fopen(filename.c_str(), "r"); + if(!infile) { + fprintf(stdout,"ControlParticles::initFromTextFileOld - unable to open '%s'\n",filename.c_str() ); + return 0; + } + + int haveNo = false; + int haveScale = false; + int haveTime = false; + int noParts = -1; + int coordCnt = 0; + int partCnt = 0; + int setCnt = 0; + ControlParticle p; p.reset(); + // scale times by constant factor while reading + LbmFloat timeScale= 1.0; + int lineCnt = 0; + + while(!feof(infile)) { + lineCnt++; + fgets(line, LINE_LEN, infile); + + if(debugRead) printf("\nDEBUG%d r '%s'\n",lineCnt, line); + + if(!line) continue; + size_t len = strlen(line); + + // skip empty lines and comments (#,//) + if(len<1) continue; + if( (line[0]=='#') || (line[0]=='\n') ) continue; + if((len>1) && (line[0]=='/' && line[1]=='/')) continue; + + // debug remove newline + if((len>=1)&&(line[len-1]=='\n')) line[len-1]='\0'; + + // first read no. of particles + if(!haveNo) { + noParts = atoi(line); + if(noParts<=0) { + fprintf(stdout,"ControlParticles::initFromTextFileOld - invalid no of particles %d\n",noParts); + mPartSets.clear(); + fclose(infile); + return 0; + } + if(debugRead) printf("DEBUG%d noparts '%d'\n",lineCnt, noParts ); + haveNo = true; + } + + // then read time scale + else if(!haveScale) { + timeScale *= (LbmFloat)atof(line); + if(debugRead) printf("DEBUG%d tsc '%f', org %f\n",lineCnt, timeScale , _initTimeScale); + haveScale = true; + } + + // then get set time + else if(!haveTime) { + ControlParticleSet cps; + mPartSets.push_back(cps); + setCnt = (int)mPartSets.size()-1; + + LbmFloat val = (LbmFloat)atof(line); + mPartSets[setCnt].time = val * timeScale; + if(debugRead) printf("DEBUG%d time '%f', %d\n",lineCnt, mPartSets[setCnt].time, setCnt ); + haveTime = true; + } + + // default read all parts + else { + LbmFloat val = (LbmFloat)atof(line); + if(debugRead) printf("DEBUG: l%d s%d,particle%d '%f' %d,%d/%d\n",lineCnt,(int)mPartSets.size(),(int)mPartSets[setCnt].particles.size(), val ,coordCnt,partCnt,noParts); + p.pos[coordCnt] = val; + coordCnt++; + if(coordCnt>=3) { + mPartSets[setCnt].particles.push_back(p); + p.reset(); + coordCnt=0; + partCnt++; + } + if(partCnt>=noParts) { + partCnt = 0; + haveTime = false; + } + //if(debugRead) printf("DEBUG%d par2 %d,%d/%d\n",lineCnt, coordCnt,partCnt,noParts); + } + //read pos, vel ... + } + + // sanity check + for(int i=0; i<(int)mPartSets.size(); i++) { + if( (int)mPartSets[i].particles.size()!=noParts) { + fprintf(stdout,"ControlParticles::initFromTextFileOld - invalid no of particles in set %d, is:%d, shouldbe:%d \n",i,(int)mPartSets[i].particles.size(), noParts); + mPartSets.clear(); + fclose(infile); + return 0; + } + } + // print stats + printf("ControlParticles::initFromTextFileOld: Read %d sets, each %d particles\n", + (int)mPartSets.size(), noParts ); + if(mPartSets.size()>0) { + printf("ControlParticles::initFromTextFileOld: Time: %f,%f\n",mPartSets[0].time, mPartSets[mPartSets.size()-1].time ); + } + + // done... + fclose(infile); + applyTrafos(); + */ + return 1; +} + +// load positions & timing from gzipped binary file +int ControlParticles::initFromBinaryFile(string filename) { + mPartSets.clear(); + if(filename.size()<1) return 0; + int fileNotFound=0; + int fileFound=0; + char ofile[256]; + + for(int set=0; ((set<10000)&&(fileNotFound<10)); set++) { + snprintf(ofile,256,"%s%04d.gz",filename.c_str(),set); + //errMsg("ControlParticle::initFromBinaryFile","set"<intersectX(ray,distance,normal, triIns, flags, true); + if(triIns) { + ntlVec3Gfx norg = ray.getOrigin() + ray.getDirection()*distance; + LbmFloat orientation = dot(normal, dir); + OId = triIns->getObjectId(); + if(orientation<=0.0) { + // outside hit + normal *= -1.0; + mGiObjInside++; + if(giObjFirstHistSide==0) giObjFirstHistSide = 1; + if(globGeoInitDebug) errMsg("IIO"," oid:"<0) { + bool mess = false; + if((mGiObjInside%2)==1) { + if(giObjFirstHistSide != -1) mess=true; + } else { + if(giObjFirstHistSide != 1) mess=true; + } + if(mess) { + // ? + //errMsg("IIIproblem","At "<0.0)) { + if( (distance<0.0) || // first intersection -> good + ((distance>0.0)&&(distance>mGiObjDistance)) // more than one intersection -> use closest one + ) { + distance = mGiObjDistance; + OId = 0; + inside = true; + } + } + + if(!inside) { + distance = firstHit; + OId = firstOId; + } + if(globGeoInitDebug) errMsg("CHIII","ins"< triangles; + vector vertices; + vector normals; + snprintf(infile,256,"%s.bobj.gz", filename.c_str() ); + model->loadBobjModel(string(infile)); + model->setLoaded(true); + model->setGeoInitId(gid); + model->setGeoInitType(FGI_FLUID); + debMsgStd("ControlParticles::initFromMVMCMesh",DM_MSG,"infile:"< *triangles, vector *vertices, vector *normals, int objectId ); + model->getTriangles(mCPSTimeStart, &triangles, &vertices, &normals, 1 ); + debMsgStd("ControlParticles::initFromMVMCMesh",DM_MSG," tris:"<addGeoClass(model); + genscene->addGeoObject(model); + genscene->buildScene(0., false); + char treeFlag = (1<<(4+gid)); + + ntlTree *tree = new ntlTree( + 15, 8, // TREEwarning - fixed values for depth & maxtriangles here... + genscene, treeFlag ); + + // TODO? use params + ntlVec3Gfx start,end; + model->getExtends(start,end); + + LbmFloat width = mCPSWidth; + if(width<=LBM_EPSILON) { errMsg("ControlParticles::initFromMVMCMesh","Invalid mCPSWidth! "< inspos; + int approxmax = (int)( ((end[0]-start[0])/width)*((end[1]-start[1])/width)*((end[2]-start[2])/width) ); + + debMsgStd("ControlParticles::initFromMVMCMesh",DM_MSG,"start"< ninspos; + mvm.transfer(vertices, ninspos); + + // init first set, check dist + ControlParticleSet firstcps; //T + mPartSets.push_back(firstcps); + mPartSets[mPartSets.size()-1].time = (gfxReal)0.; + vector useCP; + bool debugPos=false; + + for(int i=0; i<(int)inspos.size(); i++) { + ControlParticle p; p.reset(); + p.pos = vec2L(inspos[i]); + //errMsg("COMP "," "<0.)); t+=tsampling) { + ControlParticleSet nextcps; //T + mPartSets.push_back(nextcps); + mPartSets[mPartSets.size()-1].time = (gfxReal)t; + + vertices.clear(); triangles.clear(); normals.clear(); + model->getTriangles(t, &triangles, &vertices, &normals, 1 ); + mvm.transfer(vertices, ninspos); + if(tcnt%(totcnt/10)==1) debMsgStd("MeanValueMeshCoords::calculateMVMCs",DM_MSG,"Transferring animation, frame: "< swap Y and Z components everywhere +void ControlParticles::swapCoords(int a, int b) { + //return; + for(int i=0; i<(int)mPartSets.size(); i++) { + for(int j=0; j<(int)mPartSets[i].particles.size(); j++) { + TRISWAPALL( mPartSets[i].particles[j],a,b ); + } + } +} + +// helper function for LBM 2D -> mirror time +void ControlParticles::mirrorTime() { + LbmFloat maxtime = mPartSets[mPartSets.size()-1].time; + const bool debugTimeswap = false; + + for(int i=0; i<(int)mPartSets.size(); i++) { + mPartSets[i].time = maxtime - mPartSets[i].time; + } + + for(int i=0; i<(int)mPartSets.size()/2; i++) { + ControlParticleSet cps = mPartSets[i]; + if(debugTimeswap) errMsg("TIMESWAP", " s"< swap Y and Z components everywhere + //? } +#endif + + initTime(0.f, 0.f); +} + +#undef TRISWAP + +// -------------------------------------------------------------------------- +// init for a given time +void ControlParticles::initTime(LbmFloat t, LbmFloat dt) +{ + //fprintf(stdout, "CPINITTIME init %f\n",t); + _currTime = t; + if(mPartSets.size()<1) return; + + // init zero velocities + initTimeArray(t, _particles); + + // calculate velocities from prev. timestep? + if(dt>0.) { + _currTimestep = dt; + std::vector prevparts; + initTimeArray(t-dt, prevparts); + LbmFloat invdt = 1.0/dt; + for(size_t j=0; j<_particles.size(); j++) { + ControlParticle &p = _particles[j]; + ControlParticle &prevp = prevparts[j]; + for(int k=0; k<3; k++) { + p.pos[k] *= _initPartScale[k]; + p.pos[k] += _initPartOffset[k]; + prevp.pos[k] *= _initLastPartScale[k]; + prevp.pos[k] += _initLastPartOffset[k]; + } + p.vel = (p.pos - prevp.pos)*invdt; + } + + if(0) { + LbmVec avgvel(0.); + for(size_t j=0; j<_particles.size(); j++) { + avgvel += _particles[j].vel; + } + avgvel /= (LbmFloat)_particles.size(); + //fprintf(stdout," AVGVEL %f,%f,%f \n",avgvel[0],avgvel[1],avgvel[2]); // DEBUG + } + } +} + +// helper, init given array +void ControlParticles::initTimeArray(LbmFloat t, std::vector &parts) { + if(mPartSets.size()<1) return; + + if(parts.size()!=mPartSets[0].particles.size()) { + //fprintf(stdout,"PRES \n"); + parts.resize(mPartSets[0].particles.size()); + // TODO reset all? + for(size_t j=0; jt)) { + LbmFloat d = mPartSets[i+1].time-mPartSets[i].time; + LbmFloat f = (t-mPartSets[i].time)/d; + LbmFloat omf = 1.0f - f; + + for(size_t j=0; jpos * omf + src2->pos *f; + p.vel = LbmVec(0.); // reset, calculated later on src1->vel * omf + src2->vel *f; + p.rotaxis = src1->rotaxis * omf + src2->rotaxis *f; + p.influence = src1->influence * omf + src2->influence *f; + p.size = src1->size * omf + src2->size *f; + // dont modify: density, densityWeight + } + } + } + + // after last? + if(t>=mPartSets[ mPartSets.size() -1 ].time) { + //parts = mPartSets[ mPartSets.size() -1 ].particles; + const int i= (int)mPartSets.size() -1; + for(size_t j=0; jgetInfluenceAttraction()<0.) { + cp->density= + cp->densityWeight = 1.0; + continue; + } + + // normalize by kernel + //cp->densityWeight = (1.0 - (cp->density / _kernelWeight)); // store last +#if (CP_PROJECT2D==1) && (defined(MAIN_2D) || LBMDIM==2) + cp->densityWeight = (1.0 - (cp->density / (_kernelWeight*cp->size*cp->size) )); // store last +#else // 2D + cp->densityWeight = (1.0 - (cp->density / (_kernelWeight*cp->size*cp->size*cp->size) )); // store last +#endif // MAIN_2D + + if(i<10) debMsgStd("ControlParticle::prepareControl",DM_MSG,"kernelDebug i="<density,cp->densityWeight); } + avgdw /= (LbmFloat)(_particles.size()); + //if(motion) { printf("ControlParticle::kernel: avgdw:%f, kw%f, sp%f \n", avgdw, _kernelWeight, _fluidSpacing); } + + //if((simtime>=0.) && (simtime != _currTime)) + initTime(simtime, dt); + + if((motion) && (motion->getSize()>0)){ + ControlParticle *motionp = motion->getParticle(0); + //printf("ControlParticle::prepareControl motion: pos[%f,%f,%f] vel[%f,%f,%f] \n", motionp->pos[0], motionp->pos[1], motionp->pos[2], motionp->vel[0], motionp->vel[1], motionp->vel[2] ); + for(size_t i=0; i<_particles.size(); i++) { + ControlParticle *cp = &_particles[i]; + cp->pos = cp->pos + motionp->pos; + cp->vel = cp->vel + motionp->vel; + cp->size = cp->size * motionp->size; + cp->influence = cp->size * motionp->influence; + } + } + + // reset to radiusAtt by default + if(_radiusVel==0.) _radiusVel = _radiusAtt; + if(_radiusMinMaxd==0.) _radiusMinMaxd = _radiusAtt; + if(_radiusMaxd==0.) _radiusMaxd = 2.*_radiusAtt; + // has to be radiusVel_radiusAtt) _radiusVel = _radiusAtt; + if(_radiusAtt>_radiusMinMaxd) _radiusAtt = _radiusMinMaxd; + if(_radiusMinMaxd>_radiusMaxd) _radiusMinMaxd = _radiusMaxd; + + //printf("ControlParticle::radii vel:%f att:%f min:%f max:%f \n", _radiusVel,_radiusAtt,_radiusMinMaxd,_radiusMaxd); + // prepareControl done +} + +void ControlParticles::finishControl(std::vector &forces, LbmFloat iatt, LbmFloat ivel, LbmFloat imaxd) { + + //const LbmFloat iatt = this->getInfluenceAttraction() * this->getCurrTimestep(); + //const LbmFloat ivel = this->getInfluenceVelocity(); + //const LbmFloat imaxd = this->getInfluenceMaxdist() * this->getCurrTimestep(); + // prepare for usage + iatt *= this->getCurrTimestep(); + ivel *= 1.; // not necessary! + imaxd *= this->getCurrTimestep(); + + // skip when size=0 + for(int i=0; i<(int)forces.size(); i++) { + if(DEBUG_MODVEL) fprintf(stdout, "CPFORGF %d , wf:%f,f:%f,%f,%f , v:%f,%f,%f \n",i, forces[i].weightAtt, forces[i].forceAtt[0],forces[i].forceAtt[1],forces[i].forceAtt[2], forces[i].forceVel[0], forces[i].forceVel[1], forces[i].forceVel[2] ); + LbmFloat cfweight = forces[i].weightAtt; // always normalize + if((cfweight!=0.)&&(iatt!=0.)) { + // multiple kernels, normalize - note this does not normalize in d>r/2 region + if(ABS(cfweight)>1.) { cfweight = 1.0/cfweight; } + // multiply iatt afterwards to allow stronger force + cfweight *= iatt; + forces[i].forceAtt *= cfweight; + } else { + forces[i].weightAtt = 0.; + forces[i].forceAtt = LbmVec(0.); + } + + if( (cfweight==0.) && (imaxd>0.) && (forces[i].maxDistance>0.) ) { + forces[i].forceMaxd *= imaxd; + } else { + forces[i].maxDistance= 0.; + forces[i].forceMaxd = LbmVec(0.); + } + + LbmFloat cvweight = forces[i].weightVel; // always normalize + if(cvweight>0.) { + forces[i].forceVel /= cvweight; + forces[i].compAv /= cvweight; + // now modify cvweight, and write back + // important, cut at 1 - otherwise strong vel. influences... + if(cvweight>1.) { cvweight = 1.; } + // thus cvweight is in the range of 0..influenceVelocity, currently not normalized by numCParts + cvweight *= ivel; + if(cvweight<0.) cvweight=0.; if(cvweight>1.) cvweight=1.; + // LBM, FIXME todo use relaxation factor + //pvel = (cvel*0.5 * cvweight) + (pvel * (1.0-cvweight)); + forces[i].weightVel = cvweight; + + //errMsg("COMPAV","i"<density,cp->densityWeight, (1.0 - (12.0*cp->densityWeight))); } + //fprintf(stdout,"\n\nCP DONE \n\n\n"); +} + + +// -------------------------------------------------------------------------- +// calculate forces at given position, and modify velocity +// according to timestep +void ControlParticles::calculateCpInfluenceOpt(ControlParticle *cp, LbmVec fluidpos, LbmVec fluidvel, ControlForces *force, LbmFloat fillFactor) { + // dont reset, only add... + // test distance, simple squared distance reject + const LbmFloat cpfo = _radiusAtt*cp->size; + + LbmVec posDelta; + if(DEBUG_MODVEL) fprintf(stdout, "CP at %f,%f,%f bef fw:%f, f:%f,%f,%f , vw:%f, v:%f,%f,%f \n",fluidpos[0],fluidpos[1],fluidpos[2], force->weightAtt, force->forceAtt[0], force->forceAtt[1], force->forceAtt[2], force->weightVel, force->forceVel[0], force->forceVel[1], force->forceVel[2]); + posDelta = cp->pos - fluidpos; +#if LBMDIM==2 && (CP_PROJECT2D==1) + posDelta[2] = 0.; // project to xy plane, z-velocity should already be gone... +#endif + + const LbmFloat distsqr = posDelta[0]*posDelta[0]+posDelta[1]*posDelta[1]+posDelta[2]*posDelta[2]; + if(DEBUG_MODVEL) fprintf(stdout, " Pd at %f,%f,%f d%f \n",posDelta[0],posDelta[1],posDelta[2], distsqr); + // cut at influence=0.5 , scaling not really makes sense + if(cpfo*cpfo < distsqr) { + /*if(cp->influence>0.5) { + if(force->weightAtt == 0.) { + if(force->maxDistance*force->maxDistance > distsqr) { + const LbmFloat dis = sqrtf((float)distsqr); + const LbmFloat sc = dis-cpfo; + force->maxDistance = dis; + force->forceMaxd = (posDelta)*(sc/dis); + } + } } */ + return; + } + force->weightAtt += 1e-6; // for distance + force->maxDistance = 0.; // necessary for SPH? + + const LbmFloat pdistance = MAGNITUDE(posDelta); + LbmFloat pdistinv = 0.; + if(ABS(pdistance)>0.) pdistinv = 1./pdistance; + posDelta *= pdistinv; + + LbmFloat falloffAtt = 0.; //CPKernel::kernel(cpfo * 1.0, pdistance); + const LbmFloat qac = pdistance / cpfo ; + if (qac < 1.0){ // return 0.; + if(qac < 0.5) falloffAtt = 1.0f; + else falloffAtt = (1.0f - qac) * 2.0f; + } + + // vorticity force: + // - //LbmVec forceVort; + // - //CROSS(forceVort, posDelta, cp->rotaxis); + // - //NORMALIZE(forceVort); + // - if(falloffAtt>1.0) falloffAtt=1.0; + +#if (CP_PROJECT2D==1) && (defined(MAIN_2D) || LBMDIM==2) + // fillFactor *= 2.0 *0.75 * pdistance; // 2d>3d sampling +#endif // (CP_PROJECT2D==1) && (defined(MAIN_2D) || LBMDIM==2) + + LbmFloat signum = getInfluenceAttraction() > 0.0 ? 1.0 : -1.0; + cp->density += falloffAtt * fillFactor; + force->forceAtt += posDelta *cp->densityWeight *cp->influence *signum; + force->weightAtt += falloffAtt*cp->densityWeight *cp->influence; + + LbmFloat falloffVel = 0.; //CPKernel::kernel(cpfo * 1.0, pdistance); + const LbmFloat cpfv = _radiusVel*cp->size; + if(cpfv*cpfv < distsqr) { return; } + const LbmFloat qvc = pdistance / cpfo ; + //if (qvc < 1.0){ + //if(qvc < 0.5) falloffVel = 1.0f; + //else falloffVel = (1.0f - qvc) * 2.0f; + //} + falloffVel = 1.-qvc; + + LbmFloat pvWeight; // = (1.0-cp->densityWeight) * _currTimestep * falloffVel; + pvWeight = falloffVel *cp->influence; // std, without density influence + //pvWeight *= (1.0-cp->densityWeight); // use inverse density weight + //pvWeight *= cp->densityWeight; // test, use density weight + LbmVec modvel(0.); + modvel += cp->vel * pvWeight; + //pvWeight = 1.; modvel = partVel; // DEBUG!? + + if(pvWeight>0.) { + force->forceVel += modvel; + force->weightVel += pvWeight; + + cp->avgVelWeight += falloffVel; + cp->avgVel += fluidvel; + } + if(DEBUG_MODVEL) fprintf(stdout, "CP at %f,%f,%f aft fw:%f, f:%f,%f,%f , vw:%f, v:%f,%f,%f \n",fluidpos[0],fluidpos[1],fluidpos[2], force->weightAtt, force->forceAtt[0], force->forceAtt[1], force->forceAtt[2], force->weightVel, force->forceVel[0], force->forceVel[1], force->forceVel[2]); + return; +} + +void ControlParticles::calculateMaxdForce(ControlParticle *cp, LbmVec fluidpos, ControlForces *force) { + if(force->weightAtt != 0.) return; // maxd force off + if(cp->influence <= 0.5) return; // ignore + + LbmVec posDelta; + //if(DEBUG_MODVEL) fprintf(stdout, "CP at %f,%f,%f bef fw:%f, f:%f,%f,%f , vw:%f, v:%f,%f,%f \n",fluidpos[0],fluidpos[1],fluidpos[2], force->weightAtt, force->forceAtt[0], force->forceAtt[1], force->forceAtt[2], force->weightVel, force->forceVel[0], force->forceVel[1], force->forceVel[2]); + posDelta = cp->pos - fluidpos; +#if LBMDIM==2 && (CP_PROJECT2D==1) + posDelta[2] = 0.; // project to xy plane, z-velocity should already be gone... +#endif + + // dont reset, only add... + // test distance, simple squared distance reject + const LbmFloat distsqr = posDelta[0]*posDelta[0]+posDelta[1]*posDelta[1]+posDelta[2]*posDelta[2]; + + // closer cp found + if(force->maxDistance*force->maxDistance < distsqr) return; + + const LbmFloat dmin = _radiusMinMaxd*cp->size; + if(distsqrsize; + if(distsqr>dmax*dmax) return; // outside + + + if(DEBUG_MODVEL) fprintf(stdout, " Pd at %f,%f,%f d%f \n",posDelta[0],posDelta[1],posDelta[2], distsqr); + // cut at influence=0.5 , scaling not really makes sense + const LbmFloat dis = sqrtf((float)distsqr); + //const LbmFloat sc = dis - dmin; + const LbmFloat sc = (dis-dmin)/(dmax-dmin); // scale from 0-1 + force->maxDistance = dis; + force->forceMaxd = (posDelta/dis) * sc; + //debug errMsg("calculateMaxdForce","pos"<maxDistance <<" fmd"<forceMaxd ); + return; +} + diff --git a/intern/elbeem/intern/controlparticles.h b/intern/elbeem/intern/controlparticles.h new file mode 100644 index 00000000000..712dfc40273 --- /dev/null +++ b/intern/elbeem/intern/controlparticles.h @@ -0,0 +1,301 @@ +// -------------------------------------------------------------------------- +// +// El'Beem - the visual lattice boltzmann freesurface simulator +// All code distributed as part of El'Beem is covered by the version 2 of the +// GNU General Public License. See the file COPYING for details. +// +// Copyright 2008 Nils Thuerey , Richard Keiser, Mark Pauly, Ulrich Ruede +// +// control particle classes +// +// -------------------------------------------------------------------------- + +#ifndef CONTROLPARTICLES_H +#define CONTROLPARTICLES_H + +#include "ntl_geometrymodel.h" + +// indicator for LBM inclusion +//#ifndef LBMDIM + +//#include +//#include +//class MultisphGUI; +//#define NORMALIZE(a) a.normalize() +//#define MAGNITUDE(a) a.magnitude() +//#define CROSS(a,b,c) a.cross(b,c) +//#define ABS(a) (a>0. ? (a) : -(a)) +//#include "cpdefines.h" + +//#else // LBMDIM + +// use compatibility defines +//#define NORMALIZE(a) normalize(a) +//#define MAGNITUDE(a) norm(a) +//#define CROSS(a,b,c) a=cross(b,c) + +//#endif // LBMDIM + +#define MAGNITUDE(a) norm(a) + +// math.h compatibility +#define CP_PI ((LbmFloat)3.14159265358979323846) + +// project 2d test cases onto plane? +// if not, 3d distance is used for 2d sim as well +#define CP_PROJECT2D 1 + + +// default init for mincpdist, ControlForces::maxDistance +#define CPF_MAXDINIT 10000. + +// storage of influence for a fluid cell/particle in lbm/sph +class ControlForces +{ +public: + ControlForces() { }; + ~ControlForces() {}; + + // attraction force + LbmFloat weightAtt; + LbmVec forceAtt; + // velocity influence + LbmFloat weightVel; + LbmVec forceVel; + // maximal distance influence, + // first is max. distance to first control particle + // second attraction strength + LbmFloat maxDistance; + LbmVec forceMaxd; + + LbmFloat compAvWeight; + LbmVec compAv; + + void resetForces() { + weightAtt = weightVel = 0.; + maxDistance = CPF_MAXDINIT; + forceAtt = forceVel = forceMaxd = LbmVec(0.,0.,0.); + compAvWeight=0.; compAv=LbmVec(0.); + }; +}; + + +// single control particle +class ControlParticle +{ +public: + ControlParticle() { reset(); }; + ~ControlParticle() {}; + + // control parameters + + // position + LbmVec pos; + // size (influences influence radius) + LbmFloat size; + // overall strength of influence + LbmFloat influence; + // rotation axis + LbmVec rotaxis; + + // computed values + + // velocity + LbmVec vel; + // computed density + LbmFloat density; + LbmFloat densityWeight; + + LbmVec avgVel; + LbmVec avgVelAcc; + LbmFloat avgVelWeight; + + // init all zero / defaults + void reset(); +}; + + +// container for a particle configuration at time t +class ControlParticleSet +{ +public: + + // time of particle set + LbmFloat time; + // particle positions + std::vector particles; + +}; + + +// container & management of control particles +class ControlParticles +{ +public: + ControlParticles(); + ~ControlParticles(); + + // reset datastructures for next influence step + // if motion object is given, particle 1 of second system is used for overall + // position and speed offset + void prepareControl(LbmFloat simtime, LbmFloat dt, ControlParticles *motion); + // post control operations + void finishControl(std::vector &forces, LbmFloat iatt, LbmFloat ivel, LbmFloat imaxd); + // recalculate + void calculateKernelWeight(); + + // calculate forces at given position, and modify velocity + // according to timestep (from initControl) + void calculateCpInfluenceOpt (ControlParticle *cp, LbmVec fluidpos, LbmVec fluidvel, ControlForces *force, LbmFloat fillFactor); + void calculateMaxdForce (ControlParticle *cp, LbmVec fluidpos, ControlForces *force); + + // no. of particles + inline int getSize() { return (int)_particles.size(); } + int getTotalSize(); + // get particle [i] + inline ControlParticle* getParticle(int i){ return &_particles[i]; } + + // set influence parameters + void setInfluenceTangential(LbmFloat set) { _influenceTangential=set; } + void setInfluenceAttraction(LbmFloat set) { _influenceAttraction=set; } + void setInfluenceMaxdist(LbmFloat set) { _influenceMaxdist=set; } + // calculate for delta t + void setInfluenceVelocity(LbmFloat set, LbmFloat dt); + // get influence parameters + inline LbmFloat getInfluenceAttraction() { return _influenceAttraction; } + inline LbmFloat getInfluenceTangential() { return _influenceTangential; } + inline LbmFloat getInfluenceVelocity() { return _influenceVelocity; } + inline LbmFloat getInfluenceMaxdist() { return _influenceMaxdist; } + inline LbmFloat getCurrTimestep() { return _currTimestep; } + + void setRadiusAtt(LbmFloat set) { _radiusAtt=set; } + inline LbmFloat getRadiusAtt() { return _radiusAtt; } + void setRadiusVel(LbmFloat set) { _radiusVel=set; } + inline LbmFloat getRadiusVel() { return _radiusVel; } + void setRadiusMaxd(LbmFloat set) { _radiusMaxd=set; } + inline LbmFloat getRadiusMaxd() { return _radiusMaxd; } + void setRadiusMinMaxd(LbmFloat set) { _radiusMinMaxd=set; } + inline LbmFloat getRadiusMinMaxd() { return _radiusMinMaxd; } + + LbmFloat getControlTimStart(); + LbmFloat getControlTimEnd(); + + // set/get characteristic length (and inverse) + void setCharLength(LbmFloat set) { _charLength=set; _charLengthInv=1./_charLength; } + inline LbmFloat getCharLength() { return _charLength;} + inline LbmFloat getCharLengthInv() { return _charLengthInv;} + + // set init parameters + void setInitTimeScale(LbmFloat set) { _initTimeScale = set; }; + void setInitMirror(string set) { _initMirror = set; }; + string getInitMirror() { return _initMirror; }; + + void setLastOffset(LbmVec set) { _initLastPartOffset = set; }; + void setLastScale(LbmVec set) { _initLastPartScale = set; }; + void setOffset(LbmVec set) { _initPartOffset = set; }; + void setScale(LbmVec set) { _initPartScale = set; }; + + // set/get cps params + void setCPSWith(LbmFloat set) { mCPSWidth = set; }; + void setCPSTimestep(LbmFloat set) { mCPSTimestep = set; }; + void setCPSTimeStart(LbmFloat set) { mCPSTimeStart = set; }; + void setCPSTimeEnd(LbmFloat set) { mCPSTimeEnd = set; }; + void setCPSMvmWeightFac(LbmFloat set) { mCPSWeightFac = set; }; + + LbmFloat getCPSWith() { return mCPSWidth; }; + LbmFloat getCPSTimestep() { return mCPSTimestep; }; + LbmFloat getCPSTimeStart() { return mCPSTimeStart; }; + LbmFloat getCPSTimeEnd() { return mCPSTimeEnd; }; + LbmFloat getCPSMvmWeightFac() { return mCPSWeightFac; }; + + void setDebugInit(int set) { mDebugInit = set; }; + + // set init parameters + void setFluidSpacing(LbmFloat set) { _fluidSpacing = set; }; + + // load positions & timing from text file + int initFromTextFile(string filename); + int initFromTextFileOld(string filename); + // load positions & timing from gzipped binary file + int initFromBinaryFile(string filename); + int initFromMVCMesh(string filename); + // init an example test case + int initExampleSet(); + + // init for a given time + void initTime(LbmFloat t, LbmFloat dt); + + // blender test init + void initBlenderTest(); + + int initFromObject(ntlGeometryObjModel *model); + +protected: + // sets influence params + friend class MultisphGUI; + + // tangential and attraction influence + LbmFloat _influenceTangential, _influenceAttraction; + // direct velocity influence + LbmFloat _influenceVelocity; + // maximal distance influence + LbmFloat _influenceMaxdist; + + // influence radii + LbmFloat _radiusAtt, _radiusVel, _radiusMinMaxd, _radiusMaxd; + + // currently valid time & timestep + LbmFloat _currTime, _currTimestep; + // all particles + std::vector _particles; + + // particle sets + std::vector mPartSets; + + // additional parameters for initing particles + LbmFloat _initTimeScale; + LbmVec _initPartOffset; + LbmVec _initPartScale; + LbmVec _initLastPartOffset; + LbmVec _initLastPartScale; + // mirror particles for loading? + string _initMirror; + + // row spacing paramter, e.g. use for approximation of kernel area/volume + LbmFloat _fluidSpacing; + // save current kernel weight + LbmFloat _kernelWeight; + // charateristic length in world coordinates for normalizatioon of forces + LbmFloat _charLength, _charLengthInv; + + + /*! do ani mesh CPS */ + void calculateCPS(string filename); + //! ani mesh cps params + ntlVec3Gfx mvCPSStart, mvCPSEnd; + gfxReal mCPSWidth, mCPSTimestep; + gfxReal mCPSTimeStart, mCPSTimeEnd; + gfxReal mCPSWeightFac; + + int mDebugInit; + + +protected: + // apply init transformations + void applyTrafos(); + + // helper function for init -> swap components everywhere + void swapCoords(int a,int b); + // helper function for init -> mirror time + void mirrorTime(); + + // helper, init given array + void initTimeArray(LbmFloat t, std::vector &parts); + + bool checkPointInside(ntlTree *tree, ntlVec3Gfx org, gfxReal &distance); +}; + + + +#endif + diff --git a/intern/elbeem/intern/elbeem_control.cpp b/intern/elbeem/intern/elbeem_control.cpp new file mode 100644 index 00000000000..800167baf73 --- /dev/null +++ b/intern/elbeem/intern/elbeem_control.cpp @@ -0,0 +1,25 @@ +/****************************************************************************** + * + * El'Beem - Free Surface Fluid Simulation with the Lattice Boltzmann Method + * All code distributed as part of El'Beem is covered by the version 2 of the + * GNU General Public License. See the file COPYING for details. + * Copyright 2003-2006 Nils Thuerey + * + * Control API header + */ + +#include "elbeem.h" +#include "elbeem_control.h" + +// add mesh as fluidsim object +int elbeemControlAddSet(struct elbeemControl*) { + + return 0; +} + +int elbeemControlComputeMesh(struct elbeemMesh*) { + + + return 0; +} + diff --git a/intern/elbeem/intern/elbeem_control.h b/intern/elbeem/intern/elbeem_control.h new file mode 100644 index 00000000000..70a58feda89 --- /dev/null +++ b/intern/elbeem/intern/elbeem_control.h @@ -0,0 +1,62 @@ +/****************************************************************************** + * + * El'Beem - Free Surface Fluid Simulation with the Lattice Boltzmann Method + * All code distributed as part of El'Beem is covered by the version 2 of the + * GNU General Public License. See the file COPYING for details. + * Copyright 2003-2006 Nils Thuerey + * + * Control API header + */ +#ifndef ELBEEMCONTROL_API_H +#define ELBEEMCONTROL_API_H + +// a single control particle set +typedef struct elbeemControl { + /* influence forces */ + float influenceAttraction; + float *channelInfluenceAttraction; + float channelSizeInfluenceAttraction; + + float influenceVelocity; + float *channelInfluenceVelocity; + float channelSizeInfluenceVelocity; + + float influenceMaxdist; + float *channelInfluenceMaxdist; + float channelSizeInfluenceMaxdist; + + /* influence force radii */ + float radiusAttraction; + float *channelRadiusAttraction; + float channelSizeRadiusAttraction; + + float radiusVelocity; + float *channelRadiusVelocity; + float channelSizeRadiusVelocity; + + float radiusMindist; + float *channelRadiusMindist; + float channelSizeRadiusMindist; + float radiusMaxdist; + float *channelRadiusMaxdist; + float channelSizeRadiusMaxdist; + + /* control particle positions/scale */ + float offset[3]; + float *channelOffset; + float channelSizeOffset; + + float scale[3]; + float *channelScale; + float channelSizeScale; + +} elbeemControl; + + +// add mesh as fluidsim object +int elbeemControlAddSet(struct elbeemControl*); + +// sample & track mesh control particles, TODO add return type... +int elbeemControlComputeMesh(struct elbeemMesh*); + +#endif // ELBEEMCONTROL_API_H diff --git a/intern/elbeem/intern/mvmcoords.cpp b/intern/elbeem/intern/mvmcoords.cpp new file mode 100644 index 00000000000..ac954b956d0 --- /dev/null +++ b/intern/elbeem/intern/mvmcoords.cpp @@ -0,0 +1,193 @@ +/****************************************************************************** + * +// El'Beem - the visual lattice boltzmann freesurface simulator +// All code distributed as part of El'Beem is covered by the version 2 of the +// GNU General Public License. See the file COPYING for details. +// +// Copyright 2008 Nils Thuerey , Richard Keiser, Mark Pauly, Ulrich Ruede +// + * + * Mean Value Mesh Coords class + * + *****************************************************************************/ + +#include "mvmcoords.h" +#include +using std::vector; + +void MeanValueMeshCoords::clear() +{ + mVertices.resize(0); + mNumVerts = 0; +} + +void MeanValueMeshCoords::calculateMVMCs(vector &reference_vertices, vector &tris, + vector &points, gfxReal numweights) +{ + clear(); + mvmTransferPoint tds; + int mem = 0; + int i = 0; + + mNumVerts = (int)reference_vertices.size(); + + for (vector::iterator iter = points.begin(); iter != points.end(); ++iter, ++i) { + /* + if(i%(points.size()/10)==1) debMsgStd("MeanValueMeshCoords::calculateMVMCs",DM_MSG,"Computing weights, points: "< &reference_vertices, vector& tris, + mvmTransferPoint& tds, gfxReal numweights) +{ + const bool mvmFullDebug=false; + //const ntlVec3Gfx cEPS = 1.0e-6; + const mvmFloat cEPS = 1.0e-14; + + //mvmFloat d[3], s[3], phi[3],c[3]; + ntlVec3d u[3],c,d,s,phi; + int indices[3]; + + for (int i = 0; i < (int)reference_vertices.size(); ++i) { + tds.weights.push_back(mvmIndexWeight(i, 0.0)); + } + + // for each triangle + //for (vector::iterator iter = tris.begin(); iter != tris.end();) { + for(int t=0; t<(int)tris.size(); t++) { + + for (int i = 0; i < 3; ++i) { //, ++iter) { + indices[i] = tris[t].getPoints()[i]; + u[i] = vec2D(reference_vertices[ indices[i] ]-tds.lastpos); + d[i] = normalize(u[i]); //.normalize(); + //assert(d[i] != 0.); + if(mvmFullDebug) errMsg("MeanValueMeshCoords::computeWeights","t"< ignore it"); + continue; + } + const mvmFloat u0x = u[0][0]; + const mvmFloat u0y = u[0][1]; + const mvmFloat u0z = u[0][2]; + const mvmFloat u1x = u[1][0]; + const mvmFloat u1y = u[1][1]; + const mvmFloat u1z = u[1][2]; + const mvmFloat u2x = u[2][0]; + const mvmFloat u2y = u[2][1]; + const mvmFloat u2z = u[2][2]; + mvmFloat det = u0x*u1y*u2z - u0x*u1z*u2y + u0y*u1z*u2x - u0y*u1x*u2z + u0z*u1x*u2y - u0z*u1y*u2x; + //assert(det != 0.); + if (det < 0.) { + s[0] = -s[0]; + s[1] = -s[1]; + s[2] = -s[2]; + } + + tds.weights[indices[0]].weight += (phi[0]-c[1]*phi[2]-c[2]*phi[1])/(d[0]*sin(phi[1])*s[2]); + tds.weights[indices[1]].weight += (phi[1]-c[2]*phi[0]-c[0]*phi[2])/(d[1]*sin(phi[2])*s[0]); + tds.weights[indices[2]].weight += (phi[2]-c[0]*phi[1]-c[1]*phi[0])/(d[2]*sin(phi[0])*s[1]); + if(mvmFullDebug) { errMsg("MeanValueMeshCoords::computeWeights","i"<0.)&& (numweights<1.) ) { + //if( ((int)tds.weights.size() > maxNumWeights) && (maxNumWeights > 0) ) { + int maxNumWeights = (int)(tds.weights.size()*numweights); + if(maxNumWeights<=0) maxNumWeights = 1; + std::sort(tds.weights.begin(), tds.weights.end(), std::greater()); + // only use maxNumWeights-th largest weights + tds.weights.resize(maxNumWeights); + } + + // normalize weights + mvmFloat totalWeight = 0.; + for (vector::const_iterator witer = tds.weights.begin(); + witer != tds.weights.end(); ++witer) { + totalWeight += witer->weight; + } + mvmFloat invTotalWeight; + if (totalWeight == 0.) { + if(mvmFullDebug) errMsg("MeanValueMeshCoords::computeWeights","totalWeight == 0"); + invTotalWeight = 0.0; + } else { + invTotalWeight = 1.0/totalWeight; + } + + for (vector::iterator viter = tds.weights.begin(); + viter != tds.weights.end(); ++viter) { + viter->weight *= invTotalWeight; + //assert(finite(viter->weight) != 0); + if(!finite(viter->weight)) viter->weight=0.; + } +} + +void MeanValueMeshCoords::transfer(vector &vertices, vector& displacements) +{ + displacements.resize(0); + + //debMsgStd("MeanValueMeshCoords::transfer",DM_MSG,"vertices:"<::iterator titer = mVertices.begin(); titer != mVertices.end(); ++titer) { + mvmTransferPoint &tds = *titer; + ntlVec3Gfx newpos(0.0); + + for (vector::iterator witer = tds.weights.begin(); + witer != tds.weights.end(); ++witer) { + newpos += vertices[witer->index] * witer->weight; + //errMsg("transfer","np"<index]<<" w"<< witer->weight); + } + + displacements.push_back(newpos); + //displacements.push_back(newpos - tds.lastpos); + //tds.lastpos = newpos; + } +} + diff --git a/intern/elbeem/intern/mvmcoords.h b/intern/elbeem/intern/mvmcoords.h new file mode 100644 index 00000000000..889f5058a09 --- /dev/null +++ b/intern/elbeem/intern/mvmcoords.h @@ -0,0 +1,89 @@ +/****************************************************************************** + * +// El'Beem - the visual lattice boltzmann freesurface simulator +// All code distributed as part of El'Beem is covered by the version 2 of the +// GNU General Public License. See the file COPYING for details. +// +// Copyright 2008 Nils Thuerey , Richard Keiser, Mark Pauly, Ulrich Ruede +// + * + * Mean Value Mesh Coords class + * + *****************************************************************************/ + +#ifndef MVMCOORDS_H +#define MVMCOORDS_H + +#include "utilities.h" +#include "ntl_ray.h" +#include +#define mvmFloat double + +#ifdef WIN32 +#ifndef FREE_WINDOWS +#include "float.h" +#define isnan(n) _isnan(n) +#define finite _finite +#endif +#endif + +#ifdef sun +#include "ieeefp.h" +#endif + +// weight and triangle index +class mvmIndexWeight { + public: + + mvmIndexWeight() : weight(0.0) {} + + mvmIndexWeight(int const& i, mvmFloat const& w) : + weight(w), index(i) {} + + // for sorting + bool operator> (mvmIndexWeight const& w) const { return this->weight > w.weight; } + bool operator< (mvmIndexWeight const& w) const { return this->weight < w.weight; } + + mvmFloat weight; + int index; +}; + +// transfer point with weights +class mvmTransferPoint { + public: + //! position of transfer point + ntlVec3Gfx lastpos; + //! triangle weights + std::vector weights; +}; + + +//! compute mvmcs +class MeanValueMeshCoords { + + public: + + MeanValueMeshCoords() {} + ~MeanValueMeshCoords() { + clear(); + } + + void clear(); + + void calculateMVMCs(std::vector &reference_vertices, + std::vector &tris, std::vector &points, gfxReal numweights); + + void transfer(std::vector &vertices, std::vector& displacements); + + protected: + + void computeWeights(std::vector &reference_vertices, + std::vector &tris, mvmTransferPoint& tds, gfxReal numweights); + + std::vector mVertices; + int mNumVerts; + +}; + +#endif + diff --git a/intern/elbeem/intern/solver_control.cpp b/intern/elbeem/intern/solver_control.cpp new file mode 100644 index 00000000000..15b9468ed0d --- /dev/null +++ b/intern/elbeem/intern/solver_control.cpp @@ -0,0 +1,1016 @@ +/****************************************************************************** + * + * El'Beem - the visual lattice boltzmann freesurface simulator + * All code distributed as part of El'Beem is covered by the version 2 of the + * GNU General Public License. See the file COPYING for details. + * + * Copyright 2003-2008 Nils Thuerey + * + * control extensions + * + *****************************************************************************/ +#include "solver_class.h" +#include "solver_relax.h" +#include "particletracer.h" + +#include "solver_control.h" + +#include "controlparticles.h" + +#include "elbeem.h" + +#include "ntl_geometrymodel.h" + +/****************************************************************************** + * LbmControlData control set + *****************************************************************************/ + +LbmControlSet::LbmControlSet() : + mCparts(NULL), mCpmotion(NULL), mContrPartFile(""), mCpmotionFile(""), + mcForceAtt(0.), mcForceVel(0.), mcForceMaxd(0.), + mcRadiusAtt(0.), mcRadiusVel(0.), mcRadiusMind(0.), mcRadiusMaxd(0.), + mcCpScale(1.), mcCpOffset(0.) +{ +} +LbmControlSet::~LbmControlSet() { + if(mCparts) delete mCparts; + if(mCpmotion) delete mCpmotion; +} +void LbmControlSet::initCparts() { + mCparts = new ControlParticles(); + mCpmotion = new ControlParticles(); +} + + + +/****************************************************************************** + * LbmControlData control + *****************************************************************************/ + +LbmControlData::LbmControlData() : + mSetForceStrength(0.), + mCons(), + mCpUpdateInterval(8), // DG: was 16 --> causes problems (big sphere after some time), unstable + mCpOutfile(""), + mCpForces(), mCpKernel(), mMdKernel(), + mDiffVelCon(1.), + mDebugCpscale(0.), + mDebugVelScale(0.), + mDebugCompavScale(0.), + mDebugAttScale(0.), + mDebugMaxdScale(0.), + mDebugAvgVelScale(0.) +{ +} + +LbmControlData::~LbmControlData() +{ + while (!mCons.empty()) { + delete mCons.back(); mCons.pop_back(); + } +} + + +void LbmControlData::parseControldataAttrList(AttributeList *attr) { + // controlpart vars + mSetForceStrength = attr->readFloat("tforcestrength", mSetForceStrength,"LbmControlData", "mSetForceStrength", false); + //errMsg("tforcestrength set to "," "<readInt("controlparticle_updateinterval", mCpUpdateInterval,"LbmControlData","mCpUpdateInterval", false); + // tracer output file + mCpOutfile = attr->readString("controlparticle_outfile",mCpOutfile,"LbmControlData","mCpOutfile", false); + if(getenv("ELBEEM_CPOUTFILE")) { + string outfile(getenv("ELBEEM_CPOUTFILE")); + mCpOutfile = outfile; + debMsgStd("LbmControlData::parseAttrList",DM_NOTIFY,"Using envvar ELBEEM_CPOUTFILE to set mCpOutfile to "<0) + { suffix = string("0"); suffix[0]+=cpii; } + LbmControlSet *cset; + cset = new LbmControlSet(); + cset->initCparts(); + + cset->mContrPartFile = attr->readString("controlparticle"+suffix+"_file",cset->mContrPartFile,"LbmControlData","cset->mContrPartFile", false); + if((cpii==0) && (getenv("ELBEEM_CPINFILE")) ) { + string infile(getenv("ELBEEM_CPINFILE")); + cset->mContrPartFile = infile; + debMsgStd("LbmControlData::parseAttrList",DM_NOTIFY,"Using envvar ELBEEM_CPINFILE to set mContrPartFile to "<mContrPartFile,7); + } + + LbmFloat cpvort=0.; + cset->mcRadiusAtt = attr->readChannelSinglePrecFloat("controlparticle"+suffix+"_radiusatt", 0., "LbmControlData","mcRadiusAtt" ); + cset->mcRadiusVel = attr->readChannelSinglePrecFloat("controlparticle"+suffix+"_radiusvel", 0., "LbmControlData","mcRadiusVel" ); + cset->mcRadiusVel = attr->readChannelSinglePrecFloat("controlparticle"+suffix+"_radiusvel", 0., "LbmControlData","mcRadiusVel" ); + cset->mCparts->setRadiusAtt(cset->mcRadiusAtt.get(0.)); + cset->mCparts->setRadiusVel(cset->mcRadiusVel.get(0.)); + + // WARNING currently only for first set + //if(cpii==0) { + cset->mcForceAtt = attr->readChannelSinglePrecFloat("controlparticle"+suffix+"_attraction", 0. , "LbmControlData","cset->mcForceAtt", false); + cset->mcForceVel = attr->readChannelSinglePrecFloat("controlparticle"+suffix+"_velocity", 0. , "LbmControlData","mcForceVel", false); + cset->mcForceMaxd = attr->readChannelSinglePrecFloat("controlparticle"+suffix+"_maxdist", 0. , "LbmControlData","mcForceMaxd", false); + cset->mCparts->setInfluenceAttraction(cset->mcForceAtt.get(0.) ); + // warning - stores temprorarily, value converted to dt dep. factor + cset->mCparts->setInfluenceVelocity(cset->mcForceVel.get(0.) , 0.01 ); // dummy dt + cset->mCparts->setInfluenceMaxdist(cset->mcForceMaxd.get(0.) ); + cpvort = attr->readFloat("controlparticle"+suffix+"_vorticity", cpvort, "LbmControlData","cpvort", false); + cset->mCparts->setInfluenceTangential(cpvort); + + cset->mcRadiusMind = attr->readChannelSinglePrecFloat("controlparticle"+suffix+"_radiusmin", cset->mcRadiusMind.get(0.), "LbmControlData","mcRadiusMind", false); + cset->mcRadiusMaxd = attr->readChannelSinglePrecFloat("controlparticle"+suffix+"_radiusmax", cset->mcRadiusMind.get(0.), "LbmControlData","mcRadiusMaxd", false); + cset->mCparts->setRadiusMinMaxd(cset->mcRadiusMind.get(0.)); + cset->mCparts->setRadiusMaxd(cset->mcRadiusMaxd.get(0.)); + //} + + // now local... + //LbmVec cpOffset(0.), cpScale(1.); + LbmFloat cpTimescale = 1.; + string cpMirroring(""); + + //cset->mcCpOffset = attr->readChannelVec3f("controlparticle"+suffix+"_offset", ntlVec3f(0.),"LbmControlData","mcCpOffset", false); + //cset->mcCpScale = attr->readChannelVec3f("controlparticle"+suffix+"_scale", ntlVec3f(1.), "LbmControlData","mcCpScale", false); + cset->mcCpOffset = attr->readChannelVec3f("controlparticle"+suffix+"_offset", ntlVec3f(0.),"LbmControlData","mcCpOffset", false); + cset->mcCpScale = attr->readChannelVec3f("controlparticle"+suffix+"_scale", ntlVec3f(1.), "LbmControlData","mcCpScale", false); + cpTimescale = attr->readFloat("controlparticle"+suffix+"_timescale", cpTimescale, "LbmControlData","cpTimescale", false); + cpMirroring = attr->readString("controlparticle"+suffix+"_mirror", cpMirroring, "LbmControlData","cpMirroring", false); + + LbmFloat cpsWidth = cset->mCparts->getCPSWith(); + cpsWidth = attr->readFloat("controlparticle"+suffix+"_cpswidth", cpsWidth, "LbmControlData","cpsWidth", false); + LbmFloat cpsDt = cset->mCparts->getCPSTimestep(); + cpsDt = attr->readFloat("controlparticle"+suffix+"_cpstimestep", cpsDt, "LbmControlData","cpsDt", false); + LbmFloat cpsTstart = cset->mCparts->getCPSTimeStart(); + cpsTstart = attr->readFloat("controlparticle"+suffix+"_cpststart", cpsTstart, "LbmControlData","cpsTstart", false); + LbmFloat cpsTend = cset->mCparts->getCPSTimeEnd(); + cpsTend = attr->readFloat("controlparticle"+suffix+"_cpstend", cpsTend, "LbmControlData","cpsTend", false); + LbmFloat cpsMvmfac = cset->mCparts->getCPSMvmWeightFac(); + cpsMvmfac = attr->readFloat("controlparticle"+suffix+"_cpsmvmfac", cpsMvmfac, "LbmControlData","cpsMvmfac", false); + cset->mCparts->setCPSWith(cpsWidth); + cset->mCparts->setCPSTimestep(cpsDt); + cset->mCparts->setCPSTimeStart(cpsTstart); + cset->mCparts->setCPSTimeEnd(cpsTend); + cset->mCparts->setCPSMvmWeightFac(cpsMvmfac); + + cset->mCparts->setOffset( vec2L(cset->mcCpOffset.get(0.)) ); + cset->mCparts->setScale( vec2L(cset->mcCpScale.get(0.)) ); + cset->mCparts->setInitTimeScale( cpTimescale ); + cset->mCparts->setInitMirror( cpMirroring ); + + int mDebugInit = 0; + mDebugInit = attr->readInt("controlparticle"+suffix+"_debuginit", mDebugInit,"LbmControlData","mDebugInit", false); + cset->mCparts->setDebugInit(mDebugInit); + + // motion particle settings + LbmVec mcpOffset(0.), mcpScale(1.); + LbmFloat mcpTimescale = 1.; + string mcpMirroring(""); + + cset->mCpmotionFile = attr->readString("cpmotion"+suffix+"_file",cset->mCpmotionFile,"LbmControlData","mCpmotionFile", false); + mcpTimescale = attr->readFloat("cpmotion"+suffix+"_timescale", mcpTimescale, "LbmControlData","mcpTimescale", false); + mcpMirroring = attr->readString("cpmotion"+suffix+"_mirror", mcpMirroring, "LbmControlData","mcpMirroring", false); + mcpOffset = vec2L( attr->readVec3d("cpmotion"+suffix+"_offset", vec2P(mcpOffset),"LbmControlData","cpOffset", false) ); + mcpScale = vec2L( attr->readVec3d("cpmotion"+suffix+"_scale", vec2P(mcpScale), "LbmControlData","cpScale", false) ); + + cset->mCpmotion->setOffset( vec2L(mcpOffset) ); + cset->mCpmotion->setScale( vec2L(mcpScale) ); + cset->mCpmotion->setInitTimeScale( mcpTimescale ); + cset->mCpmotion->setInitMirror( mcpMirroring ); + + if(cset->mContrPartFile.length()>1) { + errMsg("LbmControlData","Using control particle set "<mContrPartFile<<" cpmfile:"<mCpmotionFile<<" mirr:'"<mCpmotion->getInitMirror()<<"' " ); + mCons.push_back( cset ); + } else { + delete cset; + } + } + + // debug, testing - make sure theres at least an empty set + if(mCons.size()<1) { + mCons.push_back( new LbmControlSet() ); + mCons[0]->initCparts(); + } + + // take from first set + for(int cpii=1; cpii<(int)mCons.size(); cpii++) { + mCons[cpii]->mCparts->setRadiusMinMaxd( mCons[0]->mCparts->getRadiusMinMaxd() ); + mCons[cpii]->mCparts->setRadiusMaxd( mCons[0]->mCparts->getRadiusMaxd() ); + mCons[cpii]->mCparts->setInfluenceAttraction( mCons[0]->mCparts->getInfluenceAttraction() ); + mCons[cpii]->mCparts->setInfluenceTangential( mCons[0]->mCparts->getInfluenceTangential() ); + mCons[cpii]->mCparts->setInfluenceVelocity( mCons[0]->mCparts->getInfluenceVelocity() , 0.01 ); // dummy dt + mCons[cpii]->mCparts->setInfluenceMaxdist( mCons[0]->mCparts->getInfluenceMaxdist() ); + } + + // invert for usage in relax macro + mDiffVelCon = 1.-attr->readFloat("cpdiffvelcon", mDiffVelCon, "LbmControlData","mDiffVelCon", false); + + mDebugCpscale = attr->readFloat("cpdebug_cpscale", mDebugCpscale, "LbmControlData","mDebugCpscale", false); + mDebugMaxdScale = attr->readFloat("cpdebug_maxdscale", mDebugMaxdScale, "LbmControlData","mDebugMaxdScale", false); + mDebugAttScale = attr->readFloat("cpdebug_attscale", mDebugAttScale, "LbmControlData","mDebugAttScale", false); + mDebugVelScale = attr->readFloat("cpdebug_velscale", mDebugVelScale, "LbmControlData","mDebugVelScale", false); + mDebugCompavScale = attr->readFloat("cpdebug_compavscale", mDebugCompavScale, "LbmControlData","mDebugCompavScale", false); + mDebugAvgVelScale = attr->readFloat("cpdebug_avgvelsc", mDebugAvgVelScale, "LbmControlData","mDebugAvgVelScale", false); +} + + +void +LbmFsgrSolver::initCpdata() +{ + // enable for cps via env. vars + //if( (getenv("ELBEEM_CPINFILE")) || (getenv("ELBEEM_CPOUTFILE")) ){ mUseTestdata=1; } + + + // manually switch on! if this is zero, nothing is done... + mpControl->mSetForceStrength = this->mTForceStrength = 1.; + mpControl->mCons.clear(); + + // init all control fluid objects + int numobjs = (int)(mpGiObjects->size()); + for(int o=0; ogetGeoInitType() & FGI_CONTROL) { + // add new control set per object + LbmControlSet *cset; + + cset = new LbmControlSet(); + cset->initCparts(); + + // dont load any file + cset->mContrPartFile = string(""); + + cset->mcForceAtt = obj->getCpsAttrFStr(); + cset->mcRadiusAtt = obj->getCpsAttrFRad(); + cset->mcForceVel = obj->getCpsVelFStr(); + cset->mcRadiusVel = obj->getCpsVelFRad(); + + cset->mCparts->setCPSTimeStart(obj->getCpsTimeStart()); + cset->mCparts->setCPSTimeEnd(obj->getCpsTimeEnd()); + + if(obj->getCpsQuality() > LBM_EPSILON) + cset->mCparts->setCPSWith(1.0 / obj->getCpsQuality()); + + // this value can be left at 0.5: + cset->mCparts->setCPSMvmWeightFac(0.5); + + mpControl->mCons.push_back( cset ); + mpControl->mCons[mpControl->mCons.size()-1]->mCparts->initFromObject(obj); + } + } + + // NT blender integration manual test setup + if(0) { + // manually switch on! if this is zero, nothing is done... + mpControl->mSetForceStrength = this->mTForceStrength = 1.; + mpControl->mCons.clear(); + + // add new set + LbmControlSet *cset; + + cset = new LbmControlSet(); + cset->initCparts(); + // dont load any file + cset->mContrPartFile = string(""); + + // set radii for attraction & velocity forces + // set strength of the forces + // don't set directly! but use channels: + // mcForceAtt, mcForceVel, mcForceMaxd, mcRadiusAtt, mcRadiusVel, mcRadiusMind, mcRadiusMaxd etc. + + // wrong: cset->mCparts->setInfluenceAttraction(1.15); cset->mCparts->setRadiusAtt(1.5); + // right, e.g., to init some constant values: + cset->mcForceAtt = AnimChannel(0.2); + cset->mcRadiusAtt = AnimChannel(0.75); + cset->mcForceVel = AnimChannel(0.2); + cset->mcRadiusVel = AnimChannel(0.75); + + // this value can be left at 0.5: + cset->mCparts->setCPSMvmWeightFac(0.5); + + mpControl->mCons.push_back( cset ); + + // instead of reading from file (cset->mContrPartFile), manually init some particles + mpControl->mCons[0]->mCparts->initBlenderTest(); + + // other values that might be interesting to change: + //cset->mCparts->setCPSTimestep(0.02); + //cset->mCparts->setCPSTimeStart(0.); + //cset->mCparts->setCPSTimeEnd(1.); + + //mpControl->mDiffVelCon = 1.; // more rigid velocity control, 0 (default) allows more turbulence + } + + // control particle ------------------------------------------------------------------------------------- + + // init cppf stage, use set 0! + if(mCppfStage>0) { + if(mpControl->mCpOutfile.length()<1) mpControl->mCpOutfile = string("cpout"); // use getOutFilename !? + char strbuf[100]; + const char *cpFormat = "_d%dcppf%d"; + + // initial coarse stage, no input + if(mCppfStage==1) { + mpControl->mCons[0]->mContrPartFile = ""; + } else { + // read from prev stage + snprintf(strbuf,100, cpFormat ,LBMDIM,mCppfStage-1); + mpControl->mCons[0]->mContrPartFile = mpControl->mCpOutfile; + mpControl->mCons[0]->mContrPartFile += strbuf; + mpControl->mCons[0]->mContrPartFile += ".cpart2"; + } + + snprintf(strbuf,100, cpFormat ,LBMDIM,mCppfStage); + mpControl->mCpOutfile += strbuf; + } // */ + + for(int cpssi=0; cpssi<(int)mpControl->mCons.size(); cpssi++) { + ControlParticles *cparts = mpControl->mCons[cpssi]->mCparts; + ControlParticles *cpmotion = mpControl->mCons[cpssi]->mCpmotion; + + // now set with real dt + cparts->setInfluenceVelocity( mpControl->mCons[cpssi]->mcForceVel.get(0.), mLevel[mMaxRefine].timestep); + cparts->setCharLength( mLevel[mMaxRefine].nodeSize ); + cparts->setCharLength( mLevel[mMaxRefine].nodeSize ); + errMsg("LbmControlData","CppfStage "<mCons[cpssi]->mContrPartFile<< + " out:"<mCpOutfile<<" cl:"<< cparts->getCharLength() ); + + // control particle test init + if(mpControl->mCons[cpssi]->mCpmotionFile.length()>=1) cpmotion->initFromTextFile(mpControl->mCons[cpssi]->mCpmotionFile); + // not really necessary... + //? cparts->setFluidSpacing( mLevel[mMaxRefine].nodeSize ); // use grid coords!? + //? cparts->calculateKernelWeight(); + //? debMsgStd("LbmFsgrSolver::initCpdata",DM_MSG,"ControlParticles - motion inited: "<getSize() ,10); + + // ensure both are on for env. var settings + // when no particles, but outfile enabled, initialize + const int lev = mMaxRefine; + if((mpParticles) && (mpControl->mCpOutfile.length()>=1) && (cpssi==0)) { + // check if auto num + if( (mpParticles->getNumInitialParticles()<=1) && + (mpParticles->getNumParticles()<=1) ) { // initParticles done afterwards anyway + int tracers = 0; + const int workSet = mLevel[lev].setCurr; + FSGR_FORIJK_BOUNDS(lev) { + if(RFLAG(lev,i,j,k, workSet)&(CFFluid)) tracers++; + } + if(LBMDIM==3) tracers /= 8; + else tracers /= 4; + mpParticles->setNumInitialParticles(tracers); + mpParticles->setDumpTextFile(mpControl->mCpOutfile); + debMsgStd("LbmFsgrSolver::initCpdata",DM_MSG,"ControlParticles - set tracers #"<getNumParticles() ,10); + } + if(mpParticles->getDumpTextInterval()<=0.) { + mpParticles->setDumpTextInterval(mLevel[lev].timestep * mLevel[lev].lSizex); + debMsgStd("LbmFsgrSolver::initCpdata",DM_MSG,"ControlParticles - dump delta t not set, using dti="<< mpParticles->getDumpTextInterval()<<", sim dt="<setDumpParts(true); // DEBUG? also dump as particle system + } + + if(mpControl->mCons[cpssi]->mContrPartFile.length()>=1) cparts->initFromTextFile(mpControl->mCons[cpssi]->mContrPartFile); + cparts->setFluidSpacing( mLevel[lev].nodeSize ); // use grid coords!? + cparts->calculateKernelWeight(); + debMsgStd("LbmFsgrSolver::initCpdata",DM_MSG,"ControlParticles mCons"<getTotalSize()<<","<getSize()<<" dt:"<getTimestep()<<" control time:"<getControlTimStart()<<" to "<getControlTimEnd() ,10); + } // cpssi + + if(getenv("ELBEEM_CPINFILE")) { + this->mTForceStrength = 1.0; + } + this->mTForceStrength = mpControl->mSetForceStrength; + if(mpControl->mCpOutfile.length()>=1) mpParticles->setDumpTextFile(mpControl->mCpOutfile); + + // control particle init end ------------------------------------------------------------------------------------- + + // make sure equiv to solver init + if(this->mTForceStrength>0.) { \ + mpControl->mCpForces.resize( mMaxRefine+1 ); + for(int lev = 0; lev<=mMaxRefine; lev++) { + LONGINT rcellSize = (mLevel[lev].lSizex*mLevel[lev].lSizey*mLevel[lev].lSizez); + debMsgStd("LbmFsgrSolver::initControl",DM_MSG,"mCpForces init, lev="<mCpForces[lev].resize( (int)(rcellSize+4) ); + //for(int i=0 ;imCpForces.push_back( ControlForces() ); + for(int i=0 ;imCpForces[lev][i].resetForces(); + } + } // on? + + debMsgStd("LbmFsgrSolver::initCpdata",DM_MSG,"ControlParticles #mCons "<mCons.size()<<" done", 6); +} + + +#define CPODEBUG 0 +//define CPINTER ((int)(mpControl->mCpUpdateInterval)) + +#define KERN(x,y,z) mpControl->mCpKernel[ (((z)*cpkarWidth + (y))*cpkarWidth + (x)) ] +#define MDKERN(x,y,z) mpControl->mMdKernel[ (((z)*mdkarWidth + (y))*mdkarWidth + (x)) ] + +#define BOUNDCHECK(x,low,high) ( ((x)high) ? high : (x) ) ) +#define BOUNDSKIP(x,low,high) ( ((x)high) ) + +void +LbmFsgrSolver::handleCpdata() +{ + myTime_t cpstart = getTime(); + int cpChecks=0; + int cpInfs=0; + //debMsgStd("ControlData::handleCpdata",DM_MSG,"called... "<mTForceStrength,1); + + // add cp influence + if((true) && (this->mTForceStrength>0.)) { + // ok continue... + } // on off + else { + return; + } + + if((mpControl->mCpUpdateInterval<1) || (this->mStepCnt%mpControl->mCpUpdateInterval==0)) { + // do full reinit later on... + } + else if(this->mStepCnt>mpControl->mCpUpdateInterval) { + // only reinit new cells + // TODO !? remove loop dependance!? +#define NOFORCEENTRY(lev, i,j,k) (LBMGET_FORCE(lev, i,j,k).maxDistance==CPF_MAXDINIT) + // interpolate missing + for(int lev=0; lev<=mMaxRefine; lev++) { + FSGR_FORIJK_BOUNDS(lev) { + if( (RFLAG(lev,i,j,k, mLevel[lev].setCurr)) & (CFFluid|CFInter) ) + //if( (RFLAG(lev,i,j,k, mLevel[lev].setCurr)) & (CFInter) ) + //if(0) + { // only check new inter? RFLAG?check + if(NOFORCEENTRY(lev, i,j,k)) { + //errMsg("CP","FE_MISSING at "<cDirNum; l++) { + int ni=i+this->dfVecX[l], nj=j+this->dfVecY[l], nk=k+this->dfVecZ[l]; + //errMsg("CP","FE_MISSING check "<0.) { + nbs = 1./nbs; + //? LBMGET_FORCE(lev, i,j,k).weightAtt = vals.weightAtt*nbs; + //? LBMGET_FORCE(lev, i,j,k).forceAtt = vals.forceAtt*nbs; + LBMGET_FORCE(lev, i,j,k).maxDistance = vals.maxDistance*nbs; + LBMGET_FORCE(lev, i,j,k).forceMaxd = vals.forceMaxd*nbs; + LBMGET_FORCE(lev, i,j,k).weightVel = vals.weightVel*nbs; + LBMGET_FORCE(lev, i,j,k).forceVel = vals.forceVel*nbs; + } + /*ControlForces *ff = &LBMGET_FORCE(lev, i,j,k); // DEBUG + errMsg("CP","FE_MISSING rec at "<weightAtt<<" wa:" <forceAtt[0],ff->forceAtt[1],ff->forceAtt[2] ) + <<" v:"<weightVel<<" wv:" <forceVel[0],ff->forceVel[1],ff->forceVel[2] ) + <<" v:"<maxDistance<<" wv:" <forceMaxd[0],ff->forceMaxd[1],ff->forceMaxd[2] ) ); // DEBUG */ + // else errMsg("CP","FE_MISSING rec at "< mpControl->mCpUpdateInterval + return; + } else { + // nothing to do ... + return; + } + + // reset + for(int lev=0; lev<=mMaxRefine; lev++) { + FSGR_FORIJK_BOUNDS(lev) { LBMGET_FORCE(lev,i,j,k).resetForces(); } + } + // do setup for coarsest level + const int coarseLev = 0; + const int fineLev = mMaxRefine; + + // init for current time + for(int cpssi=0; cpssi<(int)mpControl->mCons.size(); cpssi++) { + ControlParticles *cparts = mpControl->mCons[cpssi]->mCparts; + LbmControlSet *cset = mpControl->mCons[cpssi]; + + cparts->setRadiusAtt(cset->mcRadiusAtt.get(mSimulationTime)); + cparts->setRadiusVel(cset->mcRadiusVel.get(mSimulationTime)); + cparts->setInfluenceAttraction(cset->mcForceAtt.get(mSimulationTime) ); + cparts->setInfluenceMaxdist(cset->mcForceMaxd.get(mSimulationTime) ); + cparts->setRadiusMinMaxd(cset->mcRadiusMind.get(mSimulationTime)); + cparts->setRadiusMaxd(cset->mcRadiusMaxd.get(mSimulationTime)); + cparts->calculateKernelWeight(); // always necessary!? + cparts->setOffset( vec2L(cset->mcCpOffset.get(mSimulationTime)) ); + cparts->setScale( vec2L(cset->mcCpScale.get(mSimulationTime)) ); + + cparts->setInfluenceVelocity( cset->mcForceVel.get(mSimulationTime), mLevel[fineLev].timestep ); + cparts->setLastOffset( vec2L(cset->mcCpOffset.get(mSimulationTime-mLevel[fineLev].timestep)) ); + cparts->setLastScale( vec2L(cset->mcCpScale.get(mSimulationTime-mLevel[fineLev].timestep)) ); + + } + + // check actual values + LbmFloat iatt = ABS(mpControl->mCons[0]->mCparts->getInfluenceAttraction()); + LbmFloat ivel = mpControl->mCons[0]->mCparts->getInfluenceVelocity(); + LbmFloat imaxd = mpControl->mCons[0]->mCparts->getInfluenceMaxdist(); + //errMsg("FINCIT","iatt="<mCons[1]->mCparts->getInfluenceAttraction(); //ivel = mpControl->mCons[1]->mCparts->getInfluenceVelocity(); //imaxd = mpControl->mCons[1]->mCparts->getInfluenceMaxdist(); // TTTTTT + + // do control setup + for(int cpssi=0; cpssi<(int)mpControl->mCons.size(); cpssi++) { + ControlParticles *cparts = mpControl->mCons[cpssi]->mCparts; + ControlParticles *cpmotion = mpControl->mCons[cpssi]->mCpmotion; + + // TEST!? + bool radmod = false; + const LbmFloat minRadSize = mLevel[coarseLev].nodeSize * 1.5; + if((cparts->getRadiusAtt()>0.) && (cparts->getRadiusAtt()getRadiusAtt(); radmod=true; + debMsgStd("ControlData::initControl",DM_MSG,"Modified radii att, fac="<setRadiusAtt(cparts->getRadiusAtt()*radfac); + cparts->setRadiusVel(cparts->getRadiusVel()*radfac); + cparts->setRadiusMaxd(cparts->getRadiusMaxd()*radfac); + cparts->setRadiusMinMaxd(cparts->getRadiusMinMaxd()*radfac); + } else if((cparts->getRadiusVel()>0.) && (cparts->getRadiusVel()getRadiusVel(); + debMsgStd("ControlData::initControl",DM_MSG,"Modified radii vel, fac="<setRadiusVel(cparts->getRadiusVel()*radfac); + cparts->setRadiusMaxd(cparts->getRadiusMaxd()*radfac); + cparts->setRadiusMinMaxd(cparts->getRadiusMinMaxd()*radfac); + } else if((cparts->getRadiusMaxd()>0.) && (cparts->getRadiusMaxd()getRadiusMaxd(); + debMsgStd("ControlData::initControl",DM_MSG,"Modified radii maxd, fac="<setRadiusMaxd(cparts->getRadiusMaxd()*radfac); + cparts->setRadiusMinMaxd(cparts->getRadiusMinMaxd()*radfac); + } + if(radmod) { + debMsgStd("ControlData::initControl",DM_MSG,"Modified radii: att="<< + cparts->getRadiusAtt()<<", vel=" << cparts->getRadiusVel()<<", maxd=" << + cparts->getRadiusMaxd()<<", mind=" << cparts->getRadiusMinMaxd() ,5); + } + + cpmotion->prepareControl( mSimulationTime+((LbmFloat)mpControl->mCpUpdateInterval)*(mpParam->getTimestep()), mpParam->getTimestep(), NULL ); + cparts->prepareControl( mSimulationTime+((LbmFloat)mpControl->mCpUpdateInterval)*(mpParam->getTimestep()), mpParam->getTimestep(), cpmotion ); + } + + // do control... + for(int lev=0; lev<=mMaxRefine; lev++) { + LbmFloat levVolume = 1.; + LbmFloat levForceScale = 1.; + for(int ll=lev; llgetRadiusAtt()<<" gsx"<getSize(); cppi++) { + ControlParticle *cp = cparts->getParticle(cppi); + if(cp->influence<=0.) continue; + const int cpi = (int)( (cp->pos[0]-goffx)/gsx ); + const int cpj = (int)( (cp->pos[1]-goffy)/gsy ); + int cpk = (int)( (cp->pos[2]-goffz)/gsz ); + /*if( ((LBMDIM==3)&&(BOUNDSKIP(cpk - cpwsm, getForZMinBnd(), getForZMaxBnd(lev) ))) || + ((LBMDIM==3)&&(BOUNDSKIP(cpk + cpwsm, getForZMinBnd(), getForZMaxBnd(lev) ))) || + BOUNDSKIP(cpj - cpwsm, 0, mLevel[lev].lSizey ) || + BOUNDSKIP(cpj + cpwsm, 0, mLevel[lev].lSizey ) || + BOUNDSKIP(cpi - cpwsm, 0, mLevel[lev].lSizex ) || + BOUNDSKIP(cpi + cpwsm, 0, mLevel[lev].lSizex ) ) { + continue; + } // */ + int is,ie,js,je,ks,ke; + ks = BOUNDCHECK(cpk - cpw, getForZMinBnd(), getForZMaxBnd(lev) ); + ke = BOUNDCHECK(cpk + cpw, getForZMinBnd(), getForZMaxBnd(lev) ); + js = BOUNDCHECK(cpj - cpw, 0, mLevel[lev].lSizey ); + je = BOUNDCHECK(cpj + cpw, 0, mLevel[lev].lSizey ); + is = BOUNDCHECK(cpi - cpw, 0, mLevel[lev].lSizex ); + ie = BOUNDCHECK(cpi + cpw, 0, mLevel[lev].lSizex ); + if(LBMDIM==2) { cpk = 0; ks = 0; ke = 1; } + if(CPODEBUG) errMsg("cppft","i"<weightAtt; + // control particle mod, + // dont add multiple CFFluid fsgr boundaries + if(lev==mMaxRefine) { + //if( ( ((*pflag)&(CFFluid )) && (lev==mMaxRefine) ) || + //( ((*pflag)&(CFGrNorm)) && (lev density += levVolume* kk->weightAtt; // old CFFluid + } else if( (*pflag) & (CFEmpty) ) { + cp->density -= levVolume* 0.5; + } else { //if( ((*pflag) & (CFBnd)) ) { + cp->density -= levVolume* 0.2; // penalty + } + } else { + //if((*pflag)&(CFGrNorm)) { + //cp->density += levVolume* kk->weightAtt; // old CFFluid + //} + } + //else if(!((*pflag) & (CFUnused)) ) { cp->density -= levVolume* 0.2; } // penalty + + if( (*pflag) & (CFFluid|CFInter) ) // RFLAG_check + { + + cpChecks++; + //const LbmFloat pwforce = kk->weightAtt; + LbmFloat pwvel = kk->weightVel; + if((pwforce==0.)&&(pwvel==0.)) { continue; } + ff->weightAtt += 1e-6; // for distance + + if(pwforce>0.) { + ff->weightAtt += pwforce *cp->densityWeight *cp->influence; + ff->forceAtt += kk->forceAtt *levForceScale *cp->densityWeight *cp->influence; + + // old fill handling here + const int workSet =mLevel[lev].setCurr; + LbmFloat ux=0., uy=0., uz=0.; + FORDF1{ + const LbmFloat dfn = QCELL(lev, i,j,k, workSet, l); + ux += (this->dfDvecX[l]*dfn); + uy += (this->dfDvecY[l]*dfn); + uz += (this->dfDvecZ[l]*dfn); + } + // control particle mod + cp->avgVelWeight += levVolume*pwforce; + cp->avgVelAcc += LbmVec(ux,uy,uz) * levVolume*pwforce; + } + + if(pwvel>0.) { + // TODO make switch? vel.influence depends on density weight... + // (reduced lowering with 0.75 factor) + pwvel *= cp->influence *(1.-0.75*cp->densityWeight); + // control particle mod + // todo use Omega instead!? + ff->forceVel += cp->vel*levVolume*pwvel * velLatticeScale; // levVolume? + ff->weightVel += levVolume*pwvel; // levVolume? + ff->compAv += cp->avgVel*levVolume*pwvel; // levVolume? + ff->compAvWeight += levVolume*pwvel; // levVolume? + } + + if(CPODEBUG) errMsg("cppft","i"<weightAtt<<" wa:" + //<forceAtt[0],ff->forceAtt[1],ff->forceAtt[2] ) + //<<" v:"<weightVel<<" wv:" + //<forceVel[0],ff->forceVel[1],ff->forceVel[2] ) + //<<" v:"<maxDistance<<" wv:" + //<forceMaxd[0],ff->forceMaxd[1],ff->forceMaxd[2] ) + ); + } // celltype + + } // ijk + } // ijk + } // ijk + } // cpi, end first cp loop (att,vel) + debMsgStd("LbmFsgrSolver::handleCpdata",DM_MSG,"Force cpgrid "<mCons.size(); cpssi++) { + ControlParticles *cparts = mpControl->mCons[cpssi]->mCparts; + + // WARNING copied from above! + const LbmFloat velLatticeScale = mLevel[lev].timestep/mLevel[lev].nodeSize; + LbmFloat gsx = ((mvGeoEnd[0]-mvGeoStart[0])/(LbmFloat)mLevel[lev].lSizex); + LbmFloat gsy = ((mvGeoEnd[1]-mvGeoStart[1])/(LbmFloat)mLevel[lev].lSizey); + LbmFloat gsz = ((mvGeoEnd[2]-mvGeoStart[2])/(LbmFloat)mLevel[lev].lSizez); +#if LBMDIM==2 + gsz = gsx; +#endif + LbmFloat goffx = mvGeoStart[0]; + LbmFloat goffy = mvGeoStart[1]; + LbmFloat goffz = mvGeoStart[2]; + + //const LbmFloat cpwIncFac = 2.0; + const int mdw = MIN( mLevel[lev].lSizex/2, MAX( (int)( cparts->getRadiusMaxd() /gsx) +1 , 2) ); // wide kernel, md + const int mdkarWidth = 2*mdw+1; + mpControl->mMdKernel.resize(mdkarWidth* mdkarWidth* mdkarWidth); + ControlParticle cpt; cpt.reset(); + cpt.density = 0.5; cpt.densityWeight = 0.5; + cpt.pos = LbmVec( (gsx*(LbmFloat)mdw)+goffx, (gsy*(LbmFloat)mdw)+goffy, (gsz*(LbmFloat)mdw)+goffz ); // optimize? +#if LBMDIM==3 + for(int k= 0; kcalculateMaxdForce( &cpt, pos, &MDKERN(i,j,k) ); + } + } + + // second cpi loop, maxd forces + if(cparts->getInfluenceMaxdist()>0.) { + for(int cppi=0; cppigetSize(); cppi++) { + ControlParticle *cp = cparts->getParticle(cppi); + if(cp->influence<=0.) continue; + const int cpi = (int)( (cp->pos[0]-goffx)/gsx ); + const int cpj = (int)( (cp->pos[1]-goffy)/gsy ); + int cpk = (int)( (cp->pos[2]-goffz)/gsz ); + + int is,ie,js,je,ks,ke; + ks = BOUNDCHECK(cpk - mdw, getForZMinBnd(), getForZMaxBnd(lev) ); + ke = BOUNDCHECK(cpk + mdw, getForZMinBnd(), getForZMaxBnd(lev) ); + js = BOUNDCHECK(cpj - mdw, 0, mLevel[lev].lSizey ); + je = BOUNDCHECK(cpj + mdw, 0, mLevel[lev].lSizey ); + is = BOUNDCHECK(cpi - mdw, 0, mLevel[lev].lSizex ); + ie = BOUNDCHECK(cpi + mdw, 0, mLevel[lev].lSizex ); + if(LBMDIM==2) { cpk = 0; ks = 0; ke = 1; } + if(CPODEBUG) errMsg("cppft","i"<weightAtt == 0.) { + ControlForces *kk = &MDKERN( i-cpi+mdw, j-cpj+mdw, k-cpk+mdw); + const LbmFloat pmdf = kk->maxDistance; + if((ff->maxDistance > pmdf) || (ff->maxDistance<0.)) + ff->maxDistance = pmdf; + ff->forceMaxd = kk->forceMaxd; + // todo use Omega instead!? + ff->forceVel = cp->vel* velLatticeScale; + } + } // celltype + } } // ijk + } // cpi, md loop + } // maxd inf>0 */ + + + debMsgStd("ControlData::initControl",DM_MSG,"Maxd cpgrid "<mCons[0]->mCparts->finishControl( mpControl->mCpForces[lev], iatt,ivel,imaxd ); + + } // lev loop + + myTime_t cpend = getTime(); + debMsgStd("ControlData::handleCpdata",DM_MSG,"Time for cpgrid generation:"<< getTimeString(cpend-cpstart)<<", checks:"<mCons.size(); cpssi++) { + ControlParticles *cparts = mpControl->mCons[cpssi]->mCparts; + //ControlParticles *cpmotion = mpControl->mCons[cpssi]->mCpmotion; + // display cp parts + const bool cpCubes = false; + const bool cpDots = true; + const bool cpCpdist = true; + const bool cpHideIna = true; + glShadeModel(GL_FLAT); + glDisable( GL_LIGHTING ); // dont light lines + + // dot influence + if((mpControl->mDebugCpscale>0.) && cpDots) { + glPointSize(mpControl->mDebugCpscale * 8.); + glBegin(GL_POINTS); + for(int i=0; igetSize(); i++) { + if((cpHideIna)&&( (cparts->getParticle(i)->influence<=0.) || (cparts->getParticle(i)->size<=0.) )) continue; + ntlVec3Gfx org( vec2G(cparts->getParticle(i)->pos ) ); + //LbmFloat halfsize = 0.5; + LbmFloat scale = cparts->getParticle(i)->densityWeight; + //glColor4f( scale,scale,scale,scale ); + glColor4f( 0.,scale,0.,scale ); + glVertex3f( org[0],org[1],org[2] ); + //errMsg("lbmDebugDisplay","CP "<mDebugCpscale>0.) && cpDots) { + glPointSize(mpControl->mDebugCpscale * 3.); + glBegin(GL_POINTS); + glColor3f( 0,1,0 ); + } + for(int i=0; igetSize(); i++) { + if((cpHideIna)&&( (cparts->getParticle(i)->influence<=0.) || (cparts->getParticle(i)->size<=0.) )) continue; + ntlVec3Gfx org( vec2G(cparts->getParticle(i)->pos ) ); + LbmFloat halfsize = 0.5; + LbmFloat scale = cparts->getRadiusAtt() * cparts->getParticle(i)->densityWeight; + if(cpCubes){ glLineWidth( 1 ); + glColor3f( 1,1,1 ); + ntlVec3Gfx s = org-(halfsize * (scale)); + ntlVec3Gfx e = org+(halfsize * (scale)); + drawCubeWire( s,e ); } + if((mpControl->mDebugCpscale>0.) && cpDots) { + glVertex3f( org[0],org[1],org[2] ); + } + } + if(cpDots) glEnd(); + + if(mpControl->mDebugAvgVelScale>0.) { + const float scale = mpControl->mDebugAvgVelScale; + + glColor3f( 1.0,1.0,1 ); + glBegin(GL_LINES); + for(int i=0; igetSize(); i++) { + if((cpHideIna)&&( (cparts->getParticle(i)->influence<=0.) || (cparts->getParticle(i)->size<=0.) )) continue; + ntlVec3Gfx org( vec2G(cparts->getParticle(i)->pos ) ); + + //errMsg("CPAVGVEL","i"<getParticle(i)->avgVel);// DEBUG + float dx = cparts->getParticle(i)->avgVel[0]; + float dy = cparts->getParticle(i)->avgVel[1]; + float dz = cparts->getParticle(i)->avgVel[2]; + dx *= scale; dy *= scale; dz *= scale; + glVertex3f( org[0],org[1],org[2] ); + glVertex3f( org[0]+dx,org[1]+dy,org[2]+dz ); + } + glEnd(); + } // */ + + if( (LBMDIM==2) && (cpCpdist) ) { + + // debug, for use of e.g. LBMGET_FORCE LbmControlData *mpControl = this; +# define TESTGET_FORCE(lev,i,j,k) mpControl->mCpForces[lev][ ((k*mLevel[lev].lSizey)+j)*mLevel[lev].lSizex+i ] + + glBegin(GL_LINES); + //const int lev=0; + for(int lev=0; lev<=mMaxRefine; lev++) { + FSGR_FORIJK_BOUNDS(lev) { + LbmVec pos = LbmVec( + ((mvGeoEnd[0]-mvGeoStart[0])/(LbmFloat)mLevel[lev].lSizex) * ((LbmFloat)i+0.5) + mvGeoStart[0], + ((mvGeoEnd[1]-mvGeoStart[1])/(LbmFloat)mLevel[lev].lSizey) * ((LbmFloat)j+0.5) + mvGeoStart[1], + ((mvGeoEnd[2]-mvGeoStart[2])/(LbmFloat)mLevel[lev].lSizez) * ((LbmFloat)k+0.5) + mvGeoStart[2] ); + if(LBMDIM==2) pos[2] = ((mvGeoEnd[2]-mvGeoStart[2])*0.5 + mvGeoStart[2]); + + if((mpControl->mDebugMaxdScale>0.) && (TESTGET_FORCE(lev,i,j,k).weightAtt<=0.) ) + if(TESTGET_FORCE(lev,i,j,k).maxDistance>=0.) + if(TESTGET_FORCE(lev,i,j,k).maxDistancemDebugMaxdScale*10001.; + float dx = TESTGET_FORCE(lev,i,j,k).forceMaxd[0]; + float dy = TESTGET_FORCE(lev,i,j,k).forceMaxd[1]; + float dz = TESTGET_FORCE(lev,i,j,k).forceMaxd[2]; + dx *= scale; dy *= scale; dz *= scale; + glColor3f( 0,1,0 ); + glVertex3f( pos[0],pos[1],pos[2] ); + glVertex3f( pos[0]+dx,pos[1]+dy,pos[2]+dz ); + } // */ + if((mpControl->mDebugAttScale>0.) && (TESTGET_FORCE(lev,i,j,k).weightAtt>0.)) { + const float scale = mpControl->mDebugAttScale*100011.; + float dx = TESTGET_FORCE(lev,i,j,k).forceAtt[0]; + float dy = TESTGET_FORCE(lev,i,j,k).forceAtt[1]; + float dz = TESTGET_FORCE(lev,i,j,k).forceAtt[2]; + dx *= scale; dy *= scale; dz *= scale; + glColor3f( 1,0,0 ); + glVertex3f( pos[0],pos[1],pos[2] ); + glVertex3f( pos[0]+dx,pos[1]+dy,pos[2]+dz ); + } // */ + // why check maxDistance? + if((mpControl->mDebugVelScale>0.) && (TESTGET_FORCE(lev,i,j,k).maxDistance+TESTGET_FORCE(lev,i,j,k).weightVel>0.)) { + float scale = mpControl->mDebugVelScale*1.; + float wvscale = TESTGET_FORCE(lev,i,j,k).weightVel; + float dx = TESTGET_FORCE(lev,i,j,k).forceVel[0]; + float dy = TESTGET_FORCE(lev,i,j,k).forceVel[1]; + float dz = TESTGET_FORCE(lev,i,j,k).forceVel[2]; + scale *= wvscale; + dx *= scale; dy *= scale; dz *= scale; + glColor3f( 0.2,0.2,1 ); + glVertex3f( pos[0],pos[1],pos[2] ); + glVertex3f( pos[0]+dx,pos[1]+dy,pos[2]+dz ); + } // */ + if((mpControl->mDebugCompavScale>0.) && (TESTGET_FORCE(lev,i,j,k).compAvWeight>0.)) { + const float scale = mpControl->mDebugCompavScale*1.; + float dx = TESTGET_FORCE(lev,i,j,k).compAv[0]; + float dy = TESTGET_FORCE(lev,i,j,k).compAv[1]; + float dz = TESTGET_FORCE(lev,i,j,k).compAv[2]; + dx *= scale; dy *= scale; dz *= scale; + glColor3f( 0.2,0.2,1 ); + glVertex3f( pos[0],pos[1],pos[2] ); + glVertex3f( pos[0]+dx,pos[1]+dy,pos[2]+dz ); + } // */ + } // att,maxd + } + glEnd(); + } + } // cpssi + + //fprintf(stderr,"BLA\n"); + glEnable( GL_LIGHTING ); // dont light lines + glShadeModel(GL_SMOOTH); +} + +#else // LBM_USE_GUI==1 +void LbmFsgrSolver::cpDebugDisplay(int dispset) { } +#endif // LBM_USE_GUI==1 + + diff --git a/intern/elbeem/intern/solver_control.h b/intern/elbeem/intern/solver_control.h new file mode 100644 index 00000000000..57112b365ce --- /dev/null +++ b/intern/elbeem/intern/solver_control.h @@ -0,0 +1,182 @@ +/****************************************************************************** + * + * El'Beem - the visual lattice boltzmann freesurface simulator + * All code distributed as part of El'Beem is covered by the version 2 of the + * GNU General Public License. See the file COPYING for details. + * Copyright 2003-2006 Nils Thuerey + * + * testing extensions + * + *****************************************************************************/ + + +#ifndef LBM_TESTCLASS_H +#define LBM_TESTCLASS_H + +//class IsoSurface; +class ParticleObject; +class ControlParticles; +class ControlForces; + +//#define NUMGRIDS 2 +//#define MAXNUMSWS 10 + +// farfield modes +#define FARF_3DONLY -1 +#define FARF_BOTH 0 +#define FARF_SWEONLY 1 +// dont reuse 3d vars/init +#define FARF_SEPSWE 2 + +// relaxation macros for solver_relax.h + +// WARNING has to match controlparts.h +#define CPF_ENTRIES 12 +#define CPF_FORCE 0 +#define CPF_VELWEIGHT 3 +#define CPF_VELOCITY 4 +#define CPF_FORCEWEIGHT 7 +#define CPF_MINCPDIST 8 +#define CPF_MINCPDIR 9 + +#include "controlparticles.h" + +#include "ntl_geometrymodel.h" + +// get force entry, set=0 is unused anyway +#define LBMGET_FORCE(lev, i,j,k) mpControl->mCpForces[lev][ (LBMGI(lev,i,j,k,0)) ] + +// debug mods off... +// same as in src/solver_relax.h! +#define __PRECOLLIDE_MODS(rho,ux,uy,uz, grav) \ + ux += (grav)[0]; \ + uy += (grav)[1]; \ + uz += (grav)[2]; + +//void testMaxdmod(int i, int j,int k, LbmFloat &ux,LbmFloat &uy,LbmFloat &uz,ControlForces &ff); +#if LBMDIM==3 +#define MAXDGRAV \ + if(myforce->forceMaxd[0]*ux+myforce->forceMaxd[1]*uyforceVel[0]+ v2wi*ux; \ + uy = v2w*myforce->forceVel[1]+ v2wi*uy; } \ + /* movement inverse to g? */ \ + if((uz>LBM_EPSILON)&&(uz>myforce->forceVel[2])) { \ + uz = v2w*myforce->forceVel[2]+ v2wi*uz; } +#else // LBMDIM==3 +#define MAXDGRAV \ + if(myforce->forceMaxd[0]*uxforceVel[0]+ v2wi*ux; } \ + /* movement inverse to g? */ \ + if((uy>LBM_EPSILON)&&(uy>myforce->forceVel[1])) { \ + uy = v2w*myforce->forceVel[1]+ v2wi*uy; } +#endif // LBMDIM==3 + +// debug modifications of collide vars (testing) +// requires: lev,i,j,k +#define PRECOLLIDE_MODS(rho,ux,uy,uz, grav) \ + LbmFloat attforce = 1.; \ + if(this->mTForceStrength>0.) { \ + ControlForces* myforce = &LBMGET_FORCE(lev,i,j,k); \ + const LbmFloat vf = myforce->weightAtt;\ + const LbmFloat vw = myforce->weightVel;\ + if(vf!=0.) { attforce = MAX(0., 1.-vf); /* TODO FIXME? use ABS(vf) for repulsion force? */ \ + ux += myforce->forceAtt[0]; \ + uy += myforce->forceAtt[1]; \ + uz += myforce->forceAtt[2]; \ + \ + } else if(( myforce->maxDistance>0.) && ( myforce->maxDistancemCons[0]->mCparts->getInfluenceMaxdist() * \ + (myforce->maxDistance-mpControl->mCons[0]->mCparts->getRadiusMinMaxd()) / (mpControl->mCons[0]->mCparts->getRadiusMaxd()-mpControl->mCons[0]->mCparts->getRadiusMinMaxd()); \ + const LbmFloat v2wi = 1.-v2w; \ + if(v2w>0.){ MAXDGRAV; \ + /* errMsg("ERRMDTT","at "<forceVel<<" " ); */ \ + }\ + } \ + if(vw>0.) { \ + const LbmFloat vwi = 1.-vw;\ + const LbmFloat vwd = mpControl->mDiffVelCon;\ + ux += vw*(myforce->forceVel[0]-myforce->compAv[0] + vwd*(myforce->compAv[0]-ux) ); \ + uy += vw*(myforce->forceVel[1]-myforce->compAv[1] + vwd*(myforce->compAv[1]-uy) ); \ + uz += vw*(myforce->forceVel[2]-myforce->compAv[2] + vwd*(myforce->compAv[2]-uz) ); \ + /* TODO test!? modify smooth vel by influence of force for each lbm step, to account for force update only each N steps */ \ + myforce->compAv = (myforce->forceVel*vw+ myforce->compAv*vwi); \ + } \ + } \ + ux += (grav)[0]*attforce; \ + uy += (grav)[1]*attforce; \ + uz += (grav)[2]*attforce; \ + /* end PRECOLLIDE_MODS */ + +#define TEST_IF_CHECK \ + if((!iffilled)&&(LBMGET_FORCE(lev,i,j,k).weightAtt!=0.)) { \ + errMsg("TESTIFFILL"," at "< mcForceAtt; + AnimChannel mcForceVel; + AnimChannel mcForceMaxd; + + AnimChannel mcRadiusAtt; + AnimChannel mcRadiusVel; + AnimChannel mcRadiusMind; + AnimChannel mcRadiusMaxd; + + AnimChannel mcCpScale; + AnimChannel mcCpOffset; +}; + + + +// main control data storage +class LbmControlData +{ + public: + LbmControlData(); + virtual ~LbmControlData(); + + // control data + + // contorl params + void parseControldataAttrList(AttributeList *attr); + + // control strength, set for solver interface + LbmFloat mSetForceStrength; + // cp vars + std::vector mCons; + // update interval + int mCpUpdateInterval; + // output + string mCpOutfile; + // control particle precomputed influence + std::vector< std::vector > mCpForces; + std::vector mCpKernel; + std::vector mMdKernel; + // activate differential velcon + LbmFloat mDiffVelCon; + + // cp debug displau + LbmFloat mDebugCpscale, mDebugVelScale, mDebugCompavScale, mDebugAttScale, mDebugMaxdScale, mDebugAvgVelScale; +}; + +#endif // LBM_TESTCLASS_H diff --git a/intern/guardedalloc/intern/mmap_win.c b/intern/guardedalloc/intern/mmap_win.c index 436c99344a7..642cc16296e 100644 --- a/intern/guardedalloc/intern/mmap_win.c +++ b/intern/guardedalloc/intern/mmap_win.c @@ -151,7 +151,7 @@ void *mmap(void *start, size_t len, int prot, int flags, int fd, off_t offset) } /* munmap for windows */ -long munmap(void *ptr, long size) +intptr_t munmap(void *ptr, intptr_t size) { MemMap *mm = mmap_findlink(mmapbase, ptr); if (!mm) { diff --git a/intern/guardedalloc/mmap_win.h b/intern/guardedalloc/mmap_win.h index 53af86e1f66..443c3b6f4ce 100644 --- a/intern/guardedalloc/mmap_win.h +++ b/intern/guardedalloc/mmap_win.h @@ -45,7 +45,10 @@ #define MAP_FAILED ((void *)-1) -void *mmap(void *start, size_t len, int prot, int flags, int fd, off_t offset); -long munmap(void *ptr, long size); +#include "BLO_sys_types.h" // needed for intptr_t + +void *mmap(void *start, size_t len, int prot, int flags, int fd, off_t offset); +intptr_t munmap(void *ptr, intptr_t size); + +#endif -#endif \ No newline at end of file diff --git a/release/Makefile b/release/Makefile index 953144b5223..f440e2dface 100644 --- a/release/Makefile +++ b/release/Makefile @@ -118,7 +118,6 @@ install: package ifneq ($(OS), darwin) @[ ! -d $(OCGDIR)/bin/.blender ] || \ cp -r $(OCGDIR)/bin/.blender $(DISTDIR) - @rm -rf $(DISTDIR)/.svn $(DISTDIR)/*/.svn $(DISTDIR)/*/*/.svn @cp $(NANBLENDERHOME)/bin/.blender/.Blanguages $(CONFDIR) @cp $(NANBLENDERHOME)/bin/.blender/.bfont.ttf $(CONFDIR) endif @@ -144,8 +143,7 @@ ifneq ($(NOPLUGINS),true) @cp ../source/blender/blenpluginapi/*.h $(DISTDIR)/plugins/include/ @chmod 755 $(DISTDIR)/plugins/bmake @$(MAKE) -C $(DISTDIR)/plugins all > /dev/null || exit 1; - @rm -fr $(DISTDIR)/plugins/.svn $(DISTDIR)/plugins/*/.svn \ - $(DISTDIR)/plugins/*/*.o + @rm -f $(DISTDIR)/plugins/*/*.o #on OS X the plugins move to the installation directory ifneq ($(OS),darwin) @@ -158,7 +156,6 @@ endif @echo "----> Copy python infrastructure" @[ ! -d scripts ] || cp -r scripts $(CONFDIR)/scripts - @[ ! -d $(CONFDIR)/scripts ] || rm -fr $(CONFDIR)/scripts/.svn $(CONFDIR)/scripts/*/.svn $(CONFDIR)/scripts/*/*/.svn ifeq ($(OS),darwin) @echo "----> Move .blender to .app/Contents/MacOS/" @@ -180,6 +177,8 @@ endif @[ ! -x $(CONFIG_GUESS)/specific.sh ] || (\ echo "**--> Execute specific.sh in $(CONFIG_GUESS)/" && \ cd $(CONFIG_GUESS) && ./specific.sh ) + @echo "----> Cleaning .svn metadata directories" + @find $(DISTDIR) -type d -name ".svn" | xargs rm -fr pkg: install @echo "----> Create distribution file $(BLENDNAME)$(EXT1)" diff --git a/release/scripts/bpymodules/BPyAddMesh.py b/release/scripts/bpymodules/BPyAddMesh.py index bd3ee845d21..901e68866cc 100644 --- a/release/scripts/bpymodules/BPyAddMesh.py +++ b/release/scripts/bpymodules/BPyAddMesh.py @@ -68,11 +68,7 @@ def add_mesh_simple(name, verts, edges, faces): else: # Mesh with no data, unlikely me.edges.extend(edges) - me.faces.extend(faces) - - if is_editmode or Blender.Get('add_editmode'): - EditMode(1) - + me.faces.extend(faces) else: # Object mode add new @@ -95,9 +91,14 @@ def add_mesh_simple(name, verts, edges, faces): ob_act.setMatrix(mat) ob_act.loc = cursor - - if is_editmode or Blender.Get('add_editmode'): - EditMode(1) + + me.calcNormals() + + if is_editmode or Blender.Get('add_editmode'): + EditMode(1) + + + def write_mesh_script(filepath, me): @@ -112,7 +113,7 @@ def write_mesh_script(filepath, me): file.write('#!BPY\n') file.write('"""\n') file.write('Name: \'%s\'\n' % name) - file.write('Blender: 243\n') + file.write('Blender: 245\n') file.write('Group: \'AddMesh\'\n') file.write('"""\n\n') file.write('import BPyAddMesh\n') diff --git a/release/scripts/ms3d_import_ascii.py b/release/scripts/ms3d_import_ascii.py new file mode 100644 index 00000000000..d8c22a1ec99 --- /dev/null +++ b/release/scripts/ms3d_import_ascii.py @@ -0,0 +1,479 @@ +#!BPY +""" +Name: 'MilkShape3D ASCII (.txt)...' +Blender: 245 +Group: 'Import' +Tooltip: 'Import from a MilkShape3D ASCII file format (.txt)' +""" +# +# Author: Markus Ilmola +# Email: markus.ilmola@pp.inet.fi +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# + +# import needed stuff +import os.path +import re +import math +from math import * +import Blender +from Blender import Mathutils +from Blender.Mathutils import * + + + +# Converts ms3d euler angles to a rotation matrix +def RM(a): + sy = sin(a[2]) + cy = cos(a[2]) + sp = sin(a[1]) + cp = cos(a[1]) + sr = sin(a[0]) + cr = cos(a[0]) + return Matrix([cp*cy, cp*sy, -sp], [sr*sp*cy+cr*-sy, sr*sp*sy+cr*cy, sr*cp],[cr*sp*cy+-sr*-sy, cr*sp*sy+-sr*cy, cr*cp]) + + +# Converts ms3d euler angles to a quaternion +def RQ(a): + angle = a[2] * 0.5; + sy = sin(angle); + cy = cos(angle); + angle = a[1] * 0.5; + sp = sin(angle); + cp = cos(angle); + angle = a[0] * 0.5; + sr = sin(angle); + cr = cos(angle); + return Quaternion(cr*cp*cy+sr*sp*sy, sr*cp*cy-cr*sp*sy, cr*sp*cy+sr*cp*sy, cr*cp*sy-sr*sp*cy) + + +# takes a texture filename and tries to load it +def loadImage(path, filename): + image = None + try: + image = Blender.Image.Load(os.path.abspath(filename)) + except IOError: + print "Warning: Failed to load image: " + filename + ". Trying short path instead...\n" + try: + image = Blender.Image.Load(os.path.dirname(path) + "/" + os.path.basename(filename)) + except IOError: + print "Warning: Failed to load image: " + os.path.basename(filename) + "!\n" + return image + + + +# returns the next non-empty, non-comment line from the file +def getNextLine(file): + ready = False + while ready==False: + line = file.readline() + if len(line)==0: + print "Warning: End of file reached." + return line + ready = True + line = line.strip() + if len(line)==0 or line.isspace(): + ready = False + if len(line)>=2 and line[0]=='/' and line[1]=='/': + ready = False + return line + + + +# imports a MilkShape3D ascii file to the current scene +def import_ms3d_ascii(path): + # limits + MAX_NUMMESHES = 1000 + MAX_NUMVERTS = 100000 + MAX_NUMNORMALS = 100000 + MAX_NUMTRIS = 100000 + MAX_NUMMATS = 16 + MAX_NUMBONES = 100 + MAX_NUMPOSKEYS = 1000 + MAX_NUMROTKEYS = 1000 + + # get scene + scn = Blender.Scene.GetCurrent() + if scn==None: + return "No scene to import to!" + + # open the file + try: + file = open(path, 'r') + except IOError: + return "Failed to open the file!" + + # Read frame info + try: + lines = getNextLine(file).split() + if len(lines) != 2 or lines[0] != "Frames:": + raise ValueError + lines = getNextLine(file).split() + if len(lines) != 2 or lines[0] != "Frame:": + raise ValueError + except ValueError: + return "Frame information is invalid!" + + # Create the mesh + meshOb = Blender.Object.New('Mesh', "MilkShape3D Object") + mesh = Blender.Mesh.New("MilkShape3D Mesh") + meshOb.link(mesh) + scn.objects.link(meshOb) + + # read the number of meshes + try: + lines = getNextLine(file).split() + if len(lines)!=2 or lines[0]!="Meshes:": + raise ValueError + numMeshes = int(lines[1]) + if numMeshes < 0 or numMeshes > MAX_NUMMESHES: + raise ValueError + except ValueError: + return "Number of meshes is invalid!" + + # read meshes + vertBase = 0 + faceBase = 0 + boneIds = [] + for i in range(numMeshes): + # read name, flags and material + try: + lines = re.findall(r'\".*\"|[^ ]+', getNextLine(file)) + if len(lines)!=3: + raise ValueError + material = int(lines[2]) + except ValueError: + return "Name, flags or material in mesh " + str(i+1) + " are invalid!" + + # read the number of vertices + try: + numVerts = int(getNextLine(file)) + if numVerts < 0 or numVerts > MAX_NUMVERTS: + raise ValueError + except ValueError: + return "Number of vertices in mesh " + str(i+1) + " is invalid!" + + # read vertices + coords = [] + uvs = [] + for j in xrange(numVerts): + try: + lines = getNextLine(file).split() + if len(lines)!=7: + raise ValueError + coords.append([float(lines[1]), float(lines[2]), float(lines[3])]) + uvs.append([float(lines[4]), 1-float(lines[5])]) + boneIds.append(int(lines[6])) + except ValueError: + return "Vertex " + str(j+1) + " in mesh " + str(i+1) + " is invalid!" + mesh.verts.extend(coords) + + # read number of normals + try: + numNormals = int(getNextLine(file)) + if numNormals < 0 or numNormals > MAX_NUMNORMALS: + raise ValueError + except ValueError: + return "Number of normals in mesh " + str(i+1) + " is invalid!" + + # read normals + normals = [] + for j in xrange(numNormals): + try: + lines = getNextLine(file).split() + if len(lines)!=3: + raise ValueError + normals.append([float(lines[0]), float(lines[1]), float(lines[2])]) + except ValueError: + return "Normal " + str(j+1) + " in mesh " + str(i+1) + " is invalid!" + + # read the number of triangles + try: + numTris = int(getNextLine(file)) + if numTris < 0 or numTris > MAX_NUMTRIS: + raise ValueError + except ValueError: + return "Number of triangles in mesh " + str(i+1) + " is invalid!" + + # read triangles + faces = [] + for j in xrange(numTris): + # read the triangle + try: + lines = getNextLine(file).split() + if len(lines)!=8: + raise ValueError + v1 = int(lines[1]) + v2 = int(lines[2]) + v3 = int(lines[3]) + faces.append([v1+vertBase, v2+vertBase, v3+vertBase]) + except ValueError: + return "Triangle " + str(j+1) + " in mesh " + str(i+1) + " is invalid!" + mesh.faces.extend(faces) + + # set texture coordinates and material + for j in xrange(faceBase, len(mesh.faces)): + face = mesh.faces[j] + face.uv = [Vector(uvs[face.verts[0].index-vertBase]), Vector(uvs[face.verts[1].index-vertBase]), Vector(uvs[face.verts[2].index-vertBase])] + if material>=0: + face.mat = material + + # increase vertex and face base + vertBase = len(mesh.verts) + faceBase = len(mesh.faces) + + # read the number of materials + try: + lines = getNextLine(file).split() + if len(lines)!=2 or lines[0]!="Materials:": + raise ValueError + numMats = int(lines[1]) + if numMats < 0 or numMats > MAX_NUMMATS: + raise ValueError + except ValueError: + return "Number of materials is invalid!" + + # read the materials + for i in range(numMats): + # read name + name = getNextLine(file)[1:-1] + + # create the material + mat = Blender.Material.New(name) + mesh.materials += [mat] + + # read ambient color + try: + lines = getNextLine(file).split() + if len(lines)!=4: + raise ValueError + amb = (float(lines[0])+float(lines[1])+float(lines[2]))/3 + mat.setAmb(amb) + except ValueError: + return "Ambient color in material " + str(i+1) + " is invalid!" + + # read diffuse color + try: + lines = getNextLine(file).split() + if len(lines)!=4: + raise ValueError + mat.setRGBCol([float(lines[0]), float(lines[1]), float(lines[2])]) + except ValueError: + return "Diffuse color in material " + str(i+1) + " is invalid!" + + # read specular color + try: + lines = getNextLine(file).split() + if len(lines)!=4: + raise ValueError + mat.setSpecCol([float(lines[0]), float(lines[1]), float(lines[2])]) + except ValueError: + return "Specular color in material " + str(i+1) + " is invalid!" + + # read emissive color + try: + lines = getNextLine(file).split() + if len(lines)!=4: + raise ValueError + emit = (float(lines[0])+float(lines[1])+float(lines[2]))/3 + mat.setEmit(emit) + except ValueError: + return "Emissive color in material " + str(i+1) + " is invalid!" + + # read shininess + try: + shi = float(getNextLine(file)) + #mat.setHardness(int(shi)) + except ValueError: + return "Shininess in material " + str(i+1) + " is invalid!" + + # read transparency + try: + alpha = float(getNextLine(file)) + mat.setAlpha(alpha) + if alpha < 1: + mat.mode |= Blender.Material.Modes.ZTRANSP + except ValueError: + return "Transparency in material " + str(i+1) + " is invalid!" + + # read texturemap + texturemap = getNextLine(file)[1:-1] + if len(texturemap)>0: + colorTexture = Blender.Texture.New(name + "_texture") + colorTexture.setType('Image') + colorTexture.setImage(loadImage(path, texturemap)) + mat.setTexture(0, colorTexture, Blender.Texture.TexCo.UV, Blender.Texture.MapTo.COL) + + # read alphamap + alphamap = getNextLine(file)[1:-1] + if len(alphamap)>0: + alphaTexture = Blender.Texture.New(name + "_alpha") + alphaTexture.setType('Image') + alphaTexture.setImage(loadImage(path, alphamap)) + mat.setTexture(1, alphaTexture, Blender.Texture.TexCo.UV, Blender.Texture.MapTo.ALPHA) + + # read the number of bones + try: + lines = getNextLine(file).split() + if len(lines)!=2 or lines[0]!="Bones:": + raise ValueError + numBones = int(lines[1]) + if numBones < 0 or numBones > MAX_NUMBONES: + raise ValueError + except: + return "Number of bones is invalid!" + + # create the armature + armature = None + armOb = None + if numBones > 0: + armOb = Blender.Object.New('Armature', "MilkShape3D Skeleton") + armature = Blender.Armature.New("MilkShape3D Skeleton") + armature.drawType = Blender.Armature.STICK + armOb.link(armature) + scn.objects.link(armOb) + armOb.makeParentDeform([meshOb]) + armature.makeEditable() + + # read bones + posKeys = {} + rotKeys = {} + for i in range(numBones): + # read name + name = getNextLine(file)[1:-1] + + # create the bone + bone = Blender.Armature.Editbone() + armature.bones[name] = bone + + # read parent + parent = getNextLine(file)[1:-1] + if len(parent)>0: + bone.parent = armature.bones[parent] + + # read position and rotation + try: + lines = getNextLine(file).split() + if len(lines) != 7: + raise ValueError + pos = [float(lines[1]), float(lines[2]), float(lines[3])] + rot = [float(lines[4]), float(lines[5]), float(lines[6])] + except ValueError: + return "Invalid position or orientation in a bone!" + + # set position and orientation + if bone.hasParent(): + bone.head = Vector(pos) * bone.parent.matrix + bone.parent.head + bone.tail = bone.head + Vector([1,0,0]) + tempM = RM(rot) * bone.parent.matrix + tempM.transpose; + bone.matrix = tempM + else: + bone.head = Vector(pos) + bone.tail = bone.head + Vector([1,0,0]) + bone.matrix = RM(rot) + + # Create vertex group for this bone + mesh.addVertGroup(name) + vgroup = [] + for index, v in enumerate(boneIds): + if v==i: + vgroup.append(index) + mesh.assignVertsToGroup(name, vgroup, 1.0, 1) + + # read the number of position key frames + try: + numPosKeys = int(getNextLine(file)) + if numPosKeys < 0 or numPosKeys > MAX_NUMPOSKEYS: + raise ValueError + except ValueError: + return "Invalid number of position key frames!" + + # read position key frames + posKeys[name] = [] + for j in range(numPosKeys): + # read time and position + try: + lines = getNextLine(file).split() + if len(lines) != 4: + raise ValueError + time = float(lines[0]) + pos = [float(lines[1]), float(lines[2]), float(lines[3])] + posKeys[name].append([time, pos]) + except ValueError: + return "Invalid position key frame!" + + # read the number of rotation key frames + try: + numRotKeys = int(getNextLine(file)) + if numRotKeys < 0 or numRotKeys > MAX_NUMROTKEYS: + raise ValueError + except ValueError: + return "Invalid number of rotation key frames!" + + # read rotation key frames + rotKeys[name] = [] + for j in range(numRotKeys): + # read time and rotation + try: + lines = getNextLine(file).split() + if len(lines) != 4: + raise ValueError + time = float(lines[0]) + rot = [float(lines[1]), float(lines[2]), float(lines[3])] + rotKeys[name].append([time, rot]) + except ValueError: + return "Invalid rotation key frame!" + + # create action and pose + action = None + pose = None + if armature != None: + armature.update() + pose = armOb.getPose() + action = armOb.getAction() + if not action: + action = Blender.Armature.NLA.NewAction() + action.setActive(armOb) + + # create animation key frames + for name, pbone in pose.bones.items(): + # create position keys + for key in posKeys[name]: + pbone.loc = Vector(key[1]) + pbone.insertKey(armOb, int(key[0]+0.5), Blender.Object.Pose.LOC, True) + + # create rotation keys + for key in rotKeys[name]: + pbone.quat = RQ(key[1]) + pbone.insertKey(armOb, int(key[0]+0.5), Blender.Object.Pose.ROT, True) + + # set the imported object to be the selected one + scn.objects.selected = [] + meshOb.sel= 1 + Blender.Redraw() + + # The import was a succes! + return "" + + +# load the model +def fileCallback(filename): + error = import_ms3d_ascii(filename) + if error!="": + Blender.Draw.PupMenu("An error occured during import: " + error + "|Not all data might have been imported succesfully.", 2) + +Blender.Window.FileSelector(fileCallback, 'Import') diff --git a/release/scripts/scripttemplate_gamelogic.py b/release/scripts/scripttemplate_gamelogic.py new file mode 100644 index 00000000000..17095251155 --- /dev/null +++ b/release/scripts/scripttemplate_gamelogic.py @@ -0,0 +1,93 @@ +#!BPY +""" +Name: 'GameLogic Example' +Blender: 245 +Group: 'ScriptTemplate' +Tooltip: 'Script template with examples of how to use game logic' +""" + +from Blender import Window +import bpy + +script_data = \ +''' +# GameLogic has been added to the global namespace no need to import + +# for keyboard event comparison +# import GameKeys + +# support for Vector(), Matrix() types and advanced functions like AngleBetweenVecs(v1,v2) and RotationMatrix(...) +# import Mathutils + +# for functions like getWindowWidth(), getWindowHeight() +# import Rasterizer + +def main(): + cont = GameLogic.getCurrentController() + + # The KX_GameObject that owns this controller. + own = cont.getOwner() + + # for scripts that deal with spacial logic + own_pos = own.getPosition() + + + # Some example functions, remove to write your own script. + # check for a positive sensor, will run on any object without errors. + print 'Logic info for KX_GameObject', own.getName() + input = False + + for sens in cont.getSensors(): + # The sensor can be on another object, we may want to use it + own_sens = sens.getOwner() + print ' sensor:', sens.getName(), + if sens.isPositive(): + print '(true)' + input = True + else: + print '(false)' + + for actu in cont.getActuators(): + # The actuator can be on another object, we may want to use it + own_actu = actu.getOwner() + print ' actuator:', sens.getName() + + # This runs the actuator or turns it off + # note that actuators will continue to run unless explicitly turned off. + if input: + GameLogic.addActiveActuator(actu, True) + else: + GameLogic.addActiveActuator(actu, False) + + # Its also good practice to get sensors and actuators by names + # so any changes to their order wont break the script. + + # sens_key = cont.getSensor('key_sensor') + # actu_motion = cont.getActuator('motion') + + + # Loop through all other objects in the scene + sce = GameLogic.getCurrentScene() + print 'Scene Objects:', sce.getName() + for ob in sce.getObjectList(): + print ' ', ob.getName(), ob.getPosition() + + + # Example where collision objects are checked for their properties + # adding to our objects "life" property + """ + actu_collide = cont.getSensor('collision_sens') + for ob in actu_collide.getHitObjectList(): + # Check to see the object has this property + if hasattr(ob, 'life'): + own.life += ob.life + ob.life = 0 + print own.life + """ +main() +''' + +new_text = bpy.data.texts.new('gamelogic_example.py') +new_text.write(script_data) +bpy.data.texts.active = new_text +Window.RedrawAll() diff --git a/release/scripts/scripttemplate_gamelogic_basic.py b/release/scripts/scripttemplate_gamelogic_basic.py new file mode 100644 index 00000000000..dfd52a9f749 --- /dev/null +++ b/release/scripts/scripttemplate_gamelogic_basic.py @@ -0,0 +1,33 @@ +#!BPY +""" +Name: 'GameLogic Template' +Blender: 245 +Group: 'ScriptTemplate' +Tooltip: 'Basic template for new game logic scripts' +""" + +from Blender import Window +import bpy + +script_data = \ +''' +def main(): + + cont = GameLogic.getCurrentController() + own = cont.getOwner() + + sens = cont.getSensor('mySensor') + actu = cont.getActuator('myActuator') + + if sens.isPositive(): + GameLogic.addActiveActuator(actu, True) + else: + GameLogic.addActiveActuator(actu, False) + +main() +''' + +new_text = bpy.data.texts.new('gamelogic_example.py') +new_text.write(script_data) +bpy.data.texts.active = new_text +Window.RedrawAll() diff --git a/release/scripts/scripttemplate_ipo_gen.py b/release/scripts/scripttemplate_ipo_gen.py new file mode 100644 index 00000000000..a333b4f20bf --- /dev/null +++ b/release/scripts/scripttemplate_ipo_gen.py @@ -0,0 +1,92 @@ +#!BPY +""" +Name: 'IPO Example' +Blender: 245 +Group: 'ScriptTemplate' +Tooltip: 'Script template for setting the IPO' +""" + +from Blender import Window +import bpy + +script_data = \ +'''#!BPY +""" +Name: 'My Ipo Script' +Blender: 245 +Group: 'Animation' +Tooltip: 'Put some useful info here' +""" + +# Add a licence here if you wish to re-distribute, we recommend the GPL + +from Blender import Ipo, Mathutils, Window +import bpy, BPyMessages + +def makeRandomIpo(object, firstFrame, numberOfFrames, frameStep): + # Create an new Ipo Curve of name myIpo and type Object + myIpo = bpy.data.ipos.new('myIpo', 'Object') + + # Create LocX, LocY, and LocZ Ipo curves in our new Curve Object + # and store them so we can access them later + myIpo_x = myIpo.addCurve('LocX') + myIpo_y = myIpo.addCurve('LocY') + myIpo_z = myIpo.addCurve('LocZ') + + # What value we want to scale our random value by + ipoScale = 4 + + # This Calculates the End Frame for use in an xrange() expression + endFrame = firstFrame + (numberOfFrames * frameStep) + frameStep + + for frame in xrange(firstFrame, endFrame, frameStep): + + # Use the Mathutils Rand() function to get random numbers + ipoValue_x = Mathutils.Rand(-1, 1) * ipoScale + ipoValue_y = Mathutils.Rand(-1, 1) * ipoScale + ipoValue_z = Mathutils.Rand(-1, 1) * ipoScale + + # Append to the Ipo curve at location frame, with the value ipoValue_x + # Note that we should pass the append function a tuple or a BezTriple + myIpo_x.append((frame, ipoValue_x)) + + # Similar to above + myIpo_y.append((frame, ipoValue_y)) + myIpo_z.append((frame, ipoValue_z)) + + # Link our new Ipo Curve to the passed object + object.setIpo(myIpo) + print object + + +def main(): + + # Get the active scene, since there can be multiple ones + sce = bpy.data.scenes.active + + # Get the active object + object = sce.objects.active + + # If there is no active object, pop up an error message + if not object: + BPyMessages.Error_NoActive() + + Window.WaitCursor(1) + + # Call our makeRandomIpo function + # Pass it our object, Tell it to keys from the start frame until the end frame, at a step of 10 frames + # between them + + makeRandomIpo(object, sce.render.sFrame, sce.render.eFrame, 10) + + Window.WaitCursor(0) + +if __name__ == '__main__': + main() + +''' + +new_text = bpy.data.texts.new('ipo_template.py') +new_text.write(script_data) +bpy.data.texts.active = new_text +Window.RedrawAll() diff --git a/source/blender/blenkernel/BKE_bullet.h b/source/blender/blenkernel/BKE_bullet.h new file mode 100644 index 00000000000..a5e04f602ce --- /dev/null +++ b/source/blender/blenkernel/BKE_bullet.h @@ -0,0 +1,43 @@ +/** + * + * $Id: BKE_bullet.h 16773 2008-09-27 22:01:26Z ben2610 $ + * + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) Blender Foundation. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL LICENSE BLOCK ***** + */ +#ifndef BKE_BULLET_H +#define BKE_BULLET_H + +struct BulletSoftBody; + + +/* allocates and initializes general main data */ +extern struct BulletSoftBody *bsbNew(void); + +/* frees internal data and softbody itself */ +extern void bsbFree(struct BulletSoftBody *sb); + +#endif + diff --git a/source/blender/blenkernel/BKE_fluidsim.h b/source/blender/blenkernel/BKE_fluidsim.h new file mode 100644 index 00000000000..33c706da82b --- /dev/null +++ b/source/blender/blenkernel/BKE_fluidsim.h @@ -0,0 +1,55 @@ +/** + * BKE_fluidsim.h + * + * + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) Blender Foundation + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL LICENSE BLOCK ***** + */ + +#include "DNA_modifier_types.h" +#include "DNA_object_fluidsim.h" // N_T +#include "DNA_object_types.h" + +#include "BKE_DerivedMesh.h" + +/* old interface */ +FluidsimSettings *fluidsimSettingsNew(Object *srcob); + +void initElbeemMesh(Object *ob, int *numVertices, float **vertices, int *numTriangles, int **triangles, int useGlobalCoords, int modifierIndex); + + +/* new fluid-modifier interface */ +void fluidsim_init(FluidsimModifierData *fluidmd); +void fluidsim_free(FluidsimModifierData *fluidmd); + +DerivedMesh *fluidsim_read_cache(Object *ob, DerivedMesh *orgdm, FluidsimModifierData *fluidmd, int framenr, int useRenderParams); +DerivedMesh *fluidsimModifier_do(FluidsimModifierData *fluidmd, Object *ob, DerivedMesh *dm, int useRenderParams, int isFinalCalc); + +// get bounding box of mesh +void fluid_get_bb(MVert *mvert, int totvert, float obmat[][4], + /*RET*/ float start[3], /*RET*/ float size[3] ); + + + diff --git a/source/blender/blenkernel/BKE_simple_deform.h b/source/blender/blenkernel/BKE_simple_deform.h new file mode 100644 index 00000000000..161871a64bc --- /dev/null +++ b/source/blender/blenkernel/BKE_simple_deform.h @@ -0,0 +1,39 @@ +/** + * BKE_shrinkwrap.h + * + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) Blender Foundation. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL LICENSE BLOCK ***** + */ +#ifndef BKE_SIMPLE_DEFORM_H +#define BKE_SIMPLE_DEFORM_H + +struct Object; +struct DerivedMesh; +struct SimpleDeformModifierData; + +void SimpleDeformModifier_do(SimpleDeformModifierData *smd, struct Object *ob, struct DerivedMesh *dm, float (*vertexCos)[3], int numVerts); + +#endif + diff --git a/source/blender/blenkernel/intern/bullet.c b/source/blender/blenkernel/intern/bullet.c new file mode 100644 index 00000000000..f0a70b4767b --- /dev/null +++ b/source/blender/blenkernel/intern/bullet.c @@ -0,0 +1,95 @@ +/* + * + * $Id: bullet.c 16776 2008-09-28 03:07:13Z erwin $ + * + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) Blender Foundation + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL LICENSE BLOCK ***** + */ + +#include "MEM_guardedalloc.h" + +/* types */ +#include "DNA_object_force.h" /* here is the softbody struct */ + +#include "BKE_bullet.h" + + +/* ************ Object level, exported functions *************** */ + +/* allocates and initializes general main data */ +BulletSoftBody *bsbNew(void) +{ + BulletSoftBody *bsb; + + bsb= MEM_callocN(sizeof(BulletSoftBody), "bulletsoftbody"); + + bsb->flag = OB_BSB_BENDING_CONSTRAINTS | OB_BSB_SHAPE_MATCHING | OB_BSB_AERO_VPOINT; + bsb->linStiff = 0.5f; + bsb->angStiff = 1.0f; + bsb->volume = 1.0f; + + + bsb->viterations = 0; + bsb->piterations = 2; + bsb->diterations = 0; + bsb->citerations = 4; + + bsb->kSRHR_CL = 0.1f; + bsb->kSKHR_CL = 1.f; + bsb->kSSHR_CL = 0.5f; + bsb->kSR_SPLT_CL = 0.5f; + + bsb->kSK_SPLT_CL = 0.5f; + bsb->kSS_SPLT_CL = 0.5f; + bsb->kVCF = 1; + bsb->kDP = 0; + + bsb->kDG = 0; + bsb->kLF = 0; + bsb->kPR = 0; + bsb->kVC = 0; + + bsb->kDF = 0.2f; + bsb->kMT = 0.05; + bsb->kCHR = 1.0f; + bsb->kKHR = 0.1f; + + bsb->kSHR = 1.0f; + bsb->kAHR = 0.7f; + bsb->collisionflags = 0; + //bsb->collisionflags = OB_BSB_COL_CL_RS + OB_BSB_COL_CL_SS; + bsb->numclusteriterations = 64; + + return bsb; +} + +/* frees all */ +void bsbFree(BulletSoftBody *bsb) +{ + /* no internal data yet */ + MEM_freeN(bsb); +} + + diff --git a/source/blender/blenkernel/intern/fluidsim.c b/source/blender/blenkernel/intern/fluidsim.c new file mode 100644 index 00000000000..9c1b3ee5a8d --- /dev/null +++ b/source/blender/blenkernel/intern/fluidsim.c @@ -0,0 +1,642 @@ +/** + * fluidsim.c + * + * + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) Blender Foundation + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL LICENSE BLOCK ***** + */ + +#include "MEM_guardedalloc.h" + +#include "DNA_mesh_types.h" +#include "DNA_meshdata_types.h" +#include "DNA_object_force.h" // for pointcache +#include "DNA_particle_types.h" +#include "DNA_scene_types.h" // N_T + +#include "BLI_arithb.h" +#include "BLI_blenlib.h" + +#include "BKE_cdderivedmesh.h" +#include "BKE_customdata.h" +#include "BKE_DerivedMesh.h" +#include "BKE_fluidsim.h" +#include "BKE_global.h" +#include "BKE_modifier.h" +#include "BKE_mesh.h" +#include "BKE_pointcache.h" +#include "BKE_utildefines.h" + +// headers for fluidsim bobj meshes +#include +#include "LBM_fluidsim.h" +#include "elbeem.h" +#include +#include +#include + +/* ************************* fluidsim bobj file handling **************************** */ + +// ----------------------------------------- +// forward decleration +// ----------------------------------------- + +// ----------------------------------------- + +void fluidsim_init(FluidsimModifierData *fluidmd) +{ +#ifndef DISABLE_ELBEEM + if(fluidmd) + { + FluidsimSettings *fss = MEM_callocN(sizeof(FluidsimSettings), "fluidsimsettings"); + + fluidmd->fss = fss; + + if(!fss) + return; + + fss->type = 0; + fss->show_advancedoptions = 0; + + fss->resolutionxyz = 50; + fss->previewresxyz = 25; + fss->realsize = 0.03; + fss->guiDisplayMode = 2; // preview + fss->renderDisplayMode = 3; // render + + fss->viscosityMode = 2; // default to water + fss->viscosityValue = 1.0; + fss->viscosityExponent = 6; + + // dg TODO: change this to [] + fss->gravx = 0.0; + fss->gravy = 0.0; + fss->gravz = -9.81; + fss->animStart = 0.0; + fss->animEnd = 0.30; + fss->gstar = 0.005; // used as normgstar + fss->maxRefine = -1; + // maxRefine is set according to resolutionxyz during bake + + // fluid/inflow settings + // fss->iniVel --> automatically set to 0 + + /* elubie: changed this to default to the same dir as the render output + to prevent saving to C:\ on Windows */ + BLI_strncpy(fss->surfdataPath, btempdir, FILE_MAX); + + // first init of bounding box + // no bounding box needed + + // todo - reuse default init from elbeem! + fss->typeFlags = 0; + fss->domainNovecgen = 0; + fss->volumeInitType = 1; // volume + fss->partSlipValue = 0.0; + + fss->generateTracers = 0; + fss->generateParticles = 0.0; + fss->surfaceSmoothing = 1.0; + fss->surfaceSubdivs = 1.0; + fss->particleInfSize = 0.0; + fss->particleInfAlpha = 0.0; + + // init fluid control settings + fss->attractforceStrength = 0.2; + fss->attractforceRadius = 0.75; + fss->velocityforceStrength = 0.2; + fss->velocityforceRadius = 0.75; + fss->cpsTimeStart = fss->animStart; + fss->cpsTimeEnd = fss->animEnd; + fss->cpsQuality = 10.0; // 1.0 / 10.0 => means 0.1 width + + /* + BAD TODO: this is done in buttons_object.c in the moment + Mesh *mesh = ob->data; + // calculate bounding box + fluid_get_bb(mesh->mvert, mesh->totvert, ob->obmat, fss->bbStart, fss->bbSize); + */ + + fss->lastgoodframe = -1; + + fss->flag = 0; + + } +#endif + return; +} + +void fluidsim_free(FluidsimModifierData *fluidmd) +{ +#ifndef DISABLE_ELBEEM + if(fluidmd) + { + MEM_freeN(fluidmd->fss); + } +#endif + return; +} + +DerivedMesh *fluidsimModifier_do(FluidsimModifierData *fluidmd, Object *ob, DerivedMesh *dm, int useRenderParams, int isFinalCalc) +{ +#ifndef DISABLE_ELBEEM + DerivedMesh *result = NULL; + int framenr; + FluidsimSettings *fss = NULL; + + framenr= (int)G.scene->r.cfra; + + // only handle fluidsim domains + if(fluidmd && fluidmd->fss && (fluidmd->fss->type != OB_FLUIDSIM_DOMAIN)) + return dm; + + // sanity check + if(!fluidmd || (fluidmd && !fluidmd->fss)) + return dm; + + fss = fluidmd->fss; + + // timescale not supported yet + // clmd->sim_parms->timescale= timescale; + + // support reversing of baked fluid frames here + if((fss->flag & OB_FLUIDSIM_REVERSE) && (fss->lastgoodframe >= 0)) + { + framenr = fss->lastgoodframe - framenr + 1; + CLAMP(framenr, 1, fss->lastgoodframe); + } + + /* try to read from cache */ + if(((fss->lastgoodframe >= framenr) || (fss->lastgoodframe < 0)) && (result = fluidsim_read_cache(ob, dm, fluidmd, framenr, useRenderParams))) + { + // fss->lastgoodframe = framenr; // set also in src/fluidsim.c + return result; + } + else + { + // display last known good frame + if(fss->lastgoodframe >= 0) + { + if((result = fluidsim_read_cache(ob, dm, fluidmd, fss->lastgoodframe, useRenderParams))) + { + return result; + } + + // it was supposed to be a valid frame but it isn't! + fss->lastgoodframe = framenr - 1; + + + // this could be likely the case when you load an old fluidsim + if((result = fluidsim_read_cache(ob, dm, fluidmd, fss->lastgoodframe, useRenderParams))) + { + return result; + } + } + + result = CDDM_copy(dm); + + if(result) + { + return result; + } + } + + return dm; +#else + return NULL; +#endif +} + +#ifndef DISABLE_ELBEEM +/* read .bobj.gz file into a fluidsimDerivedMesh struct */ +static DerivedMesh *fluidsim_read_obj(char *filename) +{ + int wri,i,j; + float wrf; + int gotBytes; + gzFile gzf; + int numverts = 0, numfaces = 0; + DerivedMesh *dm = NULL; + MFace *mface; + MVert *mvert; + short *normals; + + // ------------------------------------------------ + // get numverts + numfaces first + // ------------------------------------------------ + gzf = gzopen(filename, "rb"); + if (!gzf) + { + return NULL; + } + + // read numverts + gotBytes = gzread(gzf, &wri, sizeof(wri)); + numverts = wri; + + // skip verts + for(i=0; ico[j] = wrf; + } + } + + // should be the same as numverts + gotBytes = gzread(gzf, &wri, sizeof(wri)); + if(wri != numverts) + { + if(dm) + dm->release(dm); + gzclose( gzf ); + return NULL; + } + + normals = MEM_callocN(sizeof(short) * numverts * 3, "fluid_tmp_normals" ); + if(!normals) + { + if(dm) + dm->release(dm); + gzclose( gzf ); + return NULL; + } + + // read normals from file (but don't save them yet) + for(i=0; iv1 = face[0]; + mf->v2 = face[1]; + mf->v3 = face[2]; + } + else + { + mf->v1 = face[1]; + mf->v2 = face[2]; + mf->v3 = face[0]; + } + mf->v4 = face[3]; + + test_index_face(mf, NULL, 0, 3); + } + + gzclose( gzf ); + + CDDM_calc_edges(dm); + + CDDM_apply_vert_normals(dm, (short (*)[3])normals); + MEM_freeN(normals); + + // CDDM_calc_normals(result); + + return dm; +} + +DerivedMesh *fluidsim_read_cache(Object *ob, DerivedMesh *orgdm, FluidsimModifierData *fluidmd, int framenr, int useRenderParams) +{ + int displaymode = 0; + int curFrame = framenr - 1 /*G.scene->r.sfra*/; /* start with 0 at start frame */ + char targetDir[FILE_MAXFILE+FILE_MAXDIR], targetFile[FILE_MAXFILE+FILE_MAXDIR]; + FluidsimSettings *fss = fluidmd->fss; + DerivedMesh *dm = NULL; + MFace *mface; + int numfaces; + int mat_nr, flag, i; + + if(!useRenderParams) { + displaymode = fss->guiDisplayMode; + } else { + displaymode = fss->renderDisplayMode; + } + + strncpy(targetDir, fss->surfdataPath, FILE_MAXDIR); + + // use preview or final mesh? + if(displaymode==1) + { + // just display original object + return NULL; + } + else if(displaymode==2) + { + strcat(targetDir,"fluidsurface_preview_####"); + } + else + { // 3 + strcat(targetDir,"fluidsurface_final_####"); + } + + BLI_convertstringcode(targetDir, G.sce); + BLI_convertstringframe(targetDir, curFrame); // fixed #frame-no + + strcpy(targetFile,targetDir); + strcat(targetFile, ".bobj.gz"); + + dm = fluidsim_read_obj(targetFile); + + if(!dm) + { + // switch, abort background rendering when fluidsim mesh is missing + const char *strEnvName2 = "BLENDER_ELBEEMBOBJABORT"; // from blendercall.cpp + + if(G.background==1) { + if(getenv(strEnvName2)) { + int elevel = atoi(getenv(strEnvName2)); + if(elevel>0) { + printf("Env. var %s set, fluid sim mesh '%s' not found, aborting render...\n",strEnvName2, targetFile); + exit(1); + } + } + } + + // display org. object upon failure which is in dm + return NULL; + } + + // assign material + flags to new dm + mface = orgdm->getFaceArray(orgdm); + mat_nr = mface[0].mat_nr; + flag = mface[0].flag; + + mface = dm->getFaceArray(dm); + numfaces = dm->getNumFaces(dm); + for(i=0; itotvert;i++) { for(j=0; j<3; j++) { srcob->fluidsimSettings->meshSurfNormals[i].co[j] = 0.; }} + }*/ + + return dm; +} + +void fluid_get_bb(MVert *mvert, int totvert, float obmat[][4], + /*RET*/ float start[3], /*RET*/ float size[3] ) +{ + float bbsx=0.0, bbsy=0.0, bbsz=0.0; + float bbex=1.0, bbey=1.0, bbez=1.0; + int i; + float vec[3]; + + VECCOPY(vec, mvert[0].co); + Mat4MulVecfl(obmat, vec); + bbsx = vec[0]; bbsy = vec[1]; bbsz = vec[2]; + bbex = vec[0]; bbey = vec[1]; bbez = vec[2]; + + for(i = 1; i < totvert; i++) { + VECCOPY(vec, mvert[i].co); + Mat4MulVecfl(obmat, vec); + + if(vec[0] < bbsx){ bbsx= vec[0]; } + if(vec[1] < bbsy){ bbsy= vec[1]; } + if(vec[2] < bbsz){ bbsz= vec[2]; } + if(vec[0] > bbex){ bbex= vec[0]; } + if(vec[1] > bbey){ bbey= vec[1]; } + if(vec[2] > bbez){ bbez= vec[2]; } + } + + // return values... + if(start) { + start[0] = bbsx; + start[1] = bbsy; + start[2] = bbsz; + } + if(size) { + size[0] = bbex-bbsx; + size[1] = bbey-bbsy; + size[2] = bbez-bbsz; + } +} + +//------------------------------------------------------------------------------- +// old interface +//------------------------------------------------------------------------------- + + + +//------------------------------------------------------------------------------- +// file handling +//------------------------------------------------------------------------------- + +void initElbeemMesh(struct Object *ob, + int *numVertices, float **vertices, + int *numTriangles, int **triangles, + int useGlobalCoords, int modifierIndex) +{ + DerivedMesh *dm = NULL; + MVert *mvert; + MFace *mface; + int countTris=0, i, totvert, totface; + float *verts; + int *tris; + + dm = mesh_create_derived_index_render(ob, CD_MASK_BAREMESH, modifierIndex); + //dm = mesh_create_derived_no_deform(ob,NULL); + + mvert = dm->getVertArray(dm); + mface = dm->getFaceArray(dm); + totvert = dm->getNumVerts(dm); + totface = dm->getNumFaces(dm); + + *numVertices = totvert; + verts = MEM_callocN( totvert*3*sizeof(float), "elbeemmesh_vertices"); + for(i=0; iobmat, &verts[i*3]); } + } + *vertices = verts; + + for(i=0; irelease(dm); +} + +/* read zipped fluidsim velocities into the co's of the fluidsimsettings normals struct */ +void readVelgz(char *filename, Object *srcob) +{ + int wri, i, j; + float wrf; + gzFile gzf; + MVert *vverts = srcob->fluidsimSettings->meshSurfNormals; + int len = strlen(filename); + Mesh *mesh = srcob->data; + // mesh and vverts have to be valid from loading... + + // clean up in any case + for(i=0; itotvert;i++) + { + for(j=0; j<3; j++) + { + vverts[i].co[j] = 0.; + } + } + if(srcob->fluidsimSettings->domainNovecgen>0) return; + + if(len<7) + { + return; + } + + // .bobj.gz , correct filename + // 87654321 + filename[len-6] = 'v'; + filename[len-5] = 'e'; + filename[len-4] = 'l'; + + gzf = gzopen(filename, "rb"); + if (!gzf) + return; + + gzread(gzf, &wri, sizeof( wri )); + if(wri != mesh->totvert) + { + return; + } + + for(i=0; itotvert;i++) + { + for(j=0; j<3; j++) + { + gzread(gzf, &wrf, sizeof( wrf )); + vverts[i].co[j] = wrf; + } + } + + gzclose(gzf); +} + + +#endif // DISABLE_ELBEEM + diff --git a/source/blender/blenkernel/intern/simple_deform.c b/source/blender/blenkernel/intern/simple_deform.c new file mode 100644 index 00000000000..0eb710fa48e --- /dev/null +++ b/source/blender/blenkernel/intern/simple_deform.c @@ -0,0 +1,248 @@ +/** + * deform_simple.c + * + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) Blender Foundation. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): André Pinto + * + * ***** END GPL LICENSE BLOCK ***** + */ +#include "DNA_object_types.h" +#include "DNA_modifier_types.h" +#include "DNA_meshdata_types.h" + +#include "BKE_simple_deform.h" +#include "BKE_DerivedMesh.h" +#include "BKE_deform.h" +#include "BKE_utildefines.h" +#include "BLI_arithb.h" +#include "BKE_shrinkwrap.h" + +#include +#include + + +//Clamps/Limits the given coordinate to: limits[0] <= co[axis] <= limits[1] +//The ammount of clamp is saved on dcut +static void axis_limit(int axis, const float limits[2], float co[3], float dcut[3]) +{ + float val = co[axis]; + if(limits[0] > val) val = limits[0]; + if(limits[1] < val) val = limits[1]; + + dcut[axis] = co[axis] - val; + co[axis] = val; +} + +static void simpleDeform_taper(const float factor, const float dcut[3], float *co) +{ + float x = co[0], y = co[1], z = co[2]; + float scale = z*factor; + + co[0] = x + x*scale; + co[1] = y + y*scale; + co[2] = z; + + if(dcut) + { + co[0] += dcut[0]; + co[1] += dcut[1]; + co[2] += dcut[2]; + } +} + +static void simpleDeform_stretch(const float factor, const float dcut[3], float *co) +{ + float x = co[0], y = co[1], z = co[2]; + float scale; + + scale = (z*z*factor-factor + 1.0); + + co[0] = x*scale; + co[1] = y*scale; + co[2] = z*(1.0+factor); + + + if(dcut) + { + co[0] += dcut[0]; + co[1] += dcut[1]; + co[2] += dcut[2]; + } +} + +static void simpleDeform_twist(const float factor, const float *dcut, float *co) +{ + float x = co[0], y = co[1], z = co[2]; + float theta, sint, cost; + + theta = z*factor; + sint = sin(theta); + cost = cos(theta); + + co[0] = x*cost - y*sint; + co[1] = x*sint + y*cost; + co[2] = z; + + if(dcut) + { + co[0] += dcut[0]; + co[1] += dcut[1]; + co[2] += dcut[2]; + } +} + +static void simpleDeform_bend(const float factor, const float dcut[3], float *co) +{ + float x = co[0], y = co[1], z = co[2]; + float theta, sint, cost; + + theta = x*factor; + sint = sin(theta); + cost = cos(theta); + + if(fabs(factor) > 1e-7f) + { + co[0] = -(y-1.0f/factor)*sint; + co[1] = (y-1.0f/factor)*cost + 1.0f/factor; + co[2] = z; + } + + + if(dcut) + { + co[0] += cost*dcut[0]; + co[1] += sint*dcut[0]; + co[2] += dcut[2]; + } + +} + + +/* simple deform modifier */ +void SimpleDeformModifier_do(SimpleDeformModifierData *smd, struct Object *ob, struct DerivedMesh *dm, float (*vertexCos)[3], int numVerts) +{ + static const float lock_axis[2] = {0.0f, 0.0f}; + + int i; + int limit_axis = 0; + float smd_limit[2], smd_factor; + SpaceTransform *transf = NULL, tmp_transf; + void (*simpleDeform_callback)(const float factor, const float dcut[3], float *co) = NULL; //Mode callback + int vgroup = get_named_vertexgroup_num(ob, smd->vgroup_name); + MDeformVert *dvert = NULL; + + //Safe-check + if(smd->origin == ob) smd->origin = NULL; //No self references + + if(smd->limit[0] < 0.0) smd->limit[0] = 0.0f; + if(smd->limit[0] > 1.0) smd->limit[0] = 1.0f; + + smd->limit[0] = MIN2(smd->limit[0], smd->limit[1]); //Upper limit >= than lower limit + + //Calculate matrixs do convert between coordinate spaces + if(smd->origin) + { + transf = &tmp_transf; + + if(smd->originOpts & MOD_SIMPLEDEFORM_ORIGIN_LOCAL) + { + space_transform_from_matrixs(transf, ob->obmat, smd->origin->obmat); + } + else + { + Mat4CpyMat4(transf->local2target, smd->origin->obmat); + Mat4Invert(transf->target2local, transf->local2target); + } + } + + //Setup vars + limit_axis = (smd->mode == MOD_SIMPLEDEFORM_MODE_BEND) ? 0 : 2; //Bend limits on X.. all other modes limit on Z + + //Update limits if needed + { + float lower = FLT_MAX; + float upper = -FLT_MAX; + + for(i=0; ilimit[1]; + smd_limit[0] = lower + (upper-lower)*smd->limit[0]; + + smd_factor = smd->factor / MAX2(FLT_EPSILON, smd_limit[1]-smd_limit[0]); + } + + + if(dm) + dvert = dm->getVertDataArray(dm, CD_MDEFORMVERT); + + + switch(smd->mode) + { + case MOD_SIMPLEDEFORM_MODE_TWIST: simpleDeform_callback = simpleDeform_twist; break; + case MOD_SIMPLEDEFORM_MODE_BEND: simpleDeform_callback = simpleDeform_bend; break; + case MOD_SIMPLEDEFORM_MODE_TAPER: simpleDeform_callback = simpleDeform_taper; break; + case MOD_SIMPLEDEFORM_MODE_STRETCH: simpleDeform_callback = simpleDeform_stretch; break; + default: + return; //No simpledeform mode? + } + + for(i=0; imode != MOD_SIMPLEDEFORM_MODE_BEND) //Bend mode shoulnt have any lock axis + { + if(smd->axis & MOD_SIMPLEDEFORM_LOCK_AXIS_X) axis_limit(0, lock_axis, co, dcut); + if(smd->axis & MOD_SIMPLEDEFORM_LOCK_AXIS_Y) axis_limit(1, lock_axis, co, dcut); + } + axis_limit(limit_axis, smd_limit, co, dcut); + + simpleDeform_callback(smd_factor, dcut, co); //Apply deform + VecLerpf(vertexCos[i], vertexCos[i], co, weight); //Use vertex weight has coef of linear interpolation + + if(transf) space_transform_invert(transf, vertexCos[i]); + } + } +} + + diff --git a/source/blender/freestyle/intern/rendering/GLBlendEquation.cpp b/source/blender/freestyle/intern/rendering/GLBlendEquation.cpp new file mode 100644 index 00000000000..5dc0985aa53 --- /dev/null +++ b/source/blender/freestyle/intern/rendering/GLBlendEquation.cpp @@ -0,0 +1,9 @@ +#include "GLBlendEquation.h" + +void FRS_glBlendEquation(GLenum mode) { + if( glBlendEquation ) { + glBlendEquation(mode); + } else if ( glBlendEquationEXT ) { + glBlendEquationEXT(mode); + } +} \ No newline at end of file diff --git a/source/blender/freestyle/intern/rendering/GLBlendEquation.h b/source/blender/freestyle/intern/rendering/GLBlendEquation.h new file mode 100644 index 00000000000..89a90837bea --- /dev/null +++ b/source/blender/freestyle/intern/rendering/GLBlendEquation.h @@ -0,0 +1,16 @@ +#ifndef GLBLENDEQUATION_H +#define GLBLENDEQUATION_H + +#include +#ifdef WIN32 +# include +#endif +#ifdef __MACH__ +# include +#else +# include +#endif + +void FRS_glBlendEquation(GLenum mode); + +#endif // GLBLENDEQUATION_H diff --git a/source/blender/include/BIF_keyframing.h b/source/blender/include/BIF_keyframing.h new file mode 100644 index 00000000000..7989e6adb45 --- /dev/null +++ b/source/blender/include/BIF_keyframing.h @@ -0,0 +1,117 @@ +/** + * $Id: BIF_keyframing.h 14444 2008-04-16 22:40:48Z aligorith $ + * + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place * Suite 330, Boston, MA 02111*1307, USA. + * + * The Original Code is Copyright (C) 2008, Blender Foundation + * This is a new part of Blender (with some old code) + * + * Contributor(s): Joshua Leung + * + * ***** END GPL LICENSE BLOCK ***** + */ + +#ifndef BIF_KEYFRAMING_H +#define BIF_KEYFRAMING_H + +struct ListBase; +struct ID; + +struct IpoCurve; +struct BezTriple; + +/* ************ Keyframing Management **************** */ + +/* Lesser Keyframing API call: + * Use this when validation of necessary animation data isn't necessary as it already + * exists, and there is a beztriple that can be directly copied into the array. + */ +int insert_bezt_icu(struct IpoCurve *icu, struct BezTriple *bezt); + +/* Main Keyframing API call: + * Use this when validation of necessary animation data isn't necessary as it + * already exists. It will insert a keyframe using the current value being keyframed. + */ +void insert_vert_icu(struct IpoCurve *icu, float x, float y, short flag); + + +/* flags for use in insert_key(), and insert_vert_icu() */ +enum { + INSERTKEY_NEEDED = (1<<0), /* only insert keyframes where they're needed */ + INSERTKEY_MATRIX = (1<<1), /* insert 'visual' keyframes where possible/needed */ + INSERTKEY_FAST = (1<<2), /* don't recalculate handles,etc. after adding key */ + INSERTKEY_FASTR = (1<<3), /* don't realloc mem (or increase count, as array has already been set out) */ + INSERTKEY_REPLACE = (1<<4), /* only replace an existing keyframe (this overrides INSERTKEY_NEEDED) */ +} eInsertKeyFlags; + +/* -------- */ + +/* Main Keyframing API calls: + * Use this to create any necessary animation data,, and then insert a keyframe + * using the current value being keyframed, in the relevant place. Returns success. + */ + // TODO: adapt this for new data-api -> this blocktype, etc. stuff is evil! +short insertkey(struct ID *id, int blocktype, char *actname, char *constname, int adrcode, short flag); + +/* Main Keyframing API call: + * Use this to delete keyframe on current frame for relevant channel. Will perform checks just in case. + */ +short deletekey(struct ID *id, int blocktype, char *actname, char *constname, int adrcode, short flag); + + +/* Main Keyframe Management calls: + * These handle keyframes management from various spaces. They will handle the menus + * required for each space. + */ +void common_insertkey(void); +void common_deletekey(void); + +/* ************ Auto-Keyframing ********************** */ +/* Notes: + * - All the defines for this (User-Pref settings and Per-Scene settings) + * are defined in DNA_userdef_types.h + * - Scene settings take presidence over those for userprefs, with old files + * inheriting userpref settings for the scene settings + * - "On/Off + Mode" are stored per Scene, but "settings" are currently stored + * as userprefs + */ + +/* Auto-Keying macros for use by various tools */ + /* check if auto-keyframing is enabled (per scene takes presidence) */ +#define IS_AUTOKEY_ON ((G.scene) ? (G.scene->autokey_mode & AUTOKEY_ON) : (U.autokey_mode & AUTOKEY_ON)) + /* check the mode for auto-keyframing (per scene takes presidence) */ +#define IS_AUTOKEY_MODE(mode) ((G.scene) ? (G.scene->autokey_mode == AUTOKEY_MODE_##mode) : (U.autokey_mode == AUTOKEY_MODE_##mode)) + /* check if a flag is set for auto-keyframing (as userprefs only!) */ +#define IS_AUTOKEY_FLAG(flag) (U.autokey_flag & AUTOKEY_FLAG_##flag) + +/* ************ Keyframe Checking ******************** */ + +/* Checks whether a keyframe exists for the given ID-block one the given frame */ +short id_cfra_has_keyframe(struct ID *id, short filter); + +/* filter flags fr id_cfra_has_keyframe */ +enum { + /* general */ + ANIMFILTER_ALL = 0, /* include all available animation data */ + ANIMFILTER_LOCAL = (1<<0), /* only include locally available anim data */ + + /* object specific */ + ANIMFILTER_MAT = (1<<1), /* include material keyframes too */ + ANIMFILTER_SKEY = (1<<2), /* shape keys (for geometry) */ +} eAnimFilterFlags; + +#endif /* BIF_KEYFRAMING_H */ diff --git a/source/blender/src/keyframing.c b/source/blender/src/keyframing.c new file mode 100644 index 00000000000..07b10811559 --- /dev/null +++ b/source/blender/src/keyframing.c @@ -0,0 +1,1870 @@ +/** + * $Id: keyframing.c 14881 2008-05-18 10:41:42Z aligorith $ + * + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2008, Blender Foundation + * This is a new part of Blender (with some old code) + * + * Contributor(s): Joshua Leung + * + * ***** END GPL LICENSE BLOCK ***** + */ + +#include +#include +#include +#include +#include + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include "MEM_guardedalloc.h" + +#include "BLI_arithb.h" +#include "BLI_blenlib.h" +#include "BLI_dynstr.h" + +#include "DNA_listBase.h" +#include "DNA_ID.h" +#include "DNA_action_types.h" +#include "DNA_armature_types.h" +#include "DNA_camera_types.h" +#include "DNA_constraint_types.h" +#include "DNA_curve_types.h" +#include "DNA_ipo_types.h" +#include "DNA_lamp_types.h" +#include "DNA_object_types.h" +#include "DNA_material_types.h" +#include "DNA_screen_types.h" +#include "DNA_scene_types.h" +#include "DNA_space_types.h" +#include "DNA_texture_types.h" +#include "DNA_userdef_types.h" +#include "DNA_vec_types.h" +#include "DNA_view3d_types.h" +#include "DNA_world_types.h" + +#include "BKE_global.h" +#include "BKE_utildefines.h" +#include "BKE_blender.h" +#include "BKE_main.h" +#include "BKE_action.h" +#include "BKE_armature.h" +#include "BKE_constraint.h" +#include "BKE_curve.h" +#include "BKE_depsgraph.h" +#include "BKE_ipo.h" +#include "BKE_object.h" + +#include "BIF_keyframing.h" +#include "BIF_butspace.h" +#include "BIF_editaction.h" +#include "BIF_editkey.h" +#include "BIF_interface.h" +#include "BIF_mywindow.h" +#include "BIF_poseobject.h" +#include "BIF_screen.h" +#include "BIF_space.h" +#include "BIF_toolbox.h" +#include "BIF_toets.h" + +#include "BSE_editipo.h" +#include "BSE_node.h" +#include "BSE_time.h" +#include "BSE_view.h" + +#include "blendef.h" + +#include "PIL_time.h" /* sleep */ +#include "mydevice.h" + +/* ************************************************** */ +/* LOCAL TYPES AND DEFINES */ + +/* -------------- Keying Sets ------------------- */ + +/* keying set - a set of channels that will be keyframed together */ +// TODO: move this to a header to allow custom sets someday? +typedef struct bKeyingSet { + /* callback func to consider if keyingset should be included + * (by default, if this is undefined, item will be shown) + */ + short (*include_cb)(struct bKeyingSet *, const char *); + + char name[48]; /* name of keyingset */ + int blocktype; /* blocktype that all channels belong to */ // in future, this may be eliminated + short flag; /* flags to use when setting keyframes */ + + short chan_num; /* number of channels to insert keyframe in */ + short adrcodes[32]; /* adrcodes for channels to insert keys for (ideally would be variable-len, but limit of 32 will suffice) */ +} bKeyingSet; + +/* keying set context - an array of keying sets and the number of them */ +typedef struct bKeyingContext { + bKeyingSet *keyingsets; /* array containing the keyingsets of interest */ + bKeyingSet *lastused; /* item that was chosen last time*/ + int tot; /* number of keyingsets in */ +} bKeyingContext; + + +/* ----------- Common KeyData Sources ------------ */ + +/* temporary struct to gather data combos to keyframe */ +typedef struct bCommonKeySrc { + struct bCommonKeySrc *next, *prev; + + /* general data/destination-source settings */ + ID *id; /* id-block this comes from */ + char *actname; /* name of action channel */ + char *constname; /* name of constraint channel */ + + /* general destination source settings */ + Ipo *ipo; /* ipo-block that id-block has (optional) */ + bAction *act; /* action-block that id-block has (optional) */ + + /* pose-level settings */ + bPoseChannel *pchan; /* pose channel */ + + /* buttons-window settings */ + int map; /* offset to apply to certain adrcodes */ +} bCommonKeySrc; + +/* ************************************************** */ +/* KEYFRAME INSERTION */ + +/* -------------- BezTriple Insertion -------------------- */ + +/* threshold for inserting keyframes - threshold here should be good enough for now, but should become userpref */ +#define BEZT_INSERT_THRESH 0.00001 + +/* Binary search algorithm for finding where to insert BezTriple. (for use by insert_bezt_icu) + * Returns the index to insert at (data already at that index will be offset if replace is 0) + */ +static int binarysearch_bezt_index (BezTriple array[], BezTriple *item, int arraylen, short *replace) +{ + int start=0, end=arraylen; + int loopbreaker= 0, maxloop= arraylen * 2; + const float frame= (item)? item->vec[1][0] : 0.0f; + + /* initialise replace-flag first */ + *replace= 0; + + /* sneaky optimisations (don't go through searching process if...): + * - keyframe to be added is to be added out of current bounds + * - keyframe to be added would replace one of the existing ones on bounds + */ + if ((arraylen <= 0) || ELEM(NULL, array, item)) { + printf("Warning: binarysearch_bezt_index encountered invalid array \n"); + return 0; + } + else { + /* check whether to add before/after/on */ + float framenum; + + /* 'First' Keyframe (when only one keyframe, this case is used) */ + framenum= array[0].vec[1][0]; + if (IS_EQT(frame, framenum, BEZT_INSERT_THRESH)) { + *replace = 1; + return 0; + } + else if (frame < framenum) + return 0; + + /* 'Last' Keyframe */ + framenum= array[(arraylen-1)].vec[1][0]; + if (IS_EQT(frame, framenum, BEZT_INSERT_THRESH)) { + *replace= 1; + return (arraylen - 1); + } + else if (frame > framenum) + return arraylen; + } + + + /* most of the time, this loop is just to find where to put it + * 'loopbreaker' is just here to prevent infinite loops + */ + for (loopbreaker=0; (start <= end) && (loopbreaker < maxloop); loopbreaker++) { + /* compute and get midpoint */ + int mid = (start + end) / 2; + float midfra= array[mid].vec[1][0]; + + /* check if exactly equal to midpoint */ + if (IS_EQT(frame, midfra, BEZT_INSERT_THRESH)) { + *replace = 1; + return mid; + } + + /* repeat in upper/lower half */ + if (frame > midfra) + start= mid + 1; + else if (frame < midfra) + end= mid - 1; + } + + /* print error if loop-limit exceeded */ + if (loopbreaker == (maxloop-1)) { + printf("Error: binarysearch_bezt_index was taking too long \n"); + + // include debug info + printf("\tround = %d: start = %d, end = %d, arraylen = %d \n", loopbreaker, start, end, arraylen); + } + + /* not found, so return where to place it */ + return start; +} + +/* This function adds a given BezTriple to an IPO-Curve. It will allocate + * memory for the array if needed, and will insert the BezTriple into a + * suitable place in chronological order. + * + * NOTE: any recalculate of the IPO-Curve that needs to be done will need to + * be done by the caller. + */ +int insert_bezt_icu (IpoCurve *icu, BezTriple *bezt) +{ + BezTriple *newb; + int i= 0; + + if (icu->bezt == NULL) { + icu->bezt= MEM_callocN(sizeof(BezTriple), "beztriple"); + *(icu->bezt)= *bezt; + icu->totvert= 1; + } + else { + short replace = -1; + i = binarysearch_bezt_index(icu->bezt, bezt, icu->totvert, &replace); + + if (replace) { + /* sanity check: 'i' may in rare cases exceed arraylen */ + if ((i >= 0) && (i < icu->totvert)) + *(icu->bezt + i) = *bezt; + } + else { + /* add new */ + newb= MEM_callocN((icu->totvert+1)*sizeof(BezTriple), "beztriple"); + + /* add the beztriples that should occur before the beztriple to be pasted (originally in ei->icu) */ + if (i > 0) + memcpy(newb, icu->bezt, i*sizeof(BezTriple)); + + /* add beztriple to paste at index i */ + *(newb + i)= *bezt; + + /* add the beztriples that occur after the beztriple to be pasted (originally in icu) */ + if (i < icu->totvert) + memcpy(newb+i+1, icu->bezt+i, (icu->totvert-i)*sizeof(BezTriple)); + + /* replace (+ free) old with new */ + MEM_freeN(icu->bezt); + icu->bezt= newb; + + icu->totvert++; + } + } + + /* we need to return the index, so that some tools which do post-processing can + * detect where we added the BezTriple in the array + */ + return i; +} + +/* This function is a wrapper for insert_bezt_icu, and should be used when + * adding a new keyframe to a curve, when the keyframe doesn't exist anywhere + * else yet. + * + * 'fast' - is only for the python API where importing BVH's would take an extreamly long time. + */ +void insert_vert_icu (IpoCurve *icu, float x, float y, short fast) +{ + BezTriple beztr; + int a, h1, h2; + + /* set all three points, for nicer start position */ + memset(&beztr, 0, sizeof(BezTriple)); + beztr.vec[0][0]= x; + beztr.vec[0][1]= y; + beztr.vec[1][0]= x; + beztr.vec[1][1]= y; + beztr.vec[2][0]= x; + beztr.vec[2][1]= y; + beztr.hide= IPO_BEZ; + beztr.f1= beztr.f2= beztr.f3= SELECT; + beztr.h1= beztr.h2= HD_AUTO; + + /* add temp beztriple to keyframes */ + a= insert_bezt_icu(icu, &beztr); + if (!fast) calchandles_ipocurve(icu); + + /* set handletype */ + if (icu->totvert > 2) { + BezTriple *bezt; + + h1= h2= HD_AUTO; + bezt= (icu->bezt + a); + + if (a > 0) h1= (bezt-1)->h2; + if (a < icu->totvert-1) h2= (bezt+1)->h1; + + bezt->h1= h1; + bezt->h2= h2; + + if (!fast) calchandles_ipocurve(icu); + } +} + +/* ------------------- Get Data ------------------------ */ + +/* Get pointer to use to get values from */ +// FIXME: this should not be possible with Data-API +static void *get_context_ipo_poin(ID *id, int blocktype, char *actname, char *constname, IpoCurve *icu, int *vartype) +{ + switch (blocktype) { + case ID_PO: /* posechannel */ + if (GS(id->name)==ID_OB) { + Object *ob= (Object *)id; + bPoseChannel *pchan= get_pose_channel(ob->pose, actname); + + if (pchan) { + *vartype= IPO_FLOAT; + return get_pchan_ipo_poin(pchan, icu->adrcode); + } + } + break; + + case ID_CO: /* constraint */ + if ((GS(id->name)==ID_OB) && (constname && constname[0])) { + Object *ob= (Object *)id; + bConstraint *con; + + /* assume that we only want the influence (as only used for Constraint Channels) */ + if ((ob->ipoflag & OB_ACTION_OB) && !strcmp(actname, "Object")) { + for (con= ob->constraints.first; con; con= con->next) { + if (strcmp(constname, con->name)==0) { + *vartype= IPO_FLOAT; + return &con->enforce; + } + } + } + else if (ob->pose) { + bPoseChannel *pchan= get_pose_channel(ob->pose, actname); + + if (pchan) { + for (con= pchan->constraints.first; con; con= con->next) { + if (strcmp(constname, con->name)==0) { + *vartype= IPO_FLOAT; + return &con->enforce; + } + } + } + } + } + break; + + case ID_OB: /* object */ + /* hack: layer channels for object need to be keyed WITHOUT localview flag... + * tsk... tsk... why must we just dump bitflags upon users :/ + */ + if ((GS(id->name)==ID_OB) && (icu->adrcode==OB_LAY)) { + Object *ob= (Object *)id; + static int layer = 0; + + /* init layer to be the object's layer var, then remove local view from it */ + layer = ob->lay; + layer &= 0xFFFFFF; + *vartype= IPO_INT_BIT; + + /* return pointer to this static var + * - assumes that this pointer won't be stored for use later, so may not be threadsafe + * if multiple keyframe calls are made, but that is unlikely to happen in the near future + */ + return (void *)(&layer); + } + /* no break here for other ob channel-types - as they can be done normally */ + + default: /* normal data-source */ + return get_ipo_poin(id, icu, vartype); + } + + /* not valid... */ + return NULL; +} + + +/* -------------- 'Smarter' Keyframing Functions -------------------- */ +/* return codes for new_key_needed */ +enum { + KEYNEEDED_DONTADD = 0, + KEYNEEDED_JUSTADD, + KEYNEEDED_DELPREV, + KEYNEEDED_DELNEXT +} eKeyNeededStatus; + +/* This helper function determines whether a new keyframe is needed */ +/* Cases where keyframes should not be added: + * 1. Keyframe to be added bewteen two keyframes with similar values + * 2. Keyframe to be added on frame where two keyframes are already situated + * 3. Keyframe lies at point that intersects the linear line between two keyframes + */ +static short new_key_needed (IpoCurve *icu, float cFrame, float nValue) +{ + BezTriple *bezt=NULL, *prev=NULL; + int totCount, i; + float valA = 0.0f, valB = 0.0f; + + /* safety checking */ + if (icu == NULL) return KEYNEEDED_JUSTADD; + totCount= icu->totvert; + if (totCount == 0) return KEYNEEDED_JUSTADD; + + /* loop through checking if any are the same */ + bezt= icu->bezt; + for (i=0; ivec[1][0]; + beztVal= bezt->vec[1][1]; + + if (prev) { + /* there is a keyframe before the one currently being examined */ + + /* get previous time+value */ + prevPosi= prev->vec[1][0]; + prevVal= prev->vec[1][1]; + + /* keyframe to be added at point where there are already two similar points? */ + if (IS_EQ(prevPosi, cFrame) && IS_EQ(beztPosi, cFrame) && IS_EQ(beztPosi, prevPosi)) { + return KEYNEEDED_DONTADD; + } + + /* keyframe between prev+current points ? */ + if ((prevPosi <= cFrame) && (cFrame <= beztPosi)) { + /* is the value of keyframe to be added the same as keyframes on either side ? */ + if (IS_EQ(prevVal, nValue) && IS_EQ(beztVal, nValue) && IS_EQ(prevVal, beztVal)) { + return KEYNEEDED_DONTADD; + } + else { + float realVal; + + /* get real value of curve at that point */ + realVal= eval_icu(icu, cFrame); + + /* compare whether it's the same as proposed */ + if (IS_EQ(realVal, nValue)) + return KEYNEEDED_DONTADD; + else + return KEYNEEDED_JUSTADD; + } + } + + /* new keyframe before prev beztriple? */ + if (cFrame < prevPosi) { + /* A new keyframe will be added. However, whether the previous beztriple + * stays around or not depends on whether the values of previous/current + * beztriples and new keyframe are the same. + */ + if (IS_EQ(prevVal, nValue) && IS_EQ(beztVal, nValue) && IS_EQ(prevVal, beztVal)) + return KEYNEEDED_DELNEXT; + else + return KEYNEEDED_JUSTADD; + } + } + else { + /* just add a keyframe if there's only one keyframe + * and the new one occurs before the exisiting one does. + */ + if ((cFrame < beztPosi) && (totCount==1)) + return KEYNEEDED_JUSTADD; + } + + /* continue. frame to do not yet passed (or other conditions not met) */ + if (i < (totCount-1)) { + prev= bezt; + bezt++; + } + else + break; + } + + /* Frame in which to add a new-keyframe occurs after all other keys + * -> If there are at least two existing keyframes, then if the values of the + * last two keyframes and the new-keyframe match, the last existing keyframe + * gets deleted as it is no longer required. + * -> Otherwise, a keyframe is just added. 1.0 is added so that fake-2nd-to-last + * keyframe is not equal to last keyframe. + */ + bezt= (icu->bezt + (icu->totvert - 1)); + valA= bezt->vec[1][1]; + + if (prev) + valB= prev->vec[1][1]; + else + valB= bezt->vec[1][1] + 1.0f; + + if (IS_EQ(valA, nValue) && IS_EQ(valA, valB)) + return KEYNEEDED_DELPREV; + else + return KEYNEEDED_JUSTADD; +} + +/* ------------------ 'Visual' Keyframing Functions ------------------ */ + +/* internal status codes for visualkey_can_use */ +enum { + VISUALKEY_NONE = 0, + VISUALKEY_LOC, + VISUALKEY_ROT +}; + +/* This helper function determines if visual-keyframing should be used when + * inserting keyframes for the given channel. As visual-keyframing only works + * on Object and Pose-Channel blocks, this should only get called for those + * blocktypes, when using "standard" keying but 'Visual Keying' option in Auto-Keying + * settings is on. + */ +static short visualkey_can_use (ID *id, int blocktype, char *actname, char *constname, int adrcode) +{ + Object *ob= NULL; + bConstraint *con= NULL; + short searchtype= VISUALKEY_NONE; + + /* validate data */ + if ((id == NULL) || (GS(id->name)!=ID_OB) || !(ELEM(blocktype, ID_OB, ID_PO))) + return 0; + + /* get first constraint and determine type of keyframe constraints to check for*/ + ob= (Object *)id; + + if (blocktype == ID_OB) { + con= ob->constraints.first; + + if (ELEM3(adrcode, OB_LOC_X, OB_LOC_Y, OB_LOC_Z)) + searchtype= VISUALKEY_LOC; + else if (ELEM3(adrcode, OB_ROT_X, OB_ROT_Y, OB_ROT_Z)) + searchtype= VISUALKEY_ROT; + } + else if (blocktype == ID_PO) { + bPoseChannel *pchan= get_pose_channel(ob->pose, actname); + con= pchan->constraints.first; + + if (ELEM3(adrcode, AC_LOC_X, AC_LOC_Y, AC_LOC_Z)) + searchtype= VISUALKEY_LOC; + else if (ELEM4(adrcode, AC_QUAT_W, AC_QUAT_X, AC_QUAT_Y, AC_QUAT_Z)) + searchtype= VISUALKEY_ROT; + } + + /* only search if a searchtype and initial constraint are available */ + if (searchtype && con) { + for (; con; con= con->next) { + /* only consider constraint if it is not disabled, and has influence */ + if (con->flag & CONSTRAINT_DISABLE) continue; + if (con->enforce == 0.0f) continue; + + /* some constraints may alter these transforms */ + switch (con->type) { + /* multi-transform constraints */ + case CONSTRAINT_TYPE_CHILDOF: + return 1; + case CONSTRAINT_TYPE_TRANSFORM: + return 1; + case CONSTRAINT_TYPE_FOLLOWPATH: + return 1; + + /* single-transform constraits */ + case CONSTRAINT_TYPE_TRACKTO: + if (searchtype==VISUALKEY_ROT) return 1; + break; + case CONSTRAINT_TYPE_ROTLIMIT: + if (searchtype==VISUALKEY_ROT) return 1; + break; + case CONSTRAINT_TYPE_LOCLIMIT: + if (searchtype==VISUALKEY_LOC) return 1; + break; + case CONSTRAINT_TYPE_ROTLIKE: + if (searchtype==VISUALKEY_ROT) return 1; + break; + case CONSTRAINT_TYPE_DISTLIMIT: + if (searchtype==VISUALKEY_LOC) return 1; + break; + case CONSTRAINT_TYPE_LOCLIKE: + if (searchtype==VISUALKEY_LOC) return 1; + break; + case CONSTRAINT_TYPE_LOCKTRACK: + if (searchtype==VISUALKEY_ROT) return 1; + break; + case CONSTRAINT_TYPE_MINMAX: + if (searchtype==VISUALKEY_LOC) return 1; + break; + + default: + break; + } + } + } + + /* when some condition is met, this function returns, so here it can be 0 */ + return 0; +} + +/* This helper function extracts the value to use for visual-keyframing + * In the event that it is not possible to perform visual keying, try to fall-back + * to using the poin method. Assumes that all data it has been passed is valid. + */ +static float visualkey_get_value (ID *id, int blocktype, char *actname, char *constname, int adrcode, IpoCurve *icu) +{ + Object *ob; + void *poin = NULL; + int index, vartype; + + /* validate situtation */ + if ((id==NULL) || (GS(id->name)!=ID_OB) || (ELEM(blocktype, ID_OB, ID_PO)==0)) + return 0.0f; + + /* get object */ + ob= (Object *)id; + + /* only valid for objects or posechannels */ + if (blocktype == ID_OB) { + /* parented objects are not supported, as the effects of the parent + * are included in the matrix, which kindof beats the point + */ + if (ob->parent == NULL) { + /* only Location or Rotation keyframes are supported now */ + if (ELEM3(adrcode, OB_LOC_X, OB_LOC_Y, OB_LOC_Z)) { + /* assumes that OB_LOC_Z > OB_LOC_Y > OB_LOC_X */ + index= adrcode - OB_LOC_X; + + return ob->obmat[3][index]; + } + else if (ELEM3(adrcode, OB_ROT_X, OB_ROT_Y, OB_ROT_Z)) { + float eul[3]; + + /* assumes that OB_ROT_Z > OB_ROT_Y > OB_ROT_X */ + index= adrcode - OB_ROT_X; + + Mat4ToEul(ob->obmat, eul); + return eul[index]*(5.72958); + } + } + } + else if (blocktype == ID_PO) { + bPoseChannel *pchan; + float tmat[4][4]; + + /* get data to use */ + pchan= get_pose_channel(ob->pose, actname); + + /* Although it is not strictly required for this particular space conversion, + * arg1 must not be null, as there is a null check for the other conversions to + * be safe. Therefore, the active object is passed here, and in many cases, this + * will be what owns the pose-channel that is getting this anyway. + */ + Mat4CpyMat4(tmat, pchan->pose_mat); + constraint_mat_convertspace(ob, pchan, tmat, CONSTRAINT_SPACE_POSE, CONSTRAINT_SPACE_LOCAL); + + /* Loc, Rot/Quat keyframes are supported... */ + if (ELEM3(adrcode, AC_LOC_X, AC_LOC_Y, AC_LOC_Z)) { + /* assumes that AC_LOC_Z > AC_LOC_Y > AC_LOC_X */ + index= adrcode - AC_LOC_X; + + /* only use for non-connected bones */ + if ((pchan->bone->parent) && !(pchan->bone->flag & BONE_CONNECTED)) + return tmat[3][index]; + else if (pchan->bone->parent == NULL) + return tmat[3][index]; + } + else if (ELEM4(adrcode, AC_QUAT_W, AC_QUAT_X, AC_QUAT_Y, AC_QUAT_Z)) { + float trimat[3][3], quat[4]; + + /* assumes that AC_QUAT_Z > AC_QUAT_Y > AC_QUAT_X > AC_QUAT_W */ + index= adrcode - AC_QUAT_W; + + Mat3CpyMat4(trimat, tmat); + Mat3ToQuat_is_ok(trimat, quat); + + return quat[index]; + } + } + + /* as the function hasn't returned yet, try reading from poin */ + poin= get_context_ipo_poin(id, blocktype, actname, constname, icu, &vartype); + if (poin) + return read_ipo_poin(poin, vartype); + else + return 0.0; +} + + +/* ------------------------- Insert Key API ------------------------- */ + +/* Main Keyframing API call: + * Use this when validation of necessary animation data isn't necessary as it + * already exists. It will insert a keyframe using the current value being keyframed. + * + * The flag argument is used for special settings that alter the behaviour of + * the keyframe insertion. These include the 'visual' keyframing modes, quick refresh, + * and extra keyframe filtering. + */ +short insertkey (ID *id, int blocktype, char *actname, char *constname, int adrcode, short flag) +{ + IpoCurve *icu; + + /* get ipo-curve */ + icu= verify_ipocurve(id, blocktype, actname, constname, NULL, adrcode, 1); + + /* only continue if we have an ipo-curve to add keyframe to */ + if (icu) { + float cfra = frame_to_float(CFRA); + float curval= 0.0f; + void *poin = NULL; + int vartype; + + /* apply special time tweaking */ + if (GS(id->name) == ID_OB) { + Object *ob= (Object *)id; + + /* apply NLA-scaling (if applicable) */ + if (actname && actname[0]) + cfra= get_action_frame(ob, cfra); + + /* ancient time-offset cruft */ + if ( (ob->ipoflag & OB_OFFS_OB) && (give_timeoffset(ob)) ) { + /* actually frametofloat calc again! */ + cfra-= give_timeoffset(ob)*G.scene->r.framelen; + } + } + + /* get pointer to data to read from */ + poin= get_context_ipo_poin(id, blocktype, actname, constname, icu, &vartype); + if (poin == NULL) return 0; + + /* obtain value to give keyframe */ + if ( (flag & INSERTKEY_MATRIX) && + (visualkey_can_use(id, blocktype, actname, constname, adrcode)) ) + { + /* visual-keying is only available for object and pchan datablocks, as + * it works by keyframing using a value extracted from the final matrix + * instead of using the kt system to extract a value. + */ + curval= visualkey_get_value(id, blocktype, actname, constname, adrcode, icu); + } + else { + /* use kt's read_poin function to extract value (kt->read_poin should + * exist in all cases, but it never hurts to check) + */ + curval= read_ipo_poin(poin, vartype); + } + + /* only insert keyframes where they are needed */ + if (flag & INSERTKEY_NEEDED) { + short insert_mode; + + /* check whether this curve really needs a new keyframe */ + insert_mode= new_key_needed(icu, cfra, curval); + + /* insert new keyframe at current frame */ + if (insert_mode) + insert_vert_icu(icu, cfra, curval, (flag & INSERTKEY_FAST)); + + /* delete keyframe immediately before/after newly added */ + switch (insert_mode) { + case KEYNEEDED_DELPREV: + delete_icu_key(icu, icu->totvert-2, 1); + break; + case KEYNEEDED_DELNEXT: + delete_icu_key(icu, 1, 1); + break; + } + } + else { + /* just insert keyframe */ + insert_vert_icu(icu, cfra, curval, (flag & INSERTKEY_FAST)); + } + + /* return success */ + return 1; + } + + /* return failure */ + return 0; +} + + +/* ************************************************** */ +/* KEYFRAME DELETION */ + +/* Main Keyframing API call: + * Use this when validation of necessary animation data isn't necessary as it + * already exists. It will delete a keyframe at the current frame. + * + * The flag argument is used for special settings that alter the behaviour of + * the keyframe deletion. These include the quick refresh options. + */ +short deletekey (ID *id, int blocktype, char *actname, char *constname, int adrcode, short flag) +{ + Ipo *ipo; + IpoCurve *icu; + + /* get ipo-curve + * Note: here is one of the places where we don't want new ipo + ipo-curve added! + * so 'add' var must be 0 + */ + ipo= verify_ipo(id, blocktype, actname, constname, NULL, 0); + icu= verify_ipocurve(id, blocktype, actname, constname, NULL, adrcode, 0); + + /* only continue if we have an ipo-curve to remove keyframes from */ + if (icu) { + BezTriple bezt; + float cfra = frame_to_float(CFRA); + short found = -1; + int i; + + /* apply special time tweaking */ + if (GS(id->name) == ID_OB) { + Object *ob= (Object *)id; + + /* apply NLA-scaling (if applicable) */ + if (actname && actname[0]) + cfra= get_action_frame(ob, cfra); + + /* ancient time-offset cruft */ + if ( (ob->ipoflag & OB_OFFS_OB) && (give_timeoffset(ob)) ) { + /* actually frametofloat calc again! */ + cfra-= give_timeoffset(ob)*G.scene->r.framelen; + } + } + + /* only need to set bezt->vec[1][0], as that's all binarysearch uses */ + memset(&bezt, 0, sizeof(BezTriple)); + bezt.vec[1][0]= cfra; + + /* try to find index of beztriple to get rid of */ + i = binarysearch_bezt_index(icu->bezt, &bezt, icu->totvert, &found); + if (found) { + /* delete the key at the index (will sanity check + do recalc afterwards ) */ + delete_icu_key(icu, i, 1); + + /* Only delete curve too if there isn't an ipo-driver still hanging around on an empty curve */ + if (icu->totvert==0 && icu->driver==NULL) { + BLI_remlink(&ipo->curve, icu); + free_ipo_curve(icu); + } + + /* return success */ + return 1; + } + } + + /* return failure */ + return 0; +} + +/* ************************************************** */ +/* COMMON KEYFRAME MANAGEMENT (common_insertkey/deletekey) */ + + +/* ------------- KeyingSet Defines ------------ */ +/* Note: these must all be named with the defks_* prefix, otherwise the template macro will not work! */ + +/* macro for defining keyingset contexts */ +#define KSC_TEMPLATE(ctx_name) {&defks_##ctx_name[0], NULL, sizeof(defks_##ctx_name)/sizeof(bKeyingSet)} + +/* --- */ + +/* check if option not available for deleting keys */ +static short incl_non_del_keys (bKeyingSet *ks, const char mode[]) +{ + /* as optimisation, assume that it is sufficient to check only first letter + * of mode (int comparison should be faster than string!) + */ + //if (strcmp(mode, "Delete")==0) + if (mode && mode[0]=='D') + return 0; + + return 1; +} + +/* Object KeyingSets ------ */ + +/* check if include shapekey entry */ +static short incl_v3d_ob_shapekey (bKeyingSet *ks, const char mode[]) +{ + Object *ob= (G.obedit)? (G.obedit) : (OBACT); + char *newname= NULL; + + /* not available for delete mode */ + if (strcmp(mode, "Delete")==0) + return 0; + + /* check if is geom object that can get shapekeys */ + switch (ob->type) { + /* geometry? */ + case OB_MESH: newname= "Mesh"; break; + case OB_CURVE: newname= "Curve"; break; + case OB_SURF: newname= "Surface"; break; + case OB_LATTICE: newname= "Lattice"; break; + + /* not geometry! */ + default: + return 0; + } + + /* if ks is shapekey entry (this could be callled for separator before too!) */ + if (ks->flag == -3) + sprintf(ks->name, newname); + + /* if it gets here, it's ok */ + return 1; +} + +/* array for object keyingset defines */ +bKeyingSet defks_v3d_object[] = +{ + /* include_cb, name, blocktype, flag, chan_num, adrcodes */ + {NULL, "Loc", ID_OB, 0, 3, {OB_LOC_X,OB_LOC_Y,OB_LOC_Z}}, + {NULL, "Rot", ID_OB, 0, 3, {OB_ROT_X,OB_ROT_Y,OB_ROT_Z}}, + {NULL, "Scale", ID_OB, 0, 3, {OB_SIZE_X,OB_SIZE_Y,OB_SIZE_Z}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "LocRot", ID_OB, 0, 6, + {OB_LOC_X,OB_LOC_Y,OB_LOC_Z, + OB_ROT_X,OB_ROT_Y,OB_ROT_Z}}, + + {NULL, "LocScale", ID_OB, 0, 6, + {OB_LOC_X,OB_LOC_Y,OB_LOC_Z, + OB_SIZE_X,OB_SIZE_Y,OB_SIZE_Z}}, + + {NULL, "LocRotScale", ID_OB, 0, 9, + {OB_LOC_X,OB_LOC_Y,OB_LOC_Z, + OB_ROT_X,OB_ROT_Y,OB_ROT_Z, + OB_SIZE_X,OB_SIZE_Y,OB_SIZE_Z}}, + + {NULL, "RotScale", ID_OB, 0, 6, + {OB_ROT_X,OB_ROT_Y,OB_ROT_Z, + OB_SIZE_X,OB_SIZE_Y,OB_SIZE_Z}}, + + {incl_non_del_keys, "%l", 0, -1, 0, {0}}, // separator + + {incl_non_del_keys, "VisualLoc", ID_OB, INSERTKEY_MATRIX, 3, {OB_LOC_X,OB_LOC_Y,OB_LOC_Z}}, + {incl_non_del_keys, "VisualRot", ID_OB, INSERTKEY_MATRIX, 3, {OB_ROT_X,OB_ROT_Y,OB_ROT_Z}}, + + {incl_non_del_keys, "VisualLocRot", ID_OB, INSERTKEY_MATRIX, 6, + {OB_LOC_X,OB_LOC_Y,OB_LOC_Z, + OB_ROT_X,OB_ROT_Y,OB_ROT_Z}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Layer", ID_OB, 0, 1, {OB_LAY}}, // icky option... + {NULL, "Available", ID_OB, -2, 0, {0}}, + + {incl_v3d_ob_shapekey, "%l%l", 0, -1, 0, {0}}, // separator (linked to shapekey entry) + {incl_v3d_ob_shapekey, "", ID_OB, -3, 0, {0}} +}; + +/* PoseChannel KeyingSets ------ */ + +/* array for posechannel keyingset defines */ +bKeyingSet defks_v3d_pchan[] = +{ + /* include_cb, name, blocktype, flag, chan_num, adrcodes */ + {NULL, "Loc", ID_PO, 0, 3, {AC_LOC_X,AC_LOC_Y,AC_LOC_Z}}, + {NULL, "Rot", ID_PO, 0, 4, {AC_QUAT_W,AC_QUAT_X,AC_QUAT_Y,AC_QUAT_Z}}, + {NULL, "Scale", ID_PO, 0, 3, {AC_SIZE_X,AC_SIZE_Y,AC_SIZE_Z}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "LocRot", ID_PO, 0, 7, + {AC_LOC_X,AC_LOC_Y,AC_LOC_Z,AC_QUAT_W, + AC_QUAT_X,AC_QUAT_Y,AC_QUAT_Z}}, + + {NULL, "LocScale", ID_PO, 0, 6, + {AC_LOC_X,AC_LOC_Y,AC_LOC_Z, + AC_SIZE_X,AC_SIZE_Y,AC_SIZE_Z}}, + + {NULL, "LocRotScale", ID_PO, 0, 10, + {AC_LOC_X,AC_LOC_Y,AC_LOC_Z,AC_QUAT_W,AC_QUAT_X, + AC_QUAT_Y,AC_QUAT_Z,AC_SIZE_X,AC_SIZE_Y,AC_SIZE_Z}}, + + {NULL, "RotScale", ID_PO, 0, 7, + {AC_QUAT_W,AC_QUAT_X,AC_QUAT_Y,AC_QUAT_Z, + AC_SIZE_X,AC_SIZE_Y,AC_SIZE_Z}}, + + {incl_non_del_keys, "%l", 0, -1, 0, {0}}, // separator + + {incl_non_del_keys, "VisualLoc", ID_PO, INSERTKEY_MATRIX, 3, {AC_LOC_X,AC_LOC_Y,AC_LOC_Z}}, + {incl_non_del_keys, "VisualRot", ID_PO, INSERTKEY_MATRIX, 4, {AC_QUAT_W,AC_QUAT_X,AC_QUAT_Y,AC_QUAT_Z}}, + + {incl_non_del_keys, "VisualLocRot", ID_PO, INSERTKEY_MATRIX, 7, + {AC_LOC_X,AC_LOC_Y,AC_LOC_Z,AC_QUAT_W, + AC_QUAT_X,AC_QUAT_Y,AC_QUAT_Z}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Available", ID_PO, -2, 0, {0}} +}; + +/* Material KeyingSets ------ */ + +/* array for material keyingset defines */ +bKeyingSet defks_buts_shading_mat[] = +{ + /* include_cb, name, blocktype, flag, chan_num, adrcodes */ + {NULL, "RGB", ID_MA, 0, 3, {MA_COL_R,MA_COL_G,MA_COL_B}}, + {NULL, "Alpha", ID_MA, 0, 1, {MA_ALPHA}}, + {NULL, "Halo Size", ID_MA, 0, 1, {MA_HASIZE}}, + {NULL, "Mode", ID_MA, 0, 1, {MA_MODE}}, // evil bitflags + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "All Color", ID_MA, 0, 18, + {MA_COL_R,MA_COL_G,MA_COL_B, + MA_ALPHA,MA_HASIZE, MA_MODE, + MA_SPEC_R,MA_SPEC_G,MA_SPEC_B, + MA_REF,MA_EMIT,MA_AMB,MA_SPEC,MA_HARD, + MA_MODE,MA_TRANSLU,MA_ADD}}, + + {NULL, "All Mirror", ID_MA, 0, 5, + {MA_RAYM,MA_FRESMIR,MA_FRESMIRI, + MA_FRESTRA,MA_FRESTRAI}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Ofs", ID_MA, 0, 3, {MAP_OFS_X,MAP_OFS_Y,MAP_OFS_Z}}, + {NULL, "Size", ID_MA, 0, 3, {MAP_SIZE_X,MAP_SIZE_Y,MAP_SIZE_Z}}, + + {NULL, "All Mapping", ID_MA, 0, 14, + {MAP_OFS_X,MAP_OFS_Y,MAP_OFS_Z, + MAP_SIZE_X,MAP_SIZE_Y,MAP_SIZE_Z, + MAP_R,MAP_G,MAP_B,MAP_DVAR, + MAP_COLF,MAP_NORF,MAP_VARF,MAP_DISP}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Available", ID_MA, -2, 0, {0}} +}; + +/* World KeyingSets ------ */ + +/* array for world keyingset defines */ +bKeyingSet defks_buts_shading_wo[] = +{ + /* include_cb, name, blocktype, flag, chan_num, adrcodes */ + {NULL, "Zenith RGB", ID_WO, 0, 3, {WO_ZEN_R,WO_ZEN_G,WO_ZEN_B}}, + {NULL, "Horizon RGB", ID_WO, 0, 3, {WO_HOR_R,WO_HOR_G,WO_HOR_B}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Mist", ID_WO, 0, 4, {WO_MISI,WO_MISTDI,WO_MISTSTA,WO_MISTHI}}, + {NULL, "Stars", ID_WO, 0, 5, {WO_STAR_R,WO_STAR_G,WO_STAR_B,WO_STARDIST,WO_STARSIZE}}, + + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Ofs", ID_WO, 0, 3, {MAP_OFS_X,MAP_OFS_Y,MAP_OFS_Z}}, + {NULL, "Size", ID_WO, 0, 3, {MAP_SIZE_X,MAP_SIZE_Y,MAP_SIZE_Z}}, + + {NULL, "All Mapping", ID_WO, 0, 14, + {MAP_OFS_X,MAP_OFS_Y,MAP_OFS_Z, + MAP_SIZE_X,MAP_SIZE_Y,MAP_SIZE_Z, + MAP_R,MAP_G,MAP_B,MAP_DVAR, + MAP_COLF,MAP_NORF,MAP_VARF,MAP_DISP}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Available", ID_WO, -2, 0, {0}} +}; + +/* Lamp KeyingSets ------ */ + +/* array for lamp keyingset defines */ +bKeyingSet defks_buts_shading_la[] = +{ + /* include_cb, name, blocktype, flag, chan_num, adrcodes */ + {NULL, "RGB", ID_LA, 0, 3, {LA_COL_R,LA_COL_G,LA_COL_B}}, + {NULL, "Energy", ID_LA, 0, 1, {LA_ENERGY}}, + {NULL, "Spot Size", ID_LA, 0, 1, {LA_SPOTSI}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Ofs", ID_LA, 0, 3, {MAP_OFS_X,MAP_OFS_Y,MAP_OFS_Z}}, + {NULL, "Size", ID_LA, 0, 3, {MAP_SIZE_X,MAP_SIZE_Y,MAP_SIZE_Z}}, + + {NULL, "All Mapping", ID_LA, 0, 14, + {MAP_OFS_X,MAP_OFS_Y,MAP_OFS_Z, + MAP_SIZE_X,MAP_SIZE_Y,MAP_SIZE_Z, + MAP_R,MAP_G,MAP_B,MAP_DVAR, + MAP_COLF,MAP_NORF,MAP_VARF,MAP_DISP}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Available", ID_LA, -2, 0, {0}} +}; + +/* Texture KeyingSets ------ */ + +/* array for texture keyingset defines */ +bKeyingSet defks_buts_shading_tex[] = +{ + /* include_cb, name, blocktype, flag, chan_num, adrcodes */ + {NULL, "Clouds", ID_TE, 0, 5, + {TE_NSIZE,TE_NDEPTH,TE_NTYPE, + TE_MG_TYP,TE_N_BAS1}}, + + {NULL, "Marble", ID_TE, 0, 7, + {TE_NSIZE,TE_NDEPTH,TE_NTYPE, + TE_TURB,TE_MG_TYP,TE_N_BAS1,TE_N_BAS2}}, + + {NULL, "Stucci", ID_TE, 0, 5, + {TE_NSIZE,TE_NTYPE,TE_TURB, + TE_MG_TYP,TE_N_BAS1}}, + + {NULL, "Wood", ID_TE, 0, 6, + {TE_NSIZE,TE_NTYPE,TE_TURB, + TE_MG_TYP,TE_N_BAS1,TE_N_BAS2}}, + + {NULL, "Magic", ID_TE, 0, 2, {TE_NDEPTH,TE_TURB}}, + + {NULL, "Blend", ID_TE, 0, 1, {TE_MG_TYP}}, + + {NULL, "Musgrave", ID_TE, 0, 6, + {TE_MG_TYP,TE_MGH,TE_MG_LAC, + TE_MG_OCT,TE_MG_OFF,TE_MG_GAIN}}, + + {NULL, "Voronoi", ID_TE, 0, 9, + {TE_VNW1,TE_VNW2,TE_VNW3,TE_VNW4, + TE_VNMEXP,TE_VN_DISTM,TE_VN_COLT, + TE_ISCA,TE_NSIZE}}, + + {NULL, "Distorted Noise", ID_TE, 0, 4, + {TE_MG_OCT,TE_MG_OFF,TE_MG_GAIN,TE_DISTA}}, + + {NULL, "Color Filter", ID_TE, 0, 5, + {TE_COL_R,TE_COL_G,TE_COL_B,TE_BRIGHT,TE_CONTRA}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Available", ID_TE, -2, 0, {0}} +}; + +/* Object Buttons KeyingSets ------ */ + +/* check if include particles entry */ +static short incl_buts_ob (bKeyingSet *ks, const char mode[]) +{ + Object *ob= OBACT; + /* only if object is mesh type */ + return (ob->type == OB_MESH); +} + +/* array for texture keyingset defines */ +bKeyingSet defks_buts_object[] = +{ + /* include_cb, name, blocktype, flag, chan_num, adrcodes */ + {incl_buts_ob, "Surface Damping", ID_OB, 0, 1, {OB_PD_SDAMP}}, + {incl_buts_ob, "Random Damping", ID_OB, 0, 1, {OB_PD_RDAMP}}, + {incl_buts_ob, "Permeability", ID_OB, 0, 1, {OB_PD_PERM}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Force Strength", ID_OB, 0, 1, {OB_PD_FSTR}}, + {NULL, "Force Falloff", ID_OB, 0, 1, {OB_PD_FFALL}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Available", ID_OB, -2, 0, {0}} // this will include ob-transforms too! +}; + +/* Camera Buttons KeyingSets ------ */ + +/* check if include internal-renderer entry */ +static short incl_buts_cam1 (bKeyingSet *ks, const char mode[]) +{ + /* only if renderer is internal renderer */ + return (G.scene->r.renderer==R_INTERN); +} + +/* check if include external-renderer entry */ +static short incl_buts_cam2 (bKeyingSet *ks, const char mode[]) +{ + /* only if renderer is internal renderer */ + return (G.scene->r.renderer!=R_INTERN); +} + +/* array for camera keyingset defines */ +bKeyingSet defks_buts_cam[] = +{ + /* include_cb, name, blocktype, flag, chan_num, adrcodes */ + {NULL, "Lens", ID_CA, 0, 1, {CAM_LENS}}, + {NULL, "Clipping", ID_CA, 0, 2, {CAM_STA,CAM_END}}, + {NULL, "Focal Distance", ID_CA, 0, 1, {CAM_YF_FDIST}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + + {incl_buts_cam2, "Aperture", ID_CA, 0, 1, {CAM_YF_APERT}}, + {incl_buts_cam1, "Viewplane Shift", ID_CA, 0, 2, {CAM_SHIFT_X,CAM_SHIFT_Y}}, + + {NULL, "%l", 0, -1, 0, {0}}, // separator + + {NULL, "Available", ID_CA, -2, 0, {0}} +}; + +/* --- */ + +/* Keying Context Defines - Must keep in sync with enumeration (eKS_Contexts) */ +bKeyingContext ks_contexts[] = +{ + KSC_TEMPLATE(v3d_object), + KSC_TEMPLATE(v3d_pchan), + + KSC_TEMPLATE(buts_shading_mat), + KSC_TEMPLATE(buts_shading_wo), + KSC_TEMPLATE(buts_shading_la), + KSC_TEMPLATE(buts_shading_tex), + + KSC_TEMPLATE(buts_object), + KSC_TEMPLATE(buts_cam) +}; + +/* Keying Context Enumeration - Must keep in sync with definitions*/ +typedef enum eKS_Contexts { + KSC_V3D_OBJECT = 0, + KSC_V3D_PCHAN, + + KSC_BUTS_MAT, + KSC_BUTS_WO, + KSC_BUTS_LA, + KSC_BUTS_TEX, + + KSC_BUTS_OB, + KSC_BUTS_CAM, + + /* make sure this last one remains untouched! */ + KSC_TOT_TYPES +} eKS_Contexts; + + +/* ---------------- KeyingSet Tools ------------------- */ + +/* helper for commonkey_context_get() - get keyingsets for 3d-view */ +static void commonkey_context_getv3d (ListBase *sources, bKeyingContext **ksc) +{ + Object *ob; + IpoCurve *icu; + + if ((OBACT) && (OBACT->flag & OB_POSEMODE)) { + bPoseChannel *pchan; + + /* pose-level */ + ob= OBACT; + *ksc= &ks_contexts[KSC_V3D_PCHAN]; + set_pose_keys(ob); /* sets pchan->flag to POSE_KEY if bone selected, and clears if not */ + + /* loop through posechannels */ + for (pchan=ob->pose->chanbase.first; pchan; pchan=pchan->next) { + if (pchan->flag & POSE_KEY) { + bCommonKeySrc *cks; + + /* add new keyframing destination */ + cks= MEM_callocN(sizeof(bCommonKeySrc), "bCommonKeySrc"); + BLI_addtail(sources, cks); + + /* set id-block to key to, and action */ + cks->id= (ID *)ob; + cks->act= ob->action; + + /* set pchan */ + cks->pchan= pchan; + cks->actname= pchan->name; + } + } + } + else { + Base *base; + + /* object-level */ + *ksc= &ks_contexts[KSC_V3D_OBJECT]; + + /* loop through bases */ + for (base= FIRSTBASE; base; base= base->next) { + if (TESTBASELIB(base)) { + bCommonKeySrc *cks; + + /* add new keyframing destination */ + cks= MEM_callocN(sizeof(bCommonKeySrc), "bCommonKeySrc"); + BLI_addtail(sources, cks); + + /* set id-block to key to */ + ob= base->object; + cks->id= (ID *)ob; + + /* when ob's keyframes are in an action, default to using 'Object' as achan name */ + if (ob->ipoflag & OB_ACTION_OB) + cks->actname= "Object"; + + /* set ipo-flags */ + // TODO: add checks for lib-linked data + if ((ob->ipo) || (ob->action)) { + if (ob->ipo) { + cks->ipo= ob->ipo; + } + else { + bActionChannel *achan; + + cks->act= ob->action; + achan= get_action_channel(ob->action, cks->actname); + + if (achan && achan->ipo) + cks->ipo= achan->ipo; + } + + /* deselect all ipo-curves */ + for (icu= cks->ipo->curve.first; icu; icu= icu->next) { + icu->flag &= ~IPO_SELECT; + } + } + } + } + } +} + +/* helper for commonkey_context_get() - get keyingsets for buttons window */ +static void commonkey_context_getsbuts (ListBase *sources, bKeyingContext **ksc) +{ + bCommonKeySrc *cks; + + /* check on tab-type */ + switch (G.buts->mainb) { + case CONTEXT_SHADING: /* ------------- Shading buttons ---------------- */ + /* subtabs include "Material", "Texture", "Lamp", "World"*/ + switch (G.buts->tab[CONTEXT_SHADING]) { + case TAB_SHADING_MAT: /* >------------- Material Tab -------------< */ + { + Material *ma= editnode_get_active_material(G.buts->lockpoin); + + /* add new keyframing destination */ + cks= MEM_callocN(sizeof(bCommonKeySrc), "bCommonKeySrc"); + BLI_addtail(sources, cks); + + /* set data */ + cks->id= (ID *)ma; + cks->ipo= ma->ipo; + cks->map= texchannel_to_adrcode(ma->texact); + + /* set keyingsets */ + *ksc= &ks_contexts[KSC_BUTS_MAT]; + return; + } + break; + case TAB_SHADING_WORLD: /* >------------- World Tab -------------< */ + { + World *wo= G.buts->lockpoin; + + /* add new keyframing destination */ + cks= MEM_callocN(sizeof(bCommonKeySrc), "bCommonKeySrc"); + BLI_addtail(sources, cks); + + /* set data */ + cks->id= (ID *)wo; + cks->ipo= wo->ipo; + cks->map= texchannel_to_adrcode(wo->texact); + + /* set keyingsets */ + *ksc= &ks_contexts[KSC_BUTS_WO]; + return; + } + break; + case TAB_SHADING_LAMP: /* >------------- Lamp Tab -------------< */ + { + Lamp *la= G.buts->lockpoin; + + /* add new keyframing destination */ + cks= MEM_callocN(sizeof(bCommonKeySrc), "bCommonKeySrc"); + BLI_addtail(sources, cks); + + /* set data */ + cks->id= (ID *)la; + cks->ipo= la->ipo; + cks->map= texchannel_to_adrcode(la->texact); + + /* set keyingsets */ + *ksc= &ks_contexts[KSC_BUTS_LA]; + return; + } + break; + case TAB_SHADING_TEX: /* >------------- Texture Tab -------------< */ + { + Tex *tex= G.buts->lockpoin; + + /* add new keyframing destination */ + cks= MEM_callocN(sizeof(bCommonKeySrc), "bCommonKeySrc"); + BLI_addtail(sources, cks); + + /* set data */ + cks->id= (ID *)tex; + cks->ipo= tex->ipo; + + /* set keyingsets */ + *ksc= &ks_contexts[KSC_BUTS_TEX]; + return; + } + break; + } + break; + + case CONTEXT_OBJECT: /* ------------- Object buttons ---------------- */ + { + Object *ob= OBACT; + + if (ob) { + /* add new keyframing destination */ + cks= MEM_callocN(sizeof(bCommonKeySrc), "bCommonKeySrc"); + BLI_addtail(sources, cks); + + /* set id-block to key to */ + cks->id= (ID *)ob; + cks->ipo= ob->ipo; + + /* set keyingsets */ + *ksc= &ks_contexts[KSC_BUTS_OB]; + return; + } + } + break; + + case CONTEXT_EDITING: /* ------------- Editing buttons ---------------- */ + { + Object *ob= OBACT; + + if ((ob) && (ob->type==OB_CAMERA) && (G.buts->lockpoin)) { /* >---------------- camera buttons ---------------< */ + Camera *ca= G.buts->lockpoin; + + /* add new keyframing destination */ + cks= MEM_callocN(sizeof(bCommonKeySrc), "bCommonKeySrc"); + BLI_addtail(sources, cks); + + /* set id-block to key to */ + cks->id= (ID *)ca; + cks->ipo= ca->ipo; + + /* set keyingsets */ + *ksc= &ks_contexts[KSC_BUTS_CAM]; + return; + } + } + break; + } + + /* if nothing happened... */ + *ksc= NULL; +} + + +/* get keyingsets for appropriate context */ +static void commonkey_context_get (ScrArea *sa, ListBase *sources, bKeyingContext **ksc) +{ + /* check view type */ + switch (sa->spacetype) { + /* 3d view - first one tested as most often used */ + case SPACE_VIEW3D: + { + commonkey_context_getv3d(sources, ksc); + } + break; + + /* buttons view */ + case SPACE_BUTS: + { + commonkey_context_getsbuts(sources, ksc); + } + break; + + /* timeline view - keyframe buttons */ + case SPACE_TIME: + { + ScrArea *sab; + + /* try to find largest 3d-view available + * (mostly of the time, this is what when user will want this, + * as it's a standard feature in all other apps) + */ + sab= find_biggest_area_of_type(SPACE_VIEW3D); + if (sab) { + commonkey_context_getv3d(sources, ksc); + return; + } + + /* otherwise, try to find the biggest area + * WARNING: must check if that area is another timeline, as that would cause infinite loop + */ + sab= closest_bigger_area(); + if ((sab) && (sab->spacetype != SPACE_TIME)) + commonkey_context_get(sab, sources, ksc); + } + break; + } +} + +/* flush updates after all operations */ +static void commonkey_context_finish (ListBase *sources) +{ + /* check view type */ + switch (curarea->spacetype) { + /* 3d view - first one tested as most often used */ + case SPACE_VIEW3D: + { + /* either pose or object level */ + if (OBACT && (OBACT->pose)) { + Object *ob= OBACT; + + /* recalculate ipo handles, etc. */ + if (ob->action) + remake_action_ipos(ob->action); + + /* recalculate bone-paths on adding new keyframe? */ + // TODO: currently, there is no setting to turn this on/off globally + if (ob->pose->flag & POSE_RECALCPATHS) + pose_recalculate_paths(ob); + } + else { + bCommonKeySrc *cks; + + /* loop over bases (as seen in sources) */ + for (cks= sources->first; cks; cks= cks->next) { + Object *ob= (Object *)cks->id; + + /* simply set recalc flag */ + ob->recalc |= OB_RECALC_OB; + } + } + } + break; + } +} + +/* flush refreshes after undo */ +static void commonkey_context_refresh (void) +{ + /* check view type */ + switch (curarea->spacetype) { + /* 3d view - first one tested as most often used */ + case SPACE_VIEW3D: + { + /* do refreshes */ + DAG_scene_flush_update(G.scene, screen_view3d_layers(), 0); + + allspace(REMAKEIPO, 0); + allqueue(REDRAWVIEW3D, 0); + allqueue(REDRAWMARKER, 0); + } + break; + } +} + +/* --- */ + +/* Build menu-string of available keying-sets (allocates memory for string) + * NOTE: mode must not be longer than 64 chars + */ +static char *build_keyingsets_menu (bKeyingContext *ksc, const char mode[48]) +{ + DynStr *pupds= BLI_dynstr_new(); + bKeyingSet *ks; + char buf[64]; + char *str; + int i, n; + + /* add title first */ + BLI_snprintf(buf, 64, "%s Key %%t|", mode); + BLI_dynstr_append(pupds, buf); + + /* loop through keyingsets, adding them */ + for (ks=ksc->keyingsets, i=0, n=1; i < ksc->tot; ks++, i++, n++) { + /* check if keyingset can be used */ + if (ks->flag == -1) { + /* optional separator? */ + if (ks->include_cb) { + if (ks->include_cb(ks, mode)) { + BLI_snprintf( buf, 64, "%s%s", ks->name, ((n < ksc->tot)?"|":"") ); + BLI_dynstr_append(pupds, buf); + } + } + else { + BLI_snprintf( buf, 64, "%%l%s", ((n < ksc->tot)?"|":"") ); + BLI_dynstr_append(pupds, buf); + } + } + else if ( (ks->include_cb==NULL) || (ks->include_cb(ks, mode)) ) { + /* entry can be included */ + BLI_dynstr_append(pupds, ks->name); + + /* check if special "shapekey" entry */ + if (ks->flag == -3) + BLI_snprintf( buf, 64, "%%x0%s", ((n < ksc->tot)?"|":"") ); + else + BLI_snprintf( buf, 64, "%%x%d%s", n, ((n < ksc->tot)?"|":"") ); + BLI_dynstr_append(pupds, buf); + } + } + + /* convert to normal MEM_malloc'd string */ + str= BLI_dynstr_get_cstring(pupds); + BLI_dynstr_free(pupds); + + return str; +} + +/* Get the keying set that was chosen by the user from the menu */ +static bKeyingSet *get_keyingset_fromcontext (bKeyingContext *ksc, short index) +{ + /* check if index is valid */ + if (ELEM(NULL, ksc, ksc->keyingsets)) + return NULL; + if ((index < 1) || (index > ksc->tot)) + return NULL; + + /* index starts from 1, and should directly correspond to keyingset in array */ + return (bKeyingSet *)(ksc->keyingsets + (index - 1)); +} + +/* ---------------- Keyframe Management API -------------------- */ + +/* mode for common_modifykey */ +enum { + COMMONKEY_MODE_INSERT = 0, + COMMONKEY_MODE_DELETE, +} eCommonModifyKey_Modes; + +/* Display a menu for handling the insertion of keyframes based on the active view */ +// TODO: add back an option for repeating last keytype +void common_modifykey (short mode) +{ + ListBase dsources = {NULL, NULL}; + bKeyingContext *ksc= NULL; + bCommonKeySrc *cks; + bKeyingSet *ks = NULL; + char *menustr, buf[64]; + short menu_nr; + + /* check if mode is valid */ + if (ELEM(mode, COMMONKEY_MODE_INSERT, COMMONKEY_MODE_DELETE)==0) + return; + + /* delegate to other functions or get keyingsets to use */ + switch (curarea->spacetype) { + /* spaces with their own methods */ + case SPACE_IPO: + if (mode == COMMONKEY_MODE_INSERT) + insertkey_editipo(); + return; + case SPACE_ACTION: + if (mode == COMMONKEY_MODE_INSERT) + insertkey_action(); + return; + + /* TODO: based on UI elements? will that even be handled here??? */ + + /* default - check per view */ + default: + /* get the keyingsets and the data to add keyframes to */ + commonkey_context_get(curarea, &dsources, &ksc); + break; + } + + /* check that there is data to operate on */ + if (ELEM(NULL, dsources.first, ksc)) { + BLI_freelistN(&dsources); + return; + } + + /* get menu and process it */ + if (mode == COMMONKEY_MODE_DELETE) + menustr= build_keyingsets_menu(ksc, "Delete"); + else + menustr= build_keyingsets_menu(ksc, "Insert"); + menu_nr= pupmenu(menustr); + if (menustr) MEM_freeN(menustr); + + /* no item selected or shapekey entry? */ + if (menu_nr < 1) { + /* free temp sources */ + BLI_freelistN(&dsources); + + /* check if insert new shapekey */ + if ((menu_nr == 0) && (mode == COMMONKEY_MODE_INSERT)) + insert_shapekey(OBACT); + else + ksc->lastused= NULL; + + return; + } + else { + /* try to get keyingset */ + ks= get_keyingset_fromcontext(ksc, menu_nr); + + if (ks == NULL) { + BLI_freelistN(&dsources); + return; + } + } + + /* loop over each destination, applying the keying set */ + for (cks= dsources.first; cks; cks= cks->next) { + short success= 0; + + /* special hacks for 'available' option */ + if (ks->flag == -2) { + IpoCurve *icu= NULL, *icn= NULL; + + /* get first IPO-curve */ + if (cks->act && cks->actname) { + bActionChannel *achan= get_action_channel(cks->act, cks->actname); + + // FIXME: what about constraint channels? + if (achan && achan->ipo) + icu= achan->ipo->curve.first; + } + else + icu= cks->ipo->curve.first; + + /* we get adrcodes directly from IPO curves (see method below...) */ + for (; icu; icu= icn) { + short flag; + + /* get next ipo-curve in case current is deleted */ + icn= icu->next; + + /* insert mode or delete mode */ + if (mode == COMMONKEY_MODE_DELETE) { + /* local flags only add on to global flags */ + flag = 0; + + /* delete keyframe */ + success += deletekey(cks->id, ks->blocktype, cks->actname, cks->constname, icu->adrcode, flag); + } + else { + /* local flags only add on to global flags */ + flag = ks->flag; + if (IS_AUTOKEY_FLAG(AUTOMATKEY)) flag |= INSERTKEY_MATRIX; + if (IS_AUTOKEY_FLAG(INSERTNEEDED)) flag |= INSERTKEY_NEEDED; + // if (IS_AUTOKEY_MODE(EDITKEYS)) flag |= INSERTKEY_REPLACE; + + /* insert keyframe */ + success += insertkey(cks->id, ks->blocktype, cks->actname, cks->constname, icu->adrcode, flag); + } + } + } + else { + int i; + + /* loop over channels available in keyingset */ + for (i=0; i < ks->chan_num; i++) { + short flag, adrcode; + + /* get adrcode + * - certain adrcodes (for MTEX channels need special offsets) // BAD CRUFT!!! + */ + adrcode= ks->adrcodes[i]; + if (ELEM3(ks->blocktype, ID_MA, ID_LA, ID_WO)) { + switch (adrcode) { + case MAP_OFS_X: case MAP_OFS_Y: case MAP_OFS_Z: + case MAP_SIZE_X: case MAP_SIZE_Y: case MAP_SIZE_Z: + case MAP_R: case MAP_G: case MAP_B: case MAP_DVAR: + case MAP_COLF: case MAP_NORF: case MAP_VARF: case MAP_DISP: + adrcode += cks->map; + break; + } + } + + /* insert mode or delete mode */ + if (mode == COMMONKEY_MODE_DELETE) { + /* local flags only add on to global flags */ + flag = 0; + + /* delete keyframe */ + success += deletekey(cks->id, ks->blocktype, cks->actname, cks->constname, adrcode, flag); + } + else { + /* local flags only add on to global flags */ + flag = ks->flag; + if (IS_AUTOKEY_FLAG(AUTOMATKEY)) flag |= INSERTKEY_MATRIX; + if (IS_AUTOKEY_FLAG(INSERTNEEDED)) flag |= INSERTKEY_NEEDED; + // if (IS_AUTOKEY_MODE(EDITKEYS)) flag |= INSERTKEY_REPLACE; + + /* insert keyframe */ + success += insertkey(cks->id, ks->blocktype, cks->actname, cks->constname, adrcode, flag); + } + } + } + + /* special handling for some key-sources */ + if (success) { + /* set pose recalc-paths flag */ + if (cks->pchan) { + Object *ob= (Object *)cks->id; + bPoseChannel *pchan= cks->pchan; + + /* set flag to trigger path recalc */ + if (pchan->path) + ob->pose->flag |= POSE_RECALCPATHS; + + /* clear unkeyed flag (it doesn't matter if it's set or not) */ + if (pchan->bone) + pchan->bone->flag &= ~BONE_UNKEYED; + } + } + } + + /* apply post-keying flushes for this data sources */ + commonkey_context_finish(&dsources); + ksc->lastused= ks; + + /* free temp data */ + BLI_freelistN(&dsources); + + /* undo pushes */ + if (mode == COMMONKEY_MODE_DELETE) + BLI_snprintf(buf, 64, "Delete %s Key", ks->name); + else + BLI_snprintf(buf, 64, "Insert %s Key", ks->name); + BIF_undo_push(buf); + + /* queue updates for contexts */ + commonkey_context_refresh(); +} + +/* ---- */ + +/* used to insert keyframes from any view */ +void common_insertkey (void) +{ + common_modifykey(COMMONKEY_MODE_INSERT); +} + +/* used to insert keyframes from any view */ +void common_deletekey (void) +{ + common_modifykey(COMMONKEY_MODE_DELETE); +} + +/* ************************************************** */