1ef4c721a0
Added Bullet SAP/MultiSAP support by Erwin Coumans (BulletSAPCompleteBoxPruningTest.*) AABB tree broadphase by Nathanael Presson (btDbvt.*, DbvtTest.*)
424 lines
9.0 KiB
C++
424 lines
9.0 KiB
C++
/*
|
|
Bounding Volume Hierarchy, btDbvt.cpp
|
|
Copyright (c) 2008 Nathanael Presson, as part of Bullet Physics Library
|
|
|
|
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 "stdafx.h"
|
|
#include "btDbvt.h"
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
namespace btdbvt_internals
|
|
{
|
|
|
|
//
|
|
typedef btAlignedObjectArray<btDbvt::Node*> tNodeArray;
|
|
|
|
//
|
|
static inline int indexof(const btDbvt::Node* node)
|
|
{
|
|
return(node->parent->childs[1]==node);
|
|
}
|
|
|
|
//
|
|
static inline btDbvt::Aabb merge( const btDbvt::Aabb& a,
|
|
const btDbvt::Aabb& b)
|
|
{
|
|
btDbvt::Aabb res=a;
|
|
res.mi.setMin(b.mi);
|
|
res.mx.setMax(b.mx);
|
|
return(res);
|
|
}
|
|
|
|
// volume+edge lengths
|
|
static inline btScalar size(const btDbvt::Aabb& a)
|
|
{
|
|
const btVector3 edges=a.mx-a.mi;
|
|
return( edges.x()*edges.y()*edges.z()+
|
|
edges.x()+edges.y()+edges.z());
|
|
}
|
|
|
|
// Using manhattan distance heuristic
|
|
static inline btScalar proximity( const btDbvt::Aabb& a,
|
|
const btDbvt::Aabb& b)
|
|
{
|
|
const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
|
|
return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
|
|
}
|
|
|
|
//
|
|
static inline bool contain( const btDbvt::Aabb& a,
|
|
const btDbvt::Aabb& b)
|
|
{
|
|
return( (a.mi.x()<=b.mi.x())&&
|
|
(a.mx.x()>=b.mx.x())&&
|
|
(a.mi.y()<=b.mi.y())&&
|
|
(a.mx.y()>=b.mx.y())&&
|
|
(a.mi.z()<=b.mi.z())&&
|
|
(a.mx.z()>=b.mx.z()));
|
|
}
|
|
|
|
//
|
|
static inline void deletenode( btDbvt* pdbvt,
|
|
btDbvt::Node* node)
|
|
{
|
|
if(pdbvt->m_stock) delete pdbvt->m_stock;
|
|
pdbvt->m_stock=node;
|
|
}
|
|
|
|
//
|
|
static inline void recursedeletenode( btDbvt* pdbvt,
|
|
btDbvt::Node* 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 inline btDbvt::Node* createnode( btDbvt* pdbvt,
|
|
btDbvt::Node* parent,
|
|
const btDbvt::Aabb& box,
|
|
void* data)
|
|
{
|
|
btDbvt::Node* node;
|
|
if(pdbvt->m_stock)
|
|
{ node=pdbvt->m_stock;pdbvt->m_stock=0; }
|
|
else
|
|
{ node=new btDbvt::Node(); }
|
|
node->parent = parent;
|
|
node->box = box;
|
|
node->data = data;
|
|
node->childs[1] = 0;
|
|
return(node);
|
|
}
|
|
|
|
//
|
|
static inline void insertleaf( btDbvt* pdbvt,
|
|
btDbvt::Node* root,
|
|
btDbvt::Node* leaf)
|
|
{
|
|
if(!pdbvt->m_root)
|
|
{
|
|
pdbvt->m_root = leaf;
|
|
leaf->parent = 0;
|
|
}
|
|
else
|
|
{
|
|
if(!root->isleaf())
|
|
{
|
|
do {
|
|
if( proximity(root->childs[0]->box,leaf->box)<
|
|
proximity(root->childs[1]->box,leaf->box))
|
|
root=root->childs[0];
|
|
else
|
|
root=root->childs[1];
|
|
} while(!root->isleaf());
|
|
}
|
|
btDbvt::Node* prev=root->parent;
|
|
btDbvt::Node* node=createnode(pdbvt,prev,merge(leaf->box,root->box),0);
|
|
if(prev)
|
|
{
|
|
prev->childs[indexof(root)] = node;
|
|
node->childs[0] = root;root->parent=node;
|
|
node->childs[1] = leaf;leaf->parent=node;
|
|
do {
|
|
if(contain(prev->box,node->box))
|
|
break;
|
|
else
|
|
prev->box=merge(prev->childs[0]->box,prev->childs[1]->box);
|
|
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;
|
|
}
|
|
}
|
|
++pdbvt->m_nleafs;
|
|
}
|
|
|
|
//
|
|
static inline btDbvt::Node* removeleaf( btDbvt* pdbvt,
|
|
btDbvt::Node* leaf)
|
|
{
|
|
--pdbvt->m_nleafs;
|
|
if(leaf==pdbvt->m_root)
|
|
{
|
|
pdbvt->m_root=0;
|
|
return(0);
|
|
}
|
|
else
|
|
{
|
|
btDbvt::Node* parent=leaf->parent;
|
|
btDbvt::Node* prev=parent->parent;
|
|
btDbvt::Node* sibling=parent->childs[1-indexof(leaf)];
|
|
if(prev)
|
|
{
|
|
prev->childs[indexof(parent)]=sibling;
|
|
sibling->parent=prev;
|
|
deletenode(pdbvt,parent);
|
|
while(prev)
|
|
{
|
|
const btDbvt::Aabb pb=prev->box;
|
|
prev->box = merge(prev->childs[0]->box,prev->childs[1]->box);
|
|
if(0==memcmp(&pb,&prev->box,sizeof(pb))) break;
|
|
sibling = prev;
|
|
prev = prev->parent;
|
|
}
|
|
return(prev?prev:pdbvt->m_root);
|
|
}
|
|
else
|
|
{
|
|
pdbvt->m_root=sibling;
|
|
sibling->parent=0;
|
|
deletenode(pdbvt,parent);
|
|
return(pdbvt->m_root);
|
|
}
|
|
}
|
|
}
|
|
|
|
//
|
|
static void fetchleafs( btDbvt::Node* root,
|
|
tNodeArray& leafs)
|
|
{
|
|
if(root->isinternal())
|
|
{
|
|
fetchleafs(root->childs[0],leafs);
|
|
fetchleafs(root->childs[1],leafs);
|
|
}
|
|
else
|
|
{
|
|
leafs.push_back(root);
|
|
}
|
|
}
|
|
|
|
//
|
|
static void split( const tNodeArray& leafs,
|
|
tNodeArray& left,
|
|
tNodeArray& right,
|
|
const btVector3& org,
|
|
const btVector3& axis)
|
|
{
|
|
left.resize(0);
|
|
right.resize(0);
|
|
for(int i=0,ni=leafs.size();i<ni;++i)
|
|
{
|
|
if(dot(axis,leafs[i]->box.Center()-org)<0)
|
|
left.push_back(leafs[i]);
|
|
else
|
|
right.push_back(leafs[i]);
|
|
}
|
|
}
|
|
|
|
//
|
|
static btDbvt::Aabb bounds( const tNodeArray& leafs)
|
|
{
|
|
btDbvt::Aabb box=leafs[0]->box;
|
|
for(int i=1,ni=leafs.size();i<ni;++i)
|
|
{
|
|
box=merge(box,leafs[i]->box);
|
|
}
|
|
return(box);
|
|
}
|
|
|
|
//
|
|
static void bottomup( btDbvt* pdbvt,
|
|
tNodeArray& leafs)
|
|
{
|
|
while(leafs.size()>1)
|
|
{
|
|
btScalar minsize=SIMD_INFINITY;
|
|
int minidx[2]={-1,-1};
|
|
for(int i=0;i<leafs.size();++i)
|
|
{
|
|
for(int j=i+1;j<leafs.size();++j)
|
|
{
|
|
const btScalar sz=size(merge(leafs[i]->box,leafs[j]->box));
|
|
if(sz<minsize)
|
|
{
|
|
minsize = sz;
|
|
minidx[0] = i;
|
|
minidx[1] = j;
|
|
}
|
|
}
|
|
}
|
|
btDbvt::Node* n[] = {leafs[minidx[0]],leafs[minidx[1]]};
|
|
btDbvt::Node* p = createnode(pdbvt,0,merge(n[0]->box,n[1]->box),0);
|
|
p->childs[0] = n[0];
|
|
p->childs[1] = n[1];
|
|
n[0]->parent = p;
|
|
n[1]->parent = p;
|
|
leafs[minidx[0]] = p;
|
|
leafs.swap(minidx[1],leafs.size()-1);
|
|
leafs.pop_back();
|
|
}
|
|
}
|
|
|
|
//
|
|
static btDbvt::Node* topdown(btDbvt* pdbvt,
|
|
tNodeArray& leafs)
|
|
{
|
|
static const btVector3 axis[]={btVector3(1,0,0),
|
|
btVector3(0,1,0),
|
|
btVector3(0,0,1)};
|
|
if(leafs.size()>1)
|
|
{
|
|
const btDbvt::Aabb box=bounds(leafs);
|
|
const btVector3 org=box.Center();
|
|
tNodeArray sets[2];
|
|
int bestaxis=-1;
|
|
int bestmidp=leafs.size();
|
|
sets[0].reserve(leafs.size());
|
|
sets[1].reserve(leafs.size());
|
|
for(int i=0;i<3;++i)
|
|
{
|
|
split(leafs,sets[0],sets[1],org,axis[i]);
|
|
if((sets[0].size()>0)&&(sets[1].size()>0))
|
|
{
|
|
const int midp=abs(sets[0].size()-sets[1].size());
|
|
if(midp<bestmidp)
|
|
{
|
|
bestaxis=i;
|
|
bestmidp=midp;
|
|
}
|
|
}
|
|
}
|
|
if(bestaxis>=0)
|
|
{
|
|
split(leafs,sets[0],sets[1],org,axis[bestaxis]);
|
|
}
|
|
else
|
|
{
|
|
sets[0].resize(0);
|
|
sets[1].resize(0);
|
|
for(int i=0,ni=leafs.size();i<ni;++i)
|
|
{
|
|
sets[i&1].push_back(leafs[i]);
|
|
}
|
|
}
|
|
btDbvt::Node* node=createnode(pdbvt,0,box,0);
|
|
node->childs[0]=topdown(pdbvt,sets[0]);
|
|
node->childs[1]=topdown(pdbvt,sets[1]);
|
|
node->childs[0]->parent=node;
|
|
node->childs[1]->parent=node;
|
|
return(node);
|
|
}
|
|
return(leafs[0]);
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
using namespace btdbvt_internals;
|
|
|
|
//
|
|
// Api
|
|
//
|
|
|
|
//
|
|
btDbvt* btDbvt::Create()
|
|
{
|
|
btDbvt* pdbvt = new btDbvt();
|
|
pdbvt->m_nleafs = 0;
|
|
pdbvt->m_root = 0;
|
|
pdbvt->m_stock = 0;
|
|
return(pdbvt);
|
|
}
|
|
|
|
//
|
|
void btDbvt::Delete()
|
|
{
|
|
if(m_root) recursedeletenode(this,m_root);
|
|
if(m_stock) delete m_stock;
|
|
delete this;
|
|
}
|
|
|
|
//
|
|
void btDbvt::OptimizeBottomUp()
|
|
{
|
|
if(m_root)
|
|
{
|
|
tNodeArray leafs;
|
|
leafs.reserve(m_nleafs);
|
|
fetchleafs(m_root,leafs);
|
|
for(int i=0,ni=leafs.size();i<ni;++i)
|
|
{
|
|
removeleaf(this,leafs[i]);
|
|
}
|
|
m_nleafs = leafs.size();
|
|
bottomup(this,leafs);
|
|
m_root = leafs[0];
|
|
}
|
|
}
|
|
|
|
//
|
|
void btDbvt::OptimizeTopDown()
|
|
{
|
|
if(m_root)
|
|
{
|
|
tNodeArray leafs;
|
|
leafs.reserve(m_nleafs);
|
|
fetchleafs(m_root,leafs);
|
|
for(int i=0,ni=leafs.size();i<ni;++i)
|
|
{
|
|
removeleaf(this,leafs[i]);
|
|
}
|
|
m_nleafs = leafs.size();
|
|
m_root = topdown(this,leafs);
|
|
}
|
|
}
|
|
|
|
//
|
|
btDbvt::Node* btDbvt::Insert( const btVector3& center,
|
|
const btVector3& extent,
|
|
void* data)
|
|
{
|
|
Node* leaf=createnode(this,0,Aabb::FromCE(center,extent),data);
|
|
insertleaf(this,m_root,leaf);
|
|
return(leaf);
|
|
}
|
|
|
|
//
|
|
bool btDbvt::Update( Node* leaf,
|
|
const btVector3& center,
|
|
const btVector3& extent,
|
|
btScalar margin)
|
|
{
|
|
Aabb box=Aabb::FromCE(center,extent);
|
|
if(margin>0)
|
|
{
|
|
if(contain(leaf->box,box)) return(false);
|
|
const btVector3 vm(margin,margin,margin);
|
|
box.mi-=vm;box.mx+=vm;
|
|
}
|
|
Node* root=removeleaf(this,leaf);
|
|
leaf->box=box;
|
|
insertleaf(this,root,leaf);
|
|
return(true);
|
|
}
|
|
|
|
//
|
|
void btDbvt::Remove( Node* leaf)
|
|
{
|
|
removeleaf(this,leaf);
|
|
deletenode(this,leaf);
|
|
} |