Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Update to btDbvt and btDbvtBroadphase, thanks to Nathanael Presson

Browse Source
This commit is contained in:
erwin.coumans
2008-07-09 22:45:01 +00:00
parent b9bc96e22b
commit 9996da6f2d
7 changed files with 1558 additions and 681 deletions
Download Patch File Download Diff File Expand all files Collapse all files

780
src/BulletCollision/BroadphaseCollision/btDbvt.cpp
View File

@@ -160,8 +160,7 @@ if(leaf==pdbvt->m_root)
Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
if(NotEqual(pb,prev->volume)) if(NotEqual(pb,prev->volume))
{ {
sibling = prev; prev=prev->parent;
prev = prev->parent;
} else break; } else break;
} }
return(prev?prev:pdbvt->m_root); return(prev?prev:pdbvt->m_root);
@@ -177,25 +176,25 @@ if(leaf==pdbvt->m_root)
} }
// //
static void fetchleafs( btDbvt* pdbvt, static void fetchleaves(btDbvt* pdbvt,
btDbvt::Node* root, btDbvt::Node* root,
tNodeArray& leafs, tNodeArray& leaves,
int depth=-1) int depth=-1)
{ {
if(root->isinternal()&&depth) if(root->isinternal()&&depth)
{ {
fetchleafs(pdbvt,root->childs[0],leafs,depth-1); fetchleaves(pdbvt,root->childs[0],leaves,depth-1);
fetchleafs(pdbvt,root->childs[1],leafs,depth-1); fetchleaves(pdbvt,root->childs[1],leaves,depth-1);
deletenode(pdbvt,root); deletenode(pdbvt,root);
} }
else else
{ {
leafs.push_back(root); leaves.push_back(root);
} }
} }
// //
static void split( const tNodeArray& leafs, static void split( const tNodeArray& leaves,
tNodeArray& left, tNodeArray& left,
tNodeArray& right, tNodeArray& right,
const btVector3& org, const btVector3& org,
@@ -203,39 +202,39 @@ static void split( const tNodeArray& leafs,
{ {
left.resize(0); left.resize(0);
right.resize(0); right.resize(0);
for(int i=0,ni=leafs.size();i<ni;++i) for(int i=0,ni=leaves.size();i<ni;++i)
{ {
if(dot(axis,leafs[i]->volume.Center()-org)<0) if(dot(axis,leaves[i]->volume.Center()-org)<0)
left.push_back(leafs[i]); left.push_back(leaves[i]);
else else
right.push_back(leafs[i]); right.push_back(leaves[i]);
} }
} }
// //
static btDbvt::Volume bounds( const tNodeArray& leafs) static btDbvt::Volume bounds( const tNodeArray& leaves)
{ {
btDbvt::Volume volume=leafs[0]->volume; btDbvt::Volume volume=leaves[0]->volume;
for(int i=1,ni=leafs.size();i<ni;++i) for(int i=1,ni=leaves.size();i<ni;++i)
{ {
volume=merge(volume,leafs[i]->volume); volume=merge(volume,leaves[i]->volume);
} }
return(volume); return(volume);
} }
// //
static void bottomup( btDbvt* pdbvt, static void bottomup( btDbvt* pdbvt,
tNodeArray& leafs) tNodeArray& leaves)
{ {
while(leafs.size()>1) while(leaves.size()>1)
{ {
btScalar minsize=SIMD_INFINITY; btScalar minsize=SIMD_INFINITY;
int minidx[2]={-1,-1}; int minidx[2]={-1,-1};
for(int i=0;i<leafs.size();++i) for(int i=0;i<leaves.size();++i)
{ {
for(int j=i+1;j<leafs.size();++j) for(int j=i+1;j<leaves.size();++j)
{ {
const btScalar sz=size(merge(leafs[i]->volume,leafs[j]->volume)); const btScalar sz=size(merge(leaves[i]->volume,leaves[j]->volume));
if(sz<minsize) if(sz<minsize)
{ {
minsize = sz; minsize = sz;
@@ -244,39 +243,39 @@ while(leafs.size()>1)
} }
} }
} }
btDbvt::Node* n[] = {leafs[minidx[0]],leafs[minidx[1]]}; btDbvt::Node* n[] = {leaves[minidx[0]],leaves[minidx[1]]};
btDbvt::Node* p = createnode(pdbvt,0,merge(n[0]->volume,n[1]->volume),0); btDbvt::Node* p = createnode(pdbvt,0,merge(n[0]->volume,n[1]->volume),0);
p->childs[0] = n[0]; p->childs[0] = n[0];
p->childs[1] = n[1]; p->childs[1] = n[1];
n[0]->parent = p; n[0]->parent = p;
n[1]->parent = p; n[1]->parent = p;
leafs[minidx[0]] = p; leaves[minidx[0]] = p;
leafs.swap(minidx[1],leafs.size()-1); leaves.swap(minidx[1],leaves.size()-1);
leafs.pop_back(); leaves.pop_back();
} }
} }
// //
static btDbvt::Node* topdown(btDbvt* pdbvt, static btDbvt::Node* topdown(btDbvt* pdbvt,
tNodeArray& leafs, tNodeArray& leaves,
int bu_treshold) int bu_treshold)
{ {
static const btVector3 axis[]={btVector3(1,0,0), static const btVector3 axis[]={btVector3(1,0,0),
btVector3(0,1,0), btVector3(0,1,0),
btVector3(0,0,1)}; btVector3(0,0,1)};
if(leafs.size()>1) if(leaves.size()>1)
{ {
if(leafs.size()>bu_treshold) if(leaves.size()>bu_treshold)
{ {
const btDbvt::Volume vol=bounds(leafs); const btDbvt::Volume vol=bounds(leaves);
const btVector3 org=vol.Center(); const btVector3 org=vol.Center();
tNodeArray sets[2]; tNodeArray sets[2];
int bestaxis=-1; int bestaxis=-1;
int bestmidp=leafs.size(); int bestmidp=leaves.size();
int splitcount[3][2]={{0,0},{0,0},{0,0}}; int splitcount[3][2]={{0,0},{0,0},{0,0}};
for(int i=0;i<leafs.size();++i) for(int i=0;i<leaves.size();++i)
{ {
const btVector3 x=leafs[i]->volume.Center()-org; const btVector3 x=leaves[i]->volume.Center()-org;
for(int j=0;j<3;++j) for(int j=0;j<3;++j)
{ {
++splitcount[j][dot(x,axis[j])>0?1:0]; ++splitcount[j][dot(x,axis[j])>0?1:0];
@@ -298,15 +297,15 @@ if(leafs.size()>1)
{ {
sets[0].reserve(splitcount[bestaxis][0]); sets[0].reserve(splitcount[bestaxis][0]);
sets[1].reserve(splitcount[bestaxis][1]); sets[1].reserve(splitcount[bestaxis][1]);
split(leafs,sets[0],sets[1],org,axis[bestaxis]); split(leaves,sets[0],sets[1],org,axis[bestaxis]);
} }
else else
{ {
sets[0].reserve(leafs.size()/2+1); sets[0].reserve(leaves.size()/2+1);
sets[1].reserve(leafs.size()/2); sets[1].reserve(leaves.size()/2);
for(int i=0,ni=leafs.size();i<ni;++i) for(int i=0,ni=leaves.size();i<ni;++i)
{ {
sets[i&1].push_back(leafs[i]); sets[i&1].push_back(leaves[i]);
} }
} }
btDbvt::Node* node=createnode(pdbvt,0,vol,0); btDbvt::Node* node=createnode(pdbvt,0,vol,0);
@@ -318,33 +317,46 @@ if(leafs.size()>1)
} }
else else
{ {
bottomup(pdbvt,leafs); bottomup(pdbvt,leaves);
return(leafs[0]); return(leaves[0]);
} }
} }
return(leafs[0]); return(leaves[0]);
} }
// //
static inline btDbvt::Node* refit( btDbvt* pdbvt, static inline btDbvt::Node* sort(btDbvt::Node* n,btDbvt::Node*& r)
btDbvt::Node* node)
{ {
btDbvt::Node* parent=node->parent; btDbvt::Node* p=n->parent;
if(parent) btAssert(n->isinternal());
if(p>n)
{ {
const int idx=indexof(node); const int i=indexof(n);
tNodeArray leafs; const int j=1-i;
leafs.reserve(64); btDbvt::Node* s=p->childs[j];
fetchleafs(pdbvt,node,leafs,3); btDbvt::Node* q=p->parent;
if(leafs.size()>=2) btAssert(n==p->childs[i]);
{ if(q) q->childs[indexof(p)]=n; else r=n;
bottomup(pdbvt,leafs); s->parent=n;
node=leafs[0]; p->parent=n;
node->parent=parent; n->parent=q;
parent->childs[idx]=node; 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);
return(node); }
//
static inline btDbvt::Node* walkup(btDbvt::Node* n,int count)
{
while(n&&(count--)) n=n->parent;
return(n);
} }
// //
@@ -357,7 +369,7 @@ return(node);
m_root = 0; m_root = 0;
m_free = 0; m_free = 0;
m_lkhd = -1; m_lkhd = -1;
m_leafs = 0; m_leaves = 0;
m_opath = 0; m_opath = 0;
} }
@@ -380,11 +392,11 @@ void btDbvt::optimizeBottomUp()
{ {
if(m_root) if(m_root)
{ {
tNodeArray leafs; tNodeArray leaves;
leafs.reserve(m_leafs); leaves.reserve(m_leaves);
fetchleafs(this,m_root,leafs); fetchleaves(this,m_root,leaves);
bottomup(this,leafs); bottomup(this,leaves);
m_root=leafs[0]; m_root=leaves[0];
} }
} }
@@ -393,16 +405,17 @@ void btDbvt::optimizeTopDown(int bu_treshold)
{ {
if(m_root) if(m_root)
{ {
tNodeArray leafs; tNodeArray leaves;
leafs.reserve(m_leafs); leaves.reserve(m_leaves);
fetchleafs(this,m_root,leafs); fetchleaves(this,m_root,leaves);
m_root=topdown(this,leafs,bu_treshold); m_root=topdown(this,leaves,bu_treshold);
} }
} }
// //
void btDbvt::optimizeIncremental(int passes) void btDbvt::optimizeIncremental(int passes)
{ {
if(passes<0) passes=m_leaves;
if(m_root&&(passes>0)) if(m_root&&(passes>0))
{ {
do { do {
@@ -410,7 +423,7 @@ if(m_root&&(passes>0))
unsigned bit=0; unsigned bit=0;
while(node->isinternal()) while(node->isinternal())
{ {
node=node->childs[(m_opath>>bit)&1]; node=sort(node,m_root)->childs[(m_opath>>bit)&1];
bit=(bit+1)&(sizeof(unsigned)*8-1); bit=(bit+1)&(sizeof(unsigned)*8-1);
} }
update(node); update(node);
@@ -424,7 +437,7 @@ btDbvt::Node* btDbvt::insert(const Volume& volume,void* data)
{ {
Node* leaf=createnode(this,0,volume,data); Node* leaf=createnode(this,0,volume,data);
insertleaf(this,m_root,leaf); insertleaf(this,m_root,leaf);
++m_leafs; ++m_leaves;
return(leaf); return(leaf);
} }
@@ -496,14 +509,14 @@ void btDbvt::remove(Node* leaf)
{ {
removeleaf(this,leaf); removeleaf(this,leaf);
deletenode(this,leaf); deletenode(this,leaf);
--m_leafs; --m_leaves;
} }
// //
void btDbvt::write(IWriter* iwriter) const void btDbvt::write(IWriter* iwriter) const
{ {
btDbvtNodeEnumerator nodes; btDbvtNodeEnumerator nodes;
nodes.nodes.reserve(m_leafs*2); nodes.nodes.reserve(m_leaves*2);
enumNodes(m_root,nodes); enumNodes(m_root,nodes);
iwriter->Prepare(m_root,nodes.nodes.size()); iwriter->Prepare(m_root,nodes.nodes.size());
for(int i=0;i<nodes.nodes.size();++i) for(int i=0;i<nodes.nodes.size();++i)
@@ -523,3 +536,630 @@ for(int i=0;i<nodes.nodes.size();++i)
} }
} }
} }
//
void btDbvt::clone(btDbvt& dest,IClone* iclone) const
{
dest.clear();
if(m_root!=0)
{
btAlignedObjectArray<sStkCLN> stack;
stack.reserve(m_leaves);
stack.push_back(sStkCLN(m_root,0));
do {
const int i=stack.size()-1;
const sStkCLN e=stack[i];
Node* 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::countLeaves(const Node* node)
{
if(node->isinternal())
return(countLeaves(node->childs[0])+countLeaves(node->childs[1]));
else
return(1);
}
//
void btDbvt::extractLeaves(const Node* node,btAlignedObjectArray<const Node*>& leaves)
{
if(node->isinternal())
{
extractLeaves(node->childs[0],leaves);
extractLeaves(node->childs[1],leaves);
}
else
{
leaves.push_back(node);
}
}
//
#if DBVT_ENABLE_BENCHMARK
#include <stdio.h>
#include <stdlib.h>
#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
[1] btDbvt::Volume intersections: 3986 ms (0%)
[2] btDbvt::Volume merges: 5815 ms (-1%)
[3] btDbvt::collideTT: 3267 ms (0%)
[4] btDbvt::collideTT self: 1657 ms (0%)
[5] btDbvt::collideTT xform: 7201 ms (0%)
[6] btDbvt::collideTT xform,self: 7382 ms (0%)
[7] btDbvt::collideRAY: 8855 ms (-1%),(236832 r/s)
[8] insert/remove: 3574 ms (0%),(586780 ir/s)
[9] updates (teleport): 3281 ms (0%),(639180 u/s)
[10] updates (jitter): 2658 ms (0%),(788996 u/s)
[11] optimize (incremental): 5091 ms (0%),(823000 o/s)
[12] btDbvt::Volume notequal: 4151 ms (0%)
[13] culling(OCL): 2486 ms (0%),(411 t/s)
[14] culling(OCL+qsort): 599 ms (-2%),(1709 t/s)
[15] culling(KDOP+qsort): 306 ms (0%),(3346 t/s)
*/
struct btDbvtBenchmark
{
struct NilPolicy : btDbvt::ICollide
{
NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {}
void Process(const btDbvt::Node*,const btDbvt::Node*) { ++m_pcount; }
void Process(const btDbvt::Node*) { ++m_pcount; }
void Process(const btDbvt::Node*,btScalar depth)
{
++m_pcount;
if(m_checksort)
{ if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f\r\n",depth); }
}
int m_pcount;
btScalar m_depth;
bool m_checksort;
};
struct P14 : btDbvt::ICollide
{
struct Node
{
const btDbvt::Node* leaf;
btScalar depth;
};
void Process(const btDbvt::Node* leaf,btScalar depth)
{
Node n;
n.leaf = leaf;
n.depth = depth;
}
static int sortfnc(const Node& a,const Node& b)
{
if(a.depth<b.depth) return(+1);
if(a.depth>b.depth) return(-1);
return(0);
}
btAlignedObjectArray<Node> m_nodes;
};
struct P15 : btDbvt::ICollide
{
struct Node
{
const btDbvt::Node* leaf;
btScalar depth;
};
void Process(const btDbvt::Node* leaf)
{
Node n;
n.leaf = leaf;
n.depth = dot(leaf->volume.Center(),m_axis);
}
static int sortfnc(const Node& a,const Node& b)
{
if(a.depth<b.depth) return(+1);
if(a.depth>b.depth) return(-1);
return(0);
}
btAlignedObjectArray<Node> 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 btDbvt::Volume RandVolume(btScalar cs,btScalar eb,btScalar es)
{
return(btDbvt::Volume::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<leaves;++i)
{
dbvt.insert(RandVolume(cs,eb,es),0);
}
}
};
void btDbvt::benchmark()
{
static const btScalar cfgVolumeCenterScale = 100;
static const btScalar cfgVolumeExentsBase = 1;
static const btScalar cfgVolumeExentsScale = 4;
static const int cfgLeaves = 8192;
static const bool cfgEnable = true;
//[1] btDbvt::Volume intersections
bool cfgBenchmark1_Enable = cfgEnable;
static const int cfgBenchmark1_Iterations = 8;
static const int cfgBenchmark1_Reference = 3980;
//[2] btDbvt::Volume merges
bool cfgBenchmark2_Enable = cfgEnable;
static const int cfgBenchmark2_Iterations = 4;
static const int cfgBenchmark2_Reference = 5924;
//[3] btDbvt::collideTT
bool cfgBenchmark3_Enable = cfgEnable;
static const int cfgBenchmark3_Iterations = 256;
static const int cfgBenchmark3_Reference = 3288;
//[4] btDbvt::collideTT self
bool cfgBenchmark4_Enable = cfgEnable;
static const int cfgBenchmark4_Iterations = 256;
static const int cfgBenchmark4_Reference = 1655;
//[5] btDbvt::collideTT xform
bool cfgBenchmark5_Enable = cfgEnable;
static const int cfgBenchmark5_Iterations = 256;
static const btScalar cfgBenchmark5_OffsetScale = 2;
static const int cfgBenchmark5_Reference = 7201;
//[6] btDbvt::collideTT xform,self
bool cfgBenchmark6_Enable = cfgEnable;
static const int cfgBenchmark6_Iterations = 256;
static const btScalar cfgBenchmark6_OffsetScale = 2;
static const int cfgBenchmark6_Reference = 7382;
//[7] btDbvt::collideRAY
bool cfgBenchmark7_Enable = cfgEnable;
static const int cfgBenchmark7_Passes = 32;
static const int cfgBenchmark7_Iterations = 65536;
static const int cfgBenchmark7_Reference = 8954;
//[8] insert/remove
bool cfgBenchmark8_Enable = cfgEnable;
static const int cfgBenchmark8_Passes = 32;
static const int cfgBenchmark8_Iterations = 65536;
static const int cfgBenchmark8_Reference = 3597;
//[9] updates (teleport)
bool cfgBenchmark9_Enable = cfgEnable;
static const int cfgBenchmark9_Passes = 32;
static const int cfgBenchmark9_Iterations = 65536;
static const int cfgBenchmark9_Reference = 3282;
//[10] updates (jitter)
bool cfgBenchmark10_Enable = cfgEnable;
static const btScalar cfgBenchmark10_Scale = cfgVolumeCenterScale/10000;
static const int cfgBenchmark10_Passes = 32;
static const int cfgBenchmark10_Iterations = 65536;
static const int cfgBenchmark10_Reference = 2659;
//[11] optimize (incremental)
bool cfgBenchmark11_Enable = cfgEnable;
static const int cfgBenchmark11_Passes = 64;
static const int cfgBenchmark11_Iterations = 65536;
static const int cfgBenchmark11_Reference = 5075;
//[12] btDbvt::Volume notequal
bool cfgBenchmark12_Enable = cfgEnable;
static const int cfgBenchmark12_Iterations = 32;
static const int cfgBenchmark12_Reference = 4118;
//[13] culling(OCL+fullsort)
bool cfgBenchmark13_Enable = cfgEnable;
static const int cfgBenchmark13_Iterations = 1024;
static const int cfgBenchmark13_Reference = 2483;
//[14] culling(OCL+qsort)
bool cfgBenchmark14_Enable = cfgEnable;
static const int cfgBenchmark14_Iterations = 1024;
static const int cfgBenchmark14_Reference = 614;
//[15] culling(KDOP+qsort)
bool cfgBenchmark15_Enable = cfgEnable;
static const int cfgBenchmark15_Iterations = 1024;
static const int cfgBenchmark15_Reference = 305;
btClock wallclock;
printf("Benchmarking dbvt...\r\n");
printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale);
printf("\tExtents base: %f\r\n",cfgVolumeExentsBase);
printf("\tExtents range: %f\r\n",cfgVolumeExentsScale);
printf("\tLeaves: %u\r\n",cfgLeaves);
if(cfgBenchmark1_Enable)
{// Benchmark 1
srand(380843);
btAlignedObjectArray<btDbvt::Volume> volumes;
btAlignedObjectArray<bool> results;
volumes.resize(cfgLeaves);
results.resize(cfgLeaves);
for(int i=0;i<cfgLeaves;++i)
{
volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
}
printf("[1] btDbvt::Volume intersections: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark1_Iterations;++i)
{
for(int j=0;j<cfgLeaves;++j)
{
for(int k=0;k<cfgLeaves;++k)
{
results[k]=Intersect(volumes[j],volumes[k]);
}
}
}
const int time=(int)wallclock.getTimeMilliseconds();
printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark1_Reference)*100/time);
}
if(cfgBenchmark2_Enable)
{// Benchmark 2
srand(380843);
btAlignedObjectArray<btDbvt::Volume> volumes;
btAlignedObjectArray<btDbvt::Volume> results;
volumes.resize(cfgLeaves);
results.resize(cfgLeaves);
for(int i=0;i<cfgLeaves;++i)
{
volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
}
printf("[2] btDbvt::Volume merges: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark2_Iterations;++i)
{
for(int j=0;j<cfgLeaves;++j)
{
for(int k=0;k<cfgLeaves;++k)
{
Merge(volumes[j],volumes[k],results[k]);
}
}
}
const int time=(int)wallclock.getTimeMilliseconds();
printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark2_Reference)*100/time);
}
if(cfgBenchmark3_Enable)
{// Benchmark 3
srand(380843);
btDbvt dbvt[2];
btDbvtBenchmark::NilPolicy policy;
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
dbvt[0].optimizeTopDown();
dbvt[1].optimizeTopDown();
printf("[3] btDbvt::collideTT: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark3_Iterations;++i)
{
btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy);
}
const int time=(int)wallclock.getTimeMilliseconds();
printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark3_Reference)*100/time);
}
if(cfgBenchmark4_Enable)
{// Benchmark 4
srand(380843);
btDbvt dbvt;
btDbvtBenchmark::NilPolicy policy;
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
printf("[4] btDbvt::collideTT self: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark4_Iterations;++i)
{
btDbvt::collideTT(dbvt.m_root,dbvt.m_root,policy);
}
const int time=(int)wallclock.getTimeMilliseconds();
printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark4_Reference)*100/time);
}
if(cfgBenchmark5_Enable)
{// Benchmark 5
srand(380843);
btDbvt dbvt[2];
btAlignedObjectArray<btTransform> transforms;
btDbvtBenchmark::NilPolicy policy;
transforms.resize(cfgBenchmark5_Iterations);
for(int i=0;i<transforms.size();++i)
{
transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale);
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
dbvt[0].optimizeTopDown();
dbvt[1].optimizeTopDown();
printf("[5] btDbvt::collideTT xform: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark5_Iterations;++i)
{
btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy);
}
const int time=(int)wallclock.getTimeMilliseconds();
printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark5_Reference)*100/time);
}
if(cfgBenchmark6_Enable)
{// Benchmark 6
srand(380843);
btDbvt dbvt;
btAlignedObjectArray<btTransform> transforms;
btDbvtBenchmark::NilPolicy policy;
transforms.resize(cfgBenchmark6_Iterations);
for(int i=0;i<transforms.size();++i)
{
transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale);
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
printf("[6] btDbvt::collideTT xform,self: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark6_Iterations;++i)
{
btDbvt::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy);
}
const int time=(int)wallclock.getTimeMilliseconds();
printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark6_Reference)*100/time);
}
if(cfgBenchmark7_Enable)
{// Benchmark 7
srand(380843);
btDbvt dbvt;
btAlignedObjectArray<btVector3> rayorg;
btAlignedObjectArray<btVector3> raydir;
btDbvtBenchmark::NilPolicy policy;
rayorg.resize(cfgBenchmark7_Iterations);
raydir.resize(cfgBenchmark7_Iterations);
for(int i=0;i<rayorg.size();++i)
{
rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
printf("[7] btDbvt::collideRAY: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark7_Passes;++i)
{
for(int j=0;j<cfgBenchmark7_Iterations;++j)
{
btDbvt::collideRAY(dbvt.m_root,rayorg[j],raydir[j],policy);
}
}
const int time=(int)wallclock.getTimeMilliseconds();
unsigned rays=cfgBenchmark7_Passes*cfgBenchmark7_Iterations;
printf("%u ms (%i%%),(%u r/s)\r\n",time,(time-cfgBenchmark7_Reference)*100/time,(rays*1000)/time);
}
if(cfgBenchmark8_Enable)
{// Benchmark 8
srand(380843);
btDbvt dbvt;
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
printf("[8] insert/remove: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark8_Passes;++i)
{
for(int j=0;j<cfgBenchmark8_Iterations;++j)
{
dbvt.remove(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
}
}
const int time=(int)wallclock.getTimeMilliseconds();
const int ir=cfgBenchmark8_Passes*cfgBenchmark8_Iterations;
printf("%u ms (%i%%),(%u ir/s)\r\n",time,(time-cfgBenchmark8_Reference)*100/time,ir*1000/time);
}
if(cfgBenchmark9_Enable)
{// Benchmark 9
srand(380843);
btDbvt dbvt;
btAlignedObjectArray<const btDbvt::Node*> 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<cfgBenchmark9_Passes;++i)
{
for(int j=0;j<cfgBenchmark9_Iterations;++j)
{
dbvt.update(const_cast<btDbvt::Node*>(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<const btDbvt::Node*> leaves;
btAlignedObjectArray<btVector3> vectors;
vectors.resize(cfgBenchmark10_Iterations);
for(int i=0;i<vectors.size();++i)
{
vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1))*cfgBenchmark10_Scale;
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
dbvt.extractLeaves(dbvt.m_root,leaves);
printf("[10] updates (jitter): ");
wallclock.reset();
for(int i=0;i<cfgBenchmark10_Passes;++i)
{
for(int j=0;j<cfgBenchmark10_Iterations;++j)
{
const btVector3& d=vectors[j];
btDbvt::Node* l=const_cast<btDbvt::Node*>(leaves[rand()%cfgLeaves]);
btDbvt::Volume v=btDbvt::Volume::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<cfgBenchmark11_Passes;++i)
{
dbvt.optimizeIncremental(cfgBenchmark11_Iterations);
}
const int time=(int)wallclock.getTimeMilliseconds();
const int op=cfgBenchmark11_Passes*cfgBenchmark11_Iterations;
printf("%u ms (%i%%),(%u o/s)\r\n",time,(time-cfgBenchmark11_Reference)*100/time,op/time*1000);
}
if(cfgBenchmark12_Enable)
{// Benchmark 12
srand(380843);
btAlignedObjectArray<btDbvt::Volume> volumes;
btAlignedObjectArray<bool> results;
volumes.resize(cfgLeaves);
results.resize(cfgLeaves);
for(int i=0;i<cfgLeaves;++i)
{
volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
}
printf("[12] btDbvt::Volume notequal: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark12_Iterations;++i)
{
for(int j=0;j<cfgLeaves;++j)
{
for(int k=0;k<cfgLeaves;++k)
{
results[k]=NotEqual(volumes[j],volumes[k]);
}
}
}
const int time=(int)wallclock.getTimeMilliseconds();
printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark12_Reference)*100/time);
}
if(cfgBenchmark13_Enable)
{// Benchmark 13
srand(380843);
btDbvt dbvt;
btAlignedObjectArray<btVector3> vectors;
btDbvtBenchmark::NilPolicy policy;
vectors.resize(cfgBenchmark13_Iterations);
for(int i=0;i<vectors.size();++i)
{
vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
printf("[13] culling(OCL+fullsort): ");
wallclock.reset();
for(int i=0;i<cfgBenchmark13_Iterations;++i)
{
static const btScalar offset=0;
policy.m_depth=-SIMD_INFINITY;
dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy);
}
const int time=(int)wallclock.getTimeMilliseconds();
const int t=cfgBenchmark13_Iterations;
printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark13_Reference)*100/time,(t*1000)/time);
}
if(cfgBenchmark14_Enable)
{// Benchmark 14
srand(380843);
btDbvt dbvt;
btAlignedObjectArray<btVector3> vectors;
btDbvtBenchmark::P14 policy;
vectors.resize(cfgBenchmark14_Iterations);
for(int i=0;i<vectors.size();++i)
{
vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
policy.m_nodes.reserve(cfgLeaves);
printf("[14] culling(OCL+qsort): ");
wallclock.reset();
for(int i=0;i<cfgBenchmark14_Iterations;++i)
{
static const btScalar offset=0;
policy.m_nodes.resize(0);
dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false);
policy.m_nodes.quickSort(btDbvtBenchmark::P14::sortfnc);
}
const int time=(int)wallclock.getTimeMilliseconds();
const int t=cfgBenchmark14_Iterations;
printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark14_Reference)*100/time,(t*1000)/time);
}
if(cfgBenchmark15_Enable)
{// Benchmark 15
srand(380843);
btDbvt dbvt;
btAlignedObjectArray<btVector3> vectors;
btDbvtBenchmark::P15 policy;
vectors.resize(cfgBenchmark15_Iterations);
for(int i=0;i<vectors.size();++i)
{
vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
policy.m_nodes.reserve(cfgLeaves);
printf("[15] culling(KDOP+qsort): ");
wallclock.reset();
for(int i=0;i<cfgBenchmark15_Iterations;++i)
{
static const btScalar offset=0;
policy.m_nodes.resize(0);
policy.m_axis=vectors[i];
dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy);
policy.m_nodes.quickSort(btDbvtBenchmark::P15::sortfnc);
}
const int time=(int)wallclock.getTimeMilliseconds();
const int t=cfgBenchmark15_Iterations;
printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark15_Reference)*100/time,(t*1000)/time);
}
printf("\r\n\r\n");
}
#endif

293
src/BulletCollision/BroadphaseCollision/btDbvt.h
View File

@@ -19,29 +19,56 @@ subject to the following restrictions:
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/btTransform.h"
// //
// Compile time configuration // Compile time configuration
// //
#ifdef WIN32 #ifdef WIN32
//#define DBVT_USE_TEMPLATE #define DBVT_USE_TEMPLATE 1 // Enable template for ICollide
#else
#define DBVT_USE_TEMPLATE 0 // Enable template for ICollide
#endif #endif
#ifdef DBVT_USE_TEMPLATE #define DBVT_USE_MEMMOVE 1 // Enable memmove (collideOCL)
#define DBVT_ENABLE_BENCHMARK 0 // Enable benchmarking code
//
// Auto config and checks
//
#if DBVT_USE_TEMPLATE
#define DBVT_VIRTUAL #define DBVT_VIRTUAL
#define DBVT_VIRTUAL_DESTRUCTOR(a) #define DBVT_VIRTUAL_DTOR(a)
#define DBVT_PREFIX template <typename T> #define DBVT_PREFIX template <typename T>
#define DBVT_IPOLICY T& policy #define DBVT_IPOLICY T& policy
#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)0; #define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)0;
#else #else
#define DBVT_VIRTUAL_DESTRUCTOR(a) virtual ~a() {} #define DBVT_VIRTUAL_DTOR(a) virtual ~a() {}
#define DBVT_VIRTUAL virtual #define DBVT_VIRTUAL virtual
#define DBVT_PREFIX #define DBVT_PREFIX
#define DBVT_IPOLICY ICollide& policy #define DBVT_IPOLICY ICollide& policy
#define DBVT_CHECKTYPE #define DBVT_CHECKTYPE
#endif #endif
#if DBVT_USE_MEMMOVE
#include <memory.h>
#include <string.h>
#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
// //
// Defaults volumes // Defaults volumes
// //
@@ -50,10 +77,10 @@ subject to the following restrictions:
struct btDbvtAabbMm struct btDbvtAabbMm
{ {
inline btVector3 Center() const { return((mi+mx)/2); } inline btVector3 Center() const { return((mi+mx)/2); }
inline btVector3 Extent() const { return((mx-mi)/2); } inline btVector3 Lengths() const { return(mx-mi); }
inline btVector3 Extents() const { return((mx-mi)/2); }
inline const btVector3& Mins() const { return(mi); } inline const btVector3& Mins() const { return(mi); }
inline const btVector3& Maxs() const { return(mx); } inline const btVector3& Maxs() const { return(mx); }
inline btVector3 Lengths() const { return(mx-mi); }
static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e);
static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r);
static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx);
@@ -66,6 +93,9 @@ inline int Classify(const btVector3& n,btScalar o,int s) const;
inline btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; inline btScalar ProjectMinimum(const btVector3& v,unsigned signs) const;
inline friend bool Intersect( const btDbvtAabbMm& a, inline friend bool Intersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b); const btDbvtAabbMm& b);
inline friend bool Intersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b,
const btTransform& xform);
inline friend bool Intersect( const btDbvtAabbMm& a, inline friend bool Intersect( const btDbvtAabbMm& a,
const btVector3& b); const btVector3& b);
inline friend bool Intersect( const btDbvtAabbMm& a, inline friend bool Intersect( const btDbvtAabbMm& a,
@@ -80,6 +110,8 @@ inline friend void Merge( const btDbvtAabbMm& a,
inline friend bool NotEqual( const btDbvtAabbMm& a, inline friend bool NotEqual( const btDbvtAabbMm& a,
const btDbvtAabbMm& b); const btDbvtAabbMm& b);
private: private:
inline void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
private:
btVector3 mi,mx; btVector3 mi,mx;
}; };
@@ -120,18 +152,26 @@ struct btDbvt
const Node* node; const Node* node;
int mask; int mask;
btScalar value; btScalar value;
sStkNPS() {}
sStkNPS(const Node* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} sStkNPS(const Node* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
}; };
struct sStkCLN
{
const Node* node;
Node* parent;
sStkCLN(const Node* n,Node* p) : node(n),parent(p) {}
};
// Policies/Interfaces // Policies/Interfaces
/* ICollide */ /* ICollide */
struct ICollide struct ICollide
{ {
DBVT_VIRTUAL_DESTRUCTOR(ICollide) DBVT_VIRTUAL_DTOR(ICollide)
DBVT_VIRTUAL void Process(const Node*,const Node*) {} DBVT_VIRTUAL void Process(const Node*,const Node*) {}
DBVT_VIRTUAL void Process(const Node*) {} DBVT_VIRTUAL void Process(const Node*) {}
DBVT_VIRTUAL void Process(const Node* n,btScalar) { Process(n); }
DBVT_VIRTUAL bool Descent(const Node*) { return(true); } DBVT_VIRTUAL bool Descent(const Node*) { return(true); }
DBVT_VIRTUAL bool AllLeafs(const Node*) { return(true); } DBVT_VIRTUAL bool AllLeaves(const Node*) { return(true); }
}; };
/* IWriter */ /* IWriter */
struct IWriter struct IWriter
@@ -141,6 +181,12 @@ struct btDbvt
virtual void WriteNode(const Node*,int index,int parent,int child0,int child1)=0; virtual void WriteNode(const Node*,int index,int parent,int child0,int child1)=0;
virtual void WriteLeaf(const Node*,int index,int parent)=0; virtual void WriteLeaf(const Node*,int index,int parent)=0;
}; };
/* IClone */
struct IClone
{
virtual ~IClone() {}
virtual void CloneLeaf(Node*) {}
};
// Constants // Constants
enum { enum {
@@ -152,7 +198,7 @@ struct btDbvt
Node* m_root; Node* m_root;
Node* m_free; Node* m_free;
int m_lkhd; int m_lkhd;
int m_leafs; int m_leaves;
unsigned m_opath; unsigned m_opath;
// Methods // Methods
btDbvt(); btDbvt();
@@ -170,18 +216,37 @@ struct btDbvt
bool update(Node* leaf,Volume volume,btScalar margin); bool update(Node* leaf,Volume volume,btScalar margin);
void remove(Node* leaf); void remove(Node* leaf);
void write(IWriter* iwriter) const; void write(IWriter* iwriter) const;
void clone(btDbvt& dest,IClone* iclone=0) const;
static int countLeaves(const Node* node);
static void extractLeaves(const Node* node,btAlignedObjectArray<const Node*>& leaves);
#if DBVT_ENABLE_BENCHMARK
static void benchmark();
#else
static void benchmark(){}
#endif
// DBVT_IPOLICY must support ICollide policy/interface // DBVT_IPOLICY must support ICollide policy/interface
DBVT_PREFIX DBVT_PREFIX
static void enumNodes( const Node* root, static void enumNodes( const Node* root,
DBVT_IPOLICY); DBVT_IPOLICY);
DBVT_PREFIX DBVT_PREFIX
static void enumLeafs( const Node* root, static void enumLeaves( const Node* root,
DBVT_IPOLICY); DBVT_IPOLICY);
DBVT_PREFIX DBVT_PREFIX
static void collideTT( const Node* root0, static void collideTT( const Node* root0,
const Node* root1, const Node* root1,
DBVT_IPOLICY); DBVT_IPOLICY);
DBVT_PREFIX DBVT_PREFIX
static void collideTT( const Node* root0,
const Node* root1,
const btTransform& xform,
DBVT_IPOLICY);
DBVT_PREFIX
static void collideTT( const Node* root0,
const btTransform& xform0,
const Node* root1,
const btTransform& xform1,
DBVT_IPOLICY);
DBVT_PREFIX
static void collideTV( const Node* root, static void collideTV( const Node* root,
const Volume& volume, const Volume& volume,
DBVT_IPOLICY); DBVT_IPOLICY);
@@ -202,10 +267,33 @@ struct btDbvt
const btScalar* offsets, const btScalar* offsets,
const btVector3& sortaxis, const btVector3& sortaxis,
int count, int count,
DBVT_IPOLICY); DBVT_IPOLICY,
bool fullsort=true);
DBVT_PREFIX DBVT_PREFIX
static void collideTU( const Node* root, static void collideTU( const Node* root,
DBVT_IPOLICY); DBVT_IPOLICY);
// Helpers
static inline int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
{
int m=0;
while(l<h)
{
m=(l+h)>>1;
if(a[i[m]].value>=v) l=m+1; else h=m;
}
return(h);
}
static inline int allocate( btAlignedObjectArray<int>& ifree,
btAlignedObjectArray<btDbvt::sStkNPS>& stock,
const btDbvt::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: private:
btDbvt(const btDbvt&) {} btDbvt(const btDbvt&) {}
@@ -326,30 +414,44 @@ const btVector3 p( b[(signs>>0)&1]->x(),
return(dot(p,v)); return(dot(p,v));
} }
//
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]; }
}
}
// //
inline bool Intersect( const btDbvtAabbMm& a, inline bool Intersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b) const btDbvtAabbMm& b)
{ {
#if 0
const btScalar mi[]={ b.mx.x()-a.mi.x(),
b.mx.y()-a.mi.y(),
b.mx.z()-a.mi.z()};
const unsigned* imi=(const unsigned*)mi;
if((imi[0]|imi[1]|imi[2])&0x80000000) return(false);
const btScalar mx[]={ a.mx.x()-b.mi.x(),
a.mx.y()-b.mi.y(),
a.mx.z()-b.mi.z()};
const unsigned* imx=(const unsigned*)mx;
if((imx[0]|imx[1]|imx[2])&0x80000000) return(false);
return(true);
#else
return( (a.mi.x()<=b.mx.x())&& return( (a.mi.x()<=b.mx.x())&&
(a.mi.y()<=b.mx.y())&&
(a.mi.z()<=b.mx.z())&&
(a.mx.x()>=b.mi.x())&& (a.mx.x()>=b.mi.x())&&
(a.mi.y()<=b.mx.y())&&
(a.mx.y()>=b.mi.y())&& (a.mx.y()>=b.mi.y())&&
(a.mi.z()<=b.mx.z())&&
(a.mx.z()>=b.mi.z())); (a.mx.z()>=b.mi.z()));
#endif }
//
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);
} }
// //
@@ -399,9 +501,11 @@ inline void Merge( const btDbvtAabbMm& a,
const btDbvtAabbMm& b, const btDbvtAabbMm& b,
btDbvtAabbMm& r) btDbvtAabbMm& r)
{ {
r=a; for(int i=0;i<3;++i)
r.mi.setMin(b.mi); {
r.mx.setMax(b.mx); if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
}
} }
// //
@@ -436,14 +540,14 @@ if(root->isinternal())
// //
DBVT_PREFIX DBVT_PREFIX
inline void btDbvt::enumLeafs( const Node* root, inline void btDbvt::enumLeaves( const Node* root,
DBVT_IPOLICY) DBVT_IPOLICY)
{ {
DBVT_CHECKTYPE DBVT_CHECKTYPE
if(root->isinternal()) if(root->isinternal())
{ {
enumLeafs(root->childs[0],policy); enumLeaves(root->childs[0],policy);
enumLeafs(root->childs[1],policy); enumLeaves(root->childs[1],policy);
} }
else else
{ {
@@ -509,6 +613,68 @@ if(root0&&root1)
} }
} }
//
DBVT_PREFIX
inline void btDbvt::collideTT( const Node* root0,
const Node* root1,
const btTransform& xform,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
if(root0&&root1)
{
btAlignedObjectArray<sStkNN> stack;
stack.reserve(DOUBLE_STACKSIZE);
stack.push_back(sStkNN(root0,root1));
do {
sStkNN p=stack[stack.size()-1];
stack.pop_back();
if(Intersect(p.a->volume,p.b->volume,xform))
{
if(p.a->isinternal())
{
if(p.b->isinternal())
{
stack.push_back(sStkNN(p.a->childs[0],p.b->childs[0]));
stack.push_back(sStkNN(p.a->childs[1],p.b->childs[0]));
stack.push_back(sStkNN(p.a->childs[0],p.b->childs[1]));
stack.push_back(sStkNN(p.a->childs[1],p.b->childs[1]));
}
else
{
stack.push_back(sStkNN(p.a->childs[0],p.b));
stack.push_back(sStkNN(p.a->childs[1],p.b));
}
}
else
{
if(p.b->isinternal())
{
stack.push_back(sStkNN(p.a,p.b->childs[0]));
stack.push_back(sStkNN(p.a,p.b->childs[1]));
}
else
{
policy.Process(p.a,p.b);
}
}
}
} while(stack.size()>0);
}
}
//
DBVT_PREFIX
inline void btDbvt::collideTT( const Node* root0,
const btTransform& xform0,
const Node* root1,
const btTransform& xform1,
DBVT_IPOLICY)
{
const btTransform xform=xform0.inverse()*xform1;
collideTT(root0,root1,xform,policy);
}
// //
DBVT_PREFIX DBVT_PREFIX
inline void btDbvt::collideTV( const Node* root, inline void btDbvt::collideTV( const Node* root,
@@ -627,7 +793,7 @@ if(root)
} }
else else
{ {
if(policy.AllLeafs(se.node)) enumLeafs(se.node,policy); if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
} }
} }
} while(stack.size()); } while(stack.size());
@@ -641,7 +807,8 @@ inline void btDbvt::collideOCL( const Node* root,
const btScalar* offsets, const btScalar* offsets,
const btVector3& sortaxis, const btVector3& sortaxis,
int count, int count,
DBVT_IPOLICY) DBVT_IPOLICY,
bool fsort)
{ {
DBVT_CHECKTYPE DBVT_CHECKTYPE
if(root) if(root)
@@ -650,7 +817,9 @@ if(root)
(sortaxis[1]>=0?2:0)+ (sortaxis[1]>=0?2:0)+
(sortaxis[2]>=0?4:0); (sortaxis[2]>=0?4:0);
const int inside=(1<<count)-1; const int inside=(1<<count)-1;
btAlignedObjectArray<sStkNPS> stack; btAlignedObjectArray<sStkNPS> stock;
btAlignedObjectArray<int> ifree;
btAlignedObjectArray<int> stack;
int signs[sizeof(unsigned)*8]; int signs[sizeof(unsigned)*8];
btAssert(count<(sizeof(signs)/sizeof(signs[0]))); btAssert(count<(sizeof(signs)/sizeof(signs[0])));
for(int i=0;i<count;++i) for(int i=0;i<count;++i)
@@ -659,11 +828,14 @@ if(root)
((normals[i].y()>=0)?2:0)+ ((normals[i].y()>=0)?2:0)+
((normals[i].z()>=0)?4:0); ((normals[i].z()>=0)?4:0);
} }
stock.reserve(SIMPLE_STACKSIZE);
stack.reserve(SIMPLE_STACKSIZE); stack.reserve(SIMPLE_STACKSIZE);
stack.push_back(sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))); ifree.reserve(SIMPLE_STACKSIZE);
stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
do { do {
sStkNPS se=stack[stack.size()-1]; const int id=stack[stack.size()-1];
stack.pop_back(); sStkNPS se=stock[id];
stack.pop_back();ifree.push_back(id);
if(se.mask!=inside) if(se.mask!=inside)
{ {
bool out=false; bool out=false;
@@ -685,21 +857,41 @@ if(root)
{ {
if(se.node->isinternal()) if(se.node->isinternal())
{ {
for(int i=0;i<2;++i) const Node* pns[]={ se.node->childs[0],se.node->childs[1]};
{ sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
const Node* n=se.node->childs[i]; sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
const int q=nes[0].value<nes[1].value?1:0;
int j=stack.size(); int j=stack.size();
sStkNPS ne(n,se.mask,n->volume.ProjectMinimum(sortaxis,srtsgns)); if(fsort&&(j>0))
stack.push_back(ne);
while((j>0)&&(ne.value>stack[j-1].value))
{ {
btSwap(stack[j],stack[j-1]);--j; /* 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 else
{ {
policy.Process(se.node); policy.Process(se.node,se.value);
} }
} }
} while(stack.size()); } while(stack.size());
@@ -735,10 +927,9 @@ if(root)
// PP Cleanup // PP Cleanup
// //
#ifdef DBVT_USE_TEMPLATE #undef DBVT_USE_MEMMOVE
#undef DBVT_USE_TEMPLATE #undef DBVT_USE_TEMPLATE
#endif #undef DBVT_VIRTUAL_DTOR
#undef DBVT_VIRTUAL_DESTRUCTOR
#undef DBVT_VIRTUAL #undef DBVT_VIRTUAL
#undef DBVT_PREFIX #undef DBVT_PREFIX
#undef DBVT_IPOLICY #undef DBVT_IPOLICY

26
src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
View File

@@ -1,3 +1,19 @@
/*
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" #include "btDbvtBroadphase.h"
// //
@@ -120,7 +136,9 @@ void Process(const btDbvt::Node* na,const btDbvt::Node* nb)
// //
btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
{ {
btDbvt::benchmark();
m_releasepaircache = (paircache!=0)?false:true; m_releasepaircache = (paircache!=0)?false:true;
m_predictedframes = 2;
m_stageCurrent = 0; m_stageCurrent = 0;
m_fupdates = 1; m_fupdates = 1;
m_dupdates = 1; m_dupdates = 1;
@@ -198,9 +216,9 @@ if(proxy->stage==STAGECOUNT)
{/* Moving */ {/* Moving */
const btVector3 delta=(aabbMin+aabbMax)/2-proxy->aabb.Center(); const btVector3 delta=(aabbMin+aabbMax)/2-proxy->aabb.Center();
#ifdef DBVT_BP_MARGIN #ifdef DBVT_BP_MARGIN
m_sets[0].update(proxy->leaf,aabb,delta*PREDICTED_FRAMES,DBVT_BP_MARGIN); m_sets[0].update(proxy->leaf,aabb,delta*m_predictedframes,DBVT_BP_MARGIN);
#else #else
m_sets[0].update(proxy->leaf,aabb,delta*PREDICTED_FRAMES); m_sets[0].update(proxy->leaf,aabb,delta*m_predictedframes);
#endif #endif
} }
else else
@@ -246,8 +264,8 @@ void btDbvtBroadphase::collide(btDispatcher* dispatcher)
{ {
SPC(m_profiling.m_total); SPC(m_profiling.m_total);
/* optimize */ /* optimize */
m_sets[0].optimizeIncremental(1+(m_sets[0].m_leafs*m_dupdates)/100); m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
m_sets[1].optimizeIncremental(1+(m_sets[1].m_leafs*m_fupdates)/100); m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
/* dynamic -> fixed set */ /* dynamic -> fixed set */
m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
btDbvtProxy* current=m_stageRoots[m_stageCurrent]; btDbvtProxy* current=m_stageRoots[m_stageCurrent];

3
src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
View File

@@ -62,12 +62,12 @@ enum {
DYNAMIC_SET = 0, /* Dynamic set index */ DYNAMIC_SET = 0, /* Dynamic set index */
FIXED_SET = 1, /* Fixed set index */ FIXED_SET = 1, /* Fixed set index */
STAGECOUNT = 2, /* Number of stages */ STAGECOUNT = 2, /* Number of stages */
PREDICTED_FRAMES = 2 /* Frames prediction */
}; };
/* Fields */ /* Fields */
btDbvt m_sets[2]; // Dbvt sets btDbvt m_sets[2]; // Dbvt sets
btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
btOverlappingPairCache* m_paircache; // Pair cache btOverlappingPairCache* m_paircache; // Pair cache
btScalar m_predictedframes; // Frames predicted
int m_stageCurrent; // Current stage int m_stageCurrent; // Current stage
int m_fupdates; // % of fixed updates per frame int m_fupdates; // % of fixed updates per frame
int m_dupdates; // % of dynamic updates per frame int m_dupdates; // % of dynamic updates per frame
@@ -100,4 +100,3 @@ void printStats();
}; };
#endif #endif

87
src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
View File

@@ -772,16 +772,17 @@ return(sizeof(GJK)+sizeof(EPA));
} }
// //
btScalar btGjkEpaSolver2::Distance( const btConvexShape* shape0, bool btGjkEpaSolver2::Distance( const btConvexShape* shape0,
const btTransform& wtrs0, const btTransform& wtrs0,
const btConvexShape* shape1, const btConvexShape* shape1,
const btTransform& wtrs1, const btTransform& wtrs1,
const btVector3& guess,
sResults& results) sResults& results)
{ {
tShape shape; tShape shape;
Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
GJK gjk; GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
if(gjk_status==GJK::eStatus::Valid) if(gjk_status==GJK::eStatus::Valid)
{ {
btVector3 w0=btVector3(0,0,0); btVector3 w0=btVector3(0,0,0);
@@ -794,17 +795,62 @@ if(gjk_status==GJK::eStatus::Valid)
} }
results.witnesses[0] = wtrs0*w0; results.witnesses[0] = wtrs0*w0;
results.witnesses[1] = wtrs0*w1; results.witnesses[1] = wtrs0*w1;
return((w0-w1).length()); results.normal = w0-w1;
results.distance = results.normal.length();
results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
return(true);
} }
else else
{ {
results.status = gjk_status==GJK::eStatus::Inside? results.status = gjk_status==GJK::eStatus::Inside?
sResults::Penetrating : sResults::Penetrating :
sResults::GJK_Failed ; sResults::GJK_Failed ;
return(-1); 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;i<epa.m_result.rank;++i)
{
w0+=shape.Support(epa.m_result.c[i]->d,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 btGjkEpaSolver2::SignedDistance(const btVector3& position,
btScalar margin, btScalar margin,
@@ -857,42 +903,17 @@ return(SIMD_INFINITY);
} }
// //
bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
const btTransform& wtrs0, const btTransform& wtrs0,
const btConvexShape* shape1, const btConvexShape* shape1,
const btTransform& wtrs1, const btTransform& wtrs1,
const btVector3& guess, const btVector3& guess,
sResults& results) sResults& results)
{ {
tShape shape; if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,true); return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
GJK gjk; else
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;i<epa.m_result.rank;++i)
{
w0+=shape.Support(epa.m_result.c[i]->d,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);
return(true); return(true);
} else results.status=sResults::EPA_Failed;
}
break;
case GJK::eStatus::Failed:
results.status=sResults::GJK_Failed;
break;
}
return(false);
} }
/* Symbols cleanup */ /* Symbols cleanup */

12
src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
View File

@@ -40,21 +40,29 @@ struct sResults
} status; } status;
btVector3 witnesses[2]; btVector3 witnesses[2];
btVector3 normal; btVector3 normal;
btScalar distance;
}; };
static int StackSizeRequirement(); static int StackSizeRequirement();
static btScalar Distance( const btConvexShape* shape0,const btTransform& wtrs0, static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0,
const btConvexShape* shape1,const btTransform& wtrs1, const btConvexShape* shape1,const btTransform& wtrs1,
const btVector3& guess,
sResults& results); 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, static btScalar SignedDistance( const btVector3& position,
btScalar margin, btScalar margin,
const btConvexShape* shape, const btConvexShape* shape,
const btTransform& wtrs, const btTransform& wtrs,
sResults& results); sResults& results);
static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0, static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0,
const btConvexShape* shape1,const btTransform& wtrs1, const btConvexShape* shape1,const btTransform& wtrs1,
const btVector3& guess, const btVector3& guess,
sResults& results); sResults& results);

4
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

@@ -70,8 +70,8 @@ if(node)
if(depth>=mindepth) if(depth>=mindepth)
{ {
const btScalar scl=(btScalar)(node->isinternal()?1:1); const btScalar scl=(btScalar)(node->isinternal()?1:1);
const btVector3 mi=node->volume.Center()-node->volume.Extent()*scl; const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl;
const btVector3 mx=node->volume.Center()+node->volume.Extent()*scl; const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl;
drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
} }
} }