OpenSimMirror/libraries/ode-0.9/OPCODE/OPC_AABBTree.cpp

574 lines
22 KiB
C++

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a versatile AABB tree.
* \file OPC_AABBTree.cpp
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a generic AABB tree node.
*
* \class AABBTreeNode
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a generic AABB tree.
* This is a vanilla AABB tree, without any particular optimization. It contains anonymous references to
* user-provided primitives, which can theoretically be anything - triangles, boxes, etc. Each primitive
* is surrounded by an AABB, regardless of the primitive's nature. When the primitive is a triangle, the
* resulting tree can be converted into an optimized tree. If the primitive is a box, the resulting tree
* can be used for culling - VFC or occlusion -, assuming you cull on a mesh-by-mesh basis (modern way).
*
* \class AABBTree
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTreeNode::AABBTreeNode() :
mPos (null),
#ifndef OPC_NO_NEG_VANILLA_TREE
mNeg (null),
#endif
mNbPrimitives (0),
mNodePrimitives (null)
{
#ifdef OPC_USE_TREE_COHERENCE
mBitmask = 0;
#endif
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTreeNode::~AABBTreeNode()
{
// Opcode 1.3:
const AABBTreeNode* Pos = GetPos();
const AABBTreeNode* Neg = GetNeg();
#ifndef OPC_NO_NEG_VANILLA_TREE
if(!(mPos&1)) DELETESINGLE(Pos);
if(!(mNeg&1)) DELETESINGLE(Neg);
#else
if(!(mPos&1)) DELETEARRAY(Pos);
#endif
mNodePrimitives = null; // This was just a shortcut to the global list => no release
mNbPrimitives = 0;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Splits the node along a given axis.
* The list of indices is reorganized according to the split values.
* \param axis [in] splitting axis index
* \param builder [in] the tree builder
* \return the number of primitives assigned to the first child
* \warning this method reorganizes the internal list of primitives
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword AABBTreeNode::Split(udword axis, AABBTreeBuilder* builder)
{
// Get node split value
float SplitValue = builder->GetSplittingValue(mNodePrimitives, mNbPrimitives, mBV, axis);
udword NbPos = 0;
// Loop through all node-related primitives. Their indices range from mNodePrimitives[0] to mNodePrimitives[mNbPrimitives-1].
// Those indices map the global list in the tree builder.
for(udword i=0;i<mNbPrimitives;i++)
{
// Get index in global list
udword Index = mNodePrimitives[i];
// Test against the splitting value. The primitive value is tested against the enclosing-box center.
// [We only need an approximate partition of the enclosing box here.]
float PrimitiveValue = builder->GetSplittingValue(Index, axis);
// Reorganize the list of indices in this order: positive - negative.
if(PrimitiveValue > SplitValue)
{
// Swap entries
udword Tmp = mNodePrimitives[i];
mNodePrimitives[i] = mNodePrimitives[NbPos];
mNodePrimitives[NbPos] = Tmp;
// Count primitives assigned to positive space
NbPos++;
}
}
return NbPos;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Subdivides the node.
*
* N
* / \
* / \
* N/2 N/2
* / \ / \
* N/4 N/4 N/4 N/4
* (etc)
*
* A well-balanced tree should have a O(log n) depth.
* A degenerate tree would have a O(n) depth.
* Note a perfectly-balanced tree is not well-suited to collision detection anyway.
*
* \param builder [in] the tree builder
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeNode::Subdivide(AABBTreeBuilder* builder)
{
// Checkings
if(!builder) return false;
// Stop subdividing if we reach a leaf node. This is always performed here,
// else we could end in trouble if user overrides this.
if(mNbPrimitives==1) return true;
// Let the user validate the subdivision
if(!builder->ValidateSubdivision(mNodePrimitives, mNbPrimitives, mBV)) return true;
bool ValidSplit = true; // Optimism...
udword NbPos;
if(builder->mSettings.mRules & SPLIT_LARGEST_AXIS)
{
// Find the largest axis to split along
Point Extents; mBV.GetExtents(Extents); // Box extents
udword Axis = Extents.LargestAxis(); // Index of largest axis
// Split along the axis
NbPos = Split(Axis, builder);
// Check split validity
if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false;
}
else if(builder->mSettings.mRules & SPLIT_SPLATTER_POINTS)
{
// Compute the means
Point Means(0.0f, 0.0f, 0.0f);
for(udword i=0;i<mNbPrimitives;i++)
{
udword Index = mNodePrimitives[i];
Means.x+=builder->GetSplittingValue(Index, 0);
Means.y+=builder->GetSplittingValue(Index, 1);
Means.z+=builder->GetSplittingValue(Index, 2);
}
Means/=float(mNbPrimitives);
// Compute variances
Point Vars(0.0f, 0.0f, 0.0f);
for(udword i=0;i<mNbPrimitives;i++)
{
udword Index = mNodePrimitives[i];
float Cx = builder->GetSplittingValue(Index, 0);
float Cy = builder->GetSplittingValue(Index, 1);
float Cz = builder->GetSplittingValue(Index, 2);
Vars.x += (Cx - Means.x)*(Cx - Means.x);
Vars.y += (Cy - Means.y)*(Cy - Means.y);
Vars.z += (Cz - Means.z)*(Cz - Means.z);
}
Vars/=float(mNbPrimitives-1);
// Choose axis with greatest variance
udword Axis = Vars.LargestAxis();
// Split along the axis
NbPos = Split(Axis, builder);
// Check split validity
if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false;
}
else if(builder->mSettings.mRules & SPLIT_BALANCED)
{
// Test 3 axis, take the best
float Results[3];
NbPos = Split(0, builder); Results[0] = float(NbPos)/float(mNbPrimitives);
NbPos = Split(1, builder); Results[1] = float(NbPos)/float(mNbPrimitives);
NbPos = Split(2, builder); Results[2] = float(NbPos)/float(mNbPrimitives);
Results[0]-=0.5f; Results[0]*=Results[0];
Results[1]-=0.5f; Results[1]*=Results[1];
Results[2]-=0.5f; Results[2]*=Results[2];
udword Min=0;
if(Results[1]<Results[Min]) Min = 1;
if(Results[2]<Results[Min]) Min = 2;
// Split along the axis
NbPos = Split(Min, builder);
// Check split validity
if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false;
}
else if(builder->mSettings.mRules & SPLIT_BEST_AXIS)
{
// Test largest, then middle, then smallest axis...
// Sort axis
Point Extents; mBV.GetExtents(Extents); // Box extents
udword SortedAxis[] = { 0, 1, 2 };
float* Keys = (float*)&Extents.x;
for(udword j=0;j<3;j++)
{
for(udword i=0;i<2;i++)
{
if(Keys[SortedAxis[i]]<Keys[SortedAxis[i+1]])
{
udword Tmp = SortedAxis[i];
SortedAxis[i] = SortedAxis[i+1];
SortedAxis[i+1] = Tmp;
}
}
}
// Find the largest axis to split along
udword CurAxis = 0;
ValidSplit = false;
while(!ValidSplit && CurAxis!=3)
{
NbPos = Split(SortedAxis[CurAxis], builder);
// Check the subdivision has been successful
if(!NbPos || NbPos==mNbPrimitives) CurAxis++;
else ValidSplit = true;
}
}
else if(builder->mSettings.mRules & SPLIT_FIFTY)
{
// Don't even bother splitting (mainly a performance test)
NbPos = mNbPrimitives>>1;
}
else return false; // Unknown splitting rules
// Check the subdivision has been successful
if(!ValidSplit)
{
// Here, all boxes lie in the same sub-space. Two strategies:
// - if the tree *must* be complete, make an arbitrary 50-50 split
// - else stop subdividing
// if(builder->mSettings.mRules&SPLIT_COMPLETE)
if(builder->mSettings.mLimit==1)
{
builder->IncreaseNbInvalidSplits();
NbPos = mNbPrimitives>>1;
}
else return true;
}
// Now create children and assign their pointers.
if(builder->mNodeBase)
{
// We use a pre-allocated linear pool for complete trees [Opcode 1.3]
AABBTreeNode* Pool = (AABBTreeNode*)builder->mNodeBase;
udword Count = builder->GetCount() - 1; // Count begins to 1...
// Set last bit to tell it shouldn't be freed ### pretty ugly, find a better way. Maybe one bit in mNbPrimitives
ASSERT(!(udword(&Pool[Count+0])&1));
ASSERT(!(udword(&Pool[Count+1])&1));
mPos = size_t(&Pool[Count+0])|1;
#ifndef OPC_NO_NEG_VANILLA_TREE
mNeg = size_t(&Pool[Count+1])|1;
#endif
}
else
{
// Non-complete trees and/or Opcode 1.2 allocate nodes on-the-fly
#ifndef OPC_NO_NEG_VANILLA_TREE
mPos = (size_t)new AABBTreeNode; CHECKALLOC(mPos);
mNeg = (size_t)new AABBTreeNode; CHECKALLOC(mNeg);
#else
AABBTreeNode* PosNeg = new AABBTreeNode[2];
CHECKALLOC(PosNeg);
mPos = (size_t)PosNeg;
#endif
}
// Update stats
builder->IncreaseCount(2);
// Assign children
AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
Pos->mNodePrimitives = &mNodePrimitives[0];
Pos->mNbPrimitives = NbPos;
Neg->mNodePrimitives = &mNodePrimitives[NbPos];
Neg->mNbPrimitives = mNbPrimitives - NbPos;
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive hierarchy building in a top-down fashion.
* \param builder [in] the tree builder
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeNode::_BuildHierarchy(AABBTreeBuilder* builder)
{
// 1) Compute the global box for current node. The box is stored in mBV.
builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);
// 2) Subdivide current node
Subdivide(builder);
// 3) Recurse
AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
if(Pos) Pos->_BuildHierarchy(builder);
if(Neg) Neg->_BuildHierarchy(builder);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the tree (top-down).
* \param builder [in] the tree builder
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeNode::_Refit(AABBTreeBuilder* builder)
{
// 1) Recompute the new global box for current node
builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);
// 2) Recurse
AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
if(Pos) Pos->_Refit(builder);
if(Neg) Neg->_Refit(builder);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTree::AABBTree() : mIndices(null), mTotalNbNodes(0), mPool(null)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTree::~AABBTree()
{
Release();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Releases the tree.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTree::Release()
{
DELETEARRAY(mPool);
DELETEARRAY(mIndices);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds a generic AABB tree from a tree builder.
* \param builder [in] the tree builder
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTree::Build(AABBTreeBuilder* builder)
{
// Checkings
if(!builder || !builder->mNbPrimitives) return false;
// Release previous tree
Release();
// Init stats
builder->SetCount(1);
builder->SetNbInvalidSplits(0);
// Initialize indices. This list will be modified during build.
mIndices = new udword[builder->mNbPrimitives];
CHECKALLOC(mIndices);
// Identity permutation
for(udword i=0;i<builder->mNbPrimitives;i++) mIndices[i] = i;
// Setup initial node. Here we have a complete permutation of the app's primitives.
mNodePrimitives = mIndices;
mNbPrimitives = builder->mNbPrimitives;
// Use a linear array for complete trees (since we can predict the final number of nodes) [Opcode 1.3]
// if(builder->mRules&SPLIT_COMPLETE)
if(builder->mSettings.mLimit==1)
{
// Allocate a pool of nodes
mPool = new AABBTreeNode[builder->mNbPrimitives*2 - 1];
builder->mNodeBase = mPool; // ### ugly !
}
// Build the hierarchy
_BuildHierarchy(builder);
// Get back total number of nodes
mTotalNbNodes = builder->GetCount();
// For complete trees, check the correct number of nodes has been created [Opcode 1.3]
if(mPool) ASSERT(mTotalNbNodes==builder->mNbPrimitives*2 - 1);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the depth of the tree.
* A well-balanced tree should have a log(n) depth. A degenerate tree O(n) depth.
* \return depth of the tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword AABBTree::ComputeDepth() const
{
return Walk(null, null); // Use the walking code without callback
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Walks the tree, calling the user back for each node.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword AABBTree::Walk(WalkingCallback callback, void* user_data) const
{
// Call it without callback to compute max depth
udword MaxDepth = 0;
udword CurrentDepth = 0;
struct Local
{
static void _Walk(const AABBTreeNode* current_node, udword& max_depth, udword& current_depth, WalkingCallback callback, void* user_data)
{
// Checkings
if(!current_node) return;
// Entering a new node => increase depth
current_depth++;
// Keep track of max depth
if(current_depth>max_depth) max_depth = current_depth;
// Callback
if(callback && !(callback)(current_node, current_depth, user_data)) return;
// Recurse
if(current_node->GetPos()) { _Walk(current_node->GetPos(), max_depth, current_depth, callback, user_data); current_depth--; }
if(current_node->GetNeg()) { _Walk(current_node->GetNeg(), max_depth, current_depth, callback, user_data); current_depth--; }
}
};
Local::_Walk(this, MaxDepth, CurrentDepth, callback, user_data);
return MaxDepth;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the tree in a top-down way.
* \param builder [in] the tree builder
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTree::Refit(AABBTreeBuilder* builder)
{
if(!builder) return false;
_Refit(builder);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the tree in a bottom-up way.
* \param builder [in] the tree builder
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTree::Refit2(AABBTreeBuilder* builder)
{
// Checkings
if(!builder) return false;
ASSERT(mPool);
// Bottom-up update
Point Min,Max;
Point Min_,Max_;
udword Index = mTotalNbNodes;
while(Index--)
{
AABBTreeNode& Current = mPool[Index];
if(Current.IsLeaf())
{
builder->ComputeGlobalBox(Current.GetPrimitives(), Current.GetNbPrimitives(), *(AABB*)Current.GetAABB());
}
else
{
Current.GetPos()->GetAABB()->GetMin(Min);
Current.GetPos()->GetAABB()->GetMax(Max);
Current.GetNeg()->GetAABB()->GetMin(Min_);
Current.GetNeg()->GetAABB()->GetMax(Max_);
Min.Min(Min_);
Max.Max(Max_);
((AABB*)Current.GetAABB())->SetMinMax(Min, Max);
}
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the number of bytes used by the tree.
* \return number of bytes used
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword AABBTree::GetUsedBytes() const
{
udword TotalSize = mTotalNbNodes*GetNodeSize();
if(mIndices) TotalSize+=mNbPrimitives*sizeof(udword);
return TotalSize;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the tree is a complete tree or not.
* A complete tree is made of 2*N-1 nodes, where N is the number of primitives in the tree.
* \return true for complete trees
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTree::IsComplete() const
{
return (GetNbNodes()==GetNbPrimitives()*2-1);
}