You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1076 lines
43 KiB
1076 lines
43 KiB
//=================================================================================================//
|
|
// filename: utlspheretree.h //
|
|
// //
|
|
// description: A binary tree of spheres - put stuff in it to do fast spatial queries //
|
|
// //
|
|
//=================================================================================================//
|
|
|
|
#ifndef _utlspheretree_H_
|
|
#define _utlspheretree_H_
|
|
|
|
#include "mathlib/vector4d.h"
|
|
#include "tier1/utlvector.h"
|
|
#include "collisionutils.h"
|
|
|
|
typedef Vector4D Sphere_t;
|
|
typedef Vector4D Plane_t;
|
|
|
|
|
|
// Paste the following commented text into your autoexp.dat to get a nifty debugger visualizer for CUtlSphereTree.
|
|
// (insert it below [Visualizer] or ";-------------VALVE AUTOEXP AUTOGENERATED BLOCK START----------------" )
|
|
// NOTE: you need to define DEBUGGABLE_NODES (on by default in debug) and rebuild to get the full benefit of this.
|
|
// This does bog down VS2005 if you expand many levels of the tree - paste “tree.m_Nodes.m_pElements[node]” to sidestep that.
|
|
// Also note that names are CASE-INSENSITIVE, so if you have a member 'index' and a function 'Index', it will break :o/
|
|
#ifdef _DEBUG
|
|
#define DEBUGGABLE_NODES
|
|
#endif
|
|
/*
|
|
;-------- CUtlSphereTree start ------------------------------------------------------
|
|
Vector4D{
|
|
preview
|
|
( #( "(", $c.x, ", ", $c.y, ", ", $c.z, ", ", $c.w, ")" ) )
|
|
}
|
|
CUtlSphereTree::Node{
|
|
preview
|
|
( #(
|
|
#if ( $c.deepIndex < 0 )
|
|
( #( "leaf data = ", $c.pData, ", bounds(", $c.bounds.x, ", ", $c.bounds.y, ", ", $c.bounds.z, ", ", $c.bounds.w, ")" ) )
|
|
#else
|
|
( #( "depth = ", $c.maxDepth, ", bounds(", $c.bounds.x, ", ", $c.bounds.y, ", ", $c.bounds.z, ", ", $c.bounds.w, ")" ) )
|
|
) )
|
|
children
|
|
( #(
|
|
#if ( $c.deepIndex < 0 )
|
|
( #(
|
|
#( leaf data : $c.pData )
|
|
) )
|
|
#else
|
|
( #(
|
|
#( child[deep] : $c.pTree->m_Nodes.m_pElements[ $c.deepIndex ] ),
|
|
#( child[shallow] : $c.pTree->m_Nodes.m_pElements[ $c.shallowIndex ] ),
|
|
#( subtree depth : $c.maxDepth )
|
|
) ),
|
|
#( node bounds : $c.bounds ),
|
|
#( z[raw_members] : [$c,!] )
|
|
) )
|
|
}
|
|
CUtlSphereTree::NodeRef{
|
|
preview
|
|
( #(
|
|
#if ( $c.nodeIndex < 0 )
|
|
( #( "Invalid!" ) )
|
|
#else
|
|
( #(
|
|
#if ( $c.pNodes->m_pElements[ $c.nodeIndex ].deepIndex < 0 )
|
|
( #( "leaf data = ", $c.pNodes->m_pElements[ $c.nodeIndex ].pData, ", bounds(", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.x, ", ", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.y, ", ", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.z, ", ", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.w, ")" ) )
|
|
#else
|
|
( #( "depth = ", $c.pNodes->m_pElements[ $c.nodeIndex ].maxDepth, ", bounds(", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.x, ", ", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.y, ", ", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.z, ", ", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.w, ")" ) )
|
|
) )
|
|
) )
|
|
children
|
|
( #(
|
|
#if ( $c.nodeIndex < 0 )
|
|
( #( invalid! : $c.nodeIndex ) )
|
|
#else
|
|
( #( node : $c.pNodes->m_pElements[ $c.nodeIndex ] ) ),
|
|
#( z[raw_members] : [$c,!] )
|
|
) )
|
|
}
|
|
CUtlSphereTree{
|
|
preview
|
|
( #( "treeCount = ", $c.m_NumNodesInTree, ", allocCount = ", $c.m_Nodes.m_Size, ", freelistHead = ", $c.m_FreelistHead ) )
|
|
children
|
|
( #(
|
|
deep child : $c.m_Nodes.m_pElements[ $c.m_Nodes.m_pElements[0].deepIndex ],
|
|
shallow child : $c.m_Nodes.m_pElements[ $c.m_Nodes.m_pElements[0].shallowIndex ],
|
|
z[raw_members] : [$c,!]
|
|
) )
|
|
}
|
|
;-------- CUtlSphereTree end ------------------------------------------------------
|
|
*/
|
|
|
|
class CUtlSphereTree
|
|
{
|
|
struct Node
|
|
{
|
|
// Every node either has two children or (if it is a leaf) none
|
|
// TODO: shrink the size of Node (pack child node indices and isLeaf flags into an int32, with leaf nodes in a separate vector so we can index 2^16 nodes total)
|
|
// TODO: for the case where the caller's data already contains a bounding Sphere_t, we could omit the 20-byte Sphere_t from leaf nodes
|
|
// (the void * would need wrapping with a user-provided class that provides a bounding sphere accessor function - or, just provide a pointer to the Sphere_t)
|
|
// TODO: determine whether nodes fully containing other nodes happens a lot in practise - if so, it may be worthwhile allowing non-leaf Nodes to hold a pData pointer
|
|
Sphere_t bounds;
|
|
int deepIndex, shallowIndex;// Not set (-1) for Leaf nodes
|
|
const void *pData; // Only set for Leaf nodes
|
|
int maxDepth; // The maximum number of steps to a leaf node from here
|
|
// (zero for leaf nodes), used to rebalance the tree
|
|
#ifdef DEBUGGABLE_NODES
|
|
CUtlSphereTree *pTree; // Allows debug pane tree expansion
|
|
#endif
|
|
};
|
|
|
|
struct NodeRef
|
|
{
|
|
// Convenience class to allow access to nodes, stored in the m_Nodes vector, by index
|
|
NodeRef( int node, const CUtlVector< Node > &nodes )
|
|
: nodeIndex( node ), pNodes( (CUtlVector< Node > *)&nodes ) {}
|
|
|
|
Node * Ptr( void ) const
|
|
{
|
|
Assert ( nodeIndex >= 0 );
|
|
return ( nodeIndex >= 0 ) ? &(*pNodes)[ nodeIndex ] : (Node *)NULL;
|
|
}
|
|
Node * operator->( void ) const { return Ptr(); }
|
|
Node & operator*( void ) const { return *Ptr(); }
|
|
int Index( void ) const { return nodeIndex; }
|
|
NodeRef Deep( void ) const { return NodeRef( Ptr()->deepIndex, *pNodes ); }
|
|
NodeRef Shallow( void ) const { return NodeRef( Ptr()->shallowIndex, *pNodes ); }
|
|
bool IsValid( void ) const { return ( nodeIndex >= 0 ); }
|
|
bool IsLeaf( void ) const { return ( Ptr()->deepIndex < 0 ); }
|
|
|
|
private:
|
|
NodeRef( void ) : nodeIndex( -1 ), pNodes( NULL ) { Assert( 0 ); }
|
|
|
|
int nodeIndex;
|
|
CUtlVector< Node > * pNodes; // TODO: find a way to get rid of this, given it's the same for all NodeRefs
|
|
};
|
|
|
|
public:
|
|
|
|
// Cut
|
|
//
|
|
// This member class represents a 'cut' of a sphere tree - which is the result of a depth-first traversal
|
|
// of the tree in which each node is run through a test function that may return "true", "false" or "partial".
|
|
//
|
|
// For example:
|
|
// - a sphere tree can be 'cut' by a plane
|
|
// - the cut is initialised to contain the whole tree
|
|
// - the test function is a sphere-plane test
|
|
// o nodes which return 'true' (fully in front) are included in the cut and not traversed further
|
|
// o nodes which return 'false' (fully behind) are discarded from the cut and not traversed further
|
|
// o for nodes which return 'partial' (intersecting) they are discarded and the test recurses to their children
|
|
// partial leaf nodes are included in the cut
|
|
// - the result (the 'cut') is thus the set of nodes fully/partially in front of the plane,
|
|
// subdivided only where necessary to make the result more accurate
|
|
//
|
|
// In practise, the Cut contains an (unordered) list of sub-tree root node indices.
|
|
class Cut
|
|
{
|
|
friend class CUtlSphereTree;
|
|
|
|
public:
|
|
|
|
// Flags to guide how a Cut is to be refined by a set of intersection primitives
|
|
enum CutFlags
|
|
{
|
|
PARTIAL_INTERSECTIONS = 1, // 'Partial' node intersections should be counted
|
|
// (for planes, this means include nodes only partially in front of a plane)
|
|
OR_INTERSECTIONS = 2 // Passing the intersection test for ANY primitive in the current set constitutes
|
|
// a pass ('AND' mode is the default - it requires passing w.r.t ALL primitives)
|
|
};
|
|
|
|
enum CutTestResult
|
|
{
|
|
CUT_TEST_FAIL = 0,
|
|
CUT_TEST_PASS = 1,
|
|
CUT_TEST_PARTIAL = 2
|
|
};
|
|
|
|
// Initialize a Cut with an entire sphere tree (i.e. the root node)
|
|
Cut( const CUtlSphereTree *pTree );
|
|
~Cut( void );
|
|
|
|
// Return the user data pointers of the leaf nodes within the Cut. To limit the number of results returned,
|
|
// pass 'maxResults > 0' (note that the return value may be more than this).
|
|
// This is an efficient way to find all the leaves inside/outside a set of planes.
|
|
int GetLeaves( CUtlVector< void * > &leaves, int maxLeaves = 0 ) const;
|
|
|
|
private:
|
|
|
|
// Don't use the default constructor
|
|
Cut( void ) : m_pTree( NULL ) { Assert( 0 ); };
|
|
|
|
const CUtlSphereTree *m_pTree; // The associated sphere tree
|
|
CUtlVector< int >m_NodeRefs; // Sub-tree root node indices
|
|
};
|
|
|
|
|
|
public:
|
|
|
|
CUtlSphereTree();
|
|
virtual ~CUtlSphereTree();
|
|
|
|
// It is valid to Insert a NULL pData pointer, and it is valid to pass the same pData pointer with
|
|
// multiple different bounding spheres, but it is NOT valid to pass the same pointer+sphere pair twice.
|
|
// TODO: we can build a better tree if given all the spheres at once (would definitely be a win for non-dynamic scenes)
|
|
void Insert( const void *pData, const Sphere_t *bounds );
|
|
// Inverse of Insert (make sure to pass the same parameters!)
|
|
void Remove( const void *pData, const Sphere_t *bounds );
|
|
// Empty the tree (but do not deallocate)
|
|
void RemoveAll( void );
|
|
// Empty the tree (and deallocate)
|
|
void Purge( void );
|
|
|
|
|
|
// Intersect a ray with the spheretree (or a Cut thereof), returning the user data pointers of the leaf nodes intersecting
|
|
// the ray. To limit the number of results returned, pass 'maxResults > 0' (note that the return value may be more than this).
|
|
// NOTE: these spheres are currently unordered
|
|
// TODO: write a function to return the first intersection and an ordered list of intersections (use a priority queue of nodes,
|
|
// always processing the top element and emitting leaves - time this and compare it with just quicksorting the results)
|
|
int IntersectWithRay( Vector &rayStart, Vector &rayDelta, CUtlVector< void * > &result, int maxResults = 0, CUtlSphereTree::Cut *cut = NULL ) const;
|
|
// Intersect a sphere with the spheretree (or a Cut thereof), returning the user data pointers of the leaf nodes intersecting
|
|
// the sphere. To limit the number of results returned, pass 'maxResults > 0' (note that the return value may be more than this).
|
|
// If 'bPartial' is FALSE, only leaves fully contained within the sphere are returned (otherwise partial overlaps are returned too)
|
|
// NOTE: these spheres are currently unordered
|
|
// TODO: optionally sort by distance from the sphere centre
|
|
int IntersectWithSphere( const Sphere_t &sphere, bool bPartial, CUtlVector< void * > &result, int maxResults = 0, CUtlSphereTree::Cut *cut = NULL ) const;
|
|
// Cut the tree (or refine an existing cut) using one or more planes. The output Cut will contain all
|
|
// sphere tree nodes which are partially/fully in front of any/all of the planes (see CUtlSphereTree_Cut::CutFlags).
|
|
// If outputCut is NULL, then inputCut will be updated.
|
|
void CutByPlanes( Cut *outputCut, const Cut *inputCut, int cutFlags, Plane_t *planes, int numPlanes = 1 ) const;
|
|
|
|
// Use this method to measure the effect of any changes to tree-construction logic. It computes some numbers relating to the efficiency of the tree:
|
|
// - averageDepth: the tree's depth, as compared to the depth of a 'perfect' tree (idealDepth)
|
|
// - averageRedundancy: the average 'redundancy' (volume ratio) between a node and its parent - this is in [0,1] and correlates to query cost (lower is better, less than 0.4 is good).
|
|
// - averageSphereTestCost: estimated average cost of intersecting a sphere (of a given radius) with the tree (uses the redundancy values to compute intersection probabilities)
|
|
void MeasureTreeQuality( float &averageDepth, float &idealDepth, float &averageRedundancy, float &averageSphereTestCost, float sphereTestRadius = 0.0f ) const;
|
|
|
|
private:
|
|
|
|
// To avoid node/index-shuffling, we add Removed Nodes to a freelist:
|
|
void FreelistPush( NodeRef &node );
|
|
NodeRef FreelistPop( void );
|
|
|
|
// We can't hold pointers to Nodes within a CUtlVector, so this util func converts from an index to a NodeRef:
|
|
NodeRef Ref( int node ) const { return NodeRef( node, m_Nodes ); }
|
|
// Allocates a new node off m_Nodes and init it as a leaf
|
|
NodeRef NewNode( const void *pData, const Sphere_t *bounds );
|
|
// Allocates a new node off m_Nodes and init it by copying an existing node
|
|
NodeRef CopyNode( const NodeRef &nodeToCopy );
|
|
// Assign two children to a node (recomputes its 'bounds' and 'maxDepth')
|
|
void SetNodeChildren( NodeRef &node, const NodeRef &childA, const NodeRef &childB );
|
|
|
|
// Recursive function called by Insert()
|
|
void FindPathToClosestLeaf_R( NodeRef node, const Vector &pos, const float nodeDist, float &minDist, CUtlVector< int > &path ) const;
|
|
|
|
// Recursive function called by Remove()
|
|
int Remove_R( NodeRef &node, const void *pData, const Sphere_t *bounds );
|
|
// Called by Remove_R to identify the Node to remove:
|
|
bool NodeHasData( const NodeRef &node, const void *pData, const Sphere_t *bounds );
|
|
|
|
// Called by Insert() and Remove() to ensure that the subtrees under a node are balanced
|
|
void RebalanceSubtrees( NodeRef &node, bool bInserting );
|
|
// Is this node's sub-tree balanced? (left and right sub-trees differ in 'maxDepth' by at most 1)
|
|
bool IsSubtreeBalanced( const NodeRef &node ) const;
|
|
// Useful debug function if there's a bug in tree construction
|
|
void ParanoidBalanceCheck_R( const NodeRef &node ) const;
|
|
|
|
// The basic metric used during insertion to determine the 'cost' of different node-grouping choices
|
|
float Cost( const Sphere_t &sphere ) const;
|
|
// Computes the composite bounding sphere of two spheres
|
|
Sphere_t AddSpheres( const Sphere_t &pA, const Sphere_t &pB ) const;
|
|
// Called by RebalanceSubtrees() to determine the optimal rebalance permutation
|
|
float ComputePairingCost( const NodeRef &pair1A, const NodeRef &pair1B, const NodeRef &pair2A, const NodeRef *pair2B ) const;
|
|
|
|
// Called by IntersectWithRay
|
|
void IntersectWithRay_R( const NodeRef &node, Vector &rayStart, Vector &rayDelta, CUtlVector< void * > &result, int &count ) const;
|
|
// Called by IntersectWithSphere
|
|
void IntersectWithSphere_R( const NodeRef &node, const Sphere_t &sphere, const bool bPartial, CUtlVector< void * > &result, int &count, const float distSq ) const;
|
|
bool SpheresIntersect( const Sphere_t &A, const Sphere_t &B, float &distSq ) const;
|
|
// Called by CutByPlanes
|
|
void CutByPlanes_R( Cut *outputCut, const NodeRef &node, int cutFlags, Plane_t *planes, int numPlanes ) const;
|
|
Cut::CutTestResult CutByPlanes_Test( const NodeRef &node, int cutFlags, Plane_t *planes, int numPlanes ) const;
|
|
// Leaf node list accessor for Cut::GetLeaves:
|
|
void GetLeavesUnderNode_R( const NodeRef &node, CUtlVector< void * > &leaves, int &count ) const;
|
|
// Called by MeasureTreeQuality
|
|
void MeasureTreeQuality_R( const NodeRef &node, const int steps, float &averageDepth, float &averageRedundancy, const float pathRedundancy, float &averageSphereTestCost, const float pathProbability, const float sphereTestRadius ) const;
|
|
float ComputeRedundancy( const NodeRef &parent, const NodeRef &child ) const;
|
|
float ComputeIntersectionProbability( const NodeRef &parent, const NodeRef &child, const float sphereTestRadius ) const;
|
|
|
|
// We reference Nodes in this vector by index (the root is always at index zero)
|
|
CUtlVector< Node > m_Nodes;
|
|
// Removed Nodes are added to a freelist so we don't need to do any index-shuffling
|
|
int m_FreelistHead;
|
|
// How many nodes are in the tree? (i.e. in m_Nodes but not the freelist)
|
|
int m_NumNodesInTree;
|
|
// The index of the last inserted node (should always be a leaf, -1 if invalid):
|
|
int m_PrevInsertedNode;
|
|
};
|
|
|
|
|
|
|
|
|
|
// ===== CUtlSphereTree implementation =====================================
|
|
|
|
inline CUtlSphereTree::CUtlSphereTree( void )
|
|
{
|
|
Purge();
|
|
}
|
|
|
|
inline CUtlSphereTree::~CUtlSphereTree( void )
|
|
{
|
|
}
|
|
|
|
inline void CUtlSphereTree::RemoveAll( void )
|
|
{
|
|
// Empty m_Nodes and our freelist
|
|
m_Nodes.RemoveAll();
|
|
m_FreelistHead = -1;
|
|
m_NumNodesInTree = 0;
|
|
m_PrevInsertedNode = -1;
|
|
}
|
|
|
|
inline void CUtlSphereTree::Purge( void )
|
|
{
|
|
// Empty m_Nodes and our freelist (and deallocate memory)
|
|
RemoveAll();
|
|
m_Nodes.Purge();
|
|
}
|
|
|
|
inline void CUtlSphereTree::FreelistPush( NodeRef &node )
|
|
{
|
|
// TODO: update the freelist to use a FreeNodeRef struct which can be traversed with a Debug Visualizer
|
|
if ( m_PrevInsertedNode == node.Index() )
|
|
m_PrevInsertedNode = -1;
|
|
*(int *)node.Ptr() = m_FreelistHead;
|
|
m_FreelistHead = node.Index();
|
|
Assert( m_NumNodesInTree > 0 );
|
|
m_NumNodesInTree--;
|
|
}
|
|
|
|
inline CUtlSphereTree::NodeRef CUtlSphereTree::FreelistPop( void )
|
|
{
|
|
Assert( m_FreelistHead >= 0 );
|
|
NodeRef node = Ref( m_FreelistHead );
|
|
if ( m_FreelistHead >= 0 )
|
|
m_FreelistHead = *(int *)&m_Nodes[ m_FreelistHead ];
|
|
return node;
|
|
}
|
|
|
|
inline CUtlSphereTree::NodeRef CUtlSphereTree::NewNode( const void *pData, const Sphere_t *bounds )
|
|
{
|
|
// Allocate a new node off m_Nodes (from our freelist if available) and init it as a leaf
|
|
Assert( ( m_NumNodesInTree >= 0 ) && ( m_NumNodesInTree <= m_Nodes.Count() ) );
|
|
Assert( ( m_NumNodesInTree < m_Nodes.Count() ) == ( m_FreelistHead >= 0 ) );
|
|
NodeRef node = ( m_FreelistHead >= 0 ) ? FreelistPop() : Ref( m_Nodes.AddToTail() );
|
|
m_NumNodesInTree++;
|
|
node->bounds = bounds ? *bounds : Sphere_t(0,0,0,0);
|
|
node->deepIndex = -1;
|
|
node->shallowIndex = -1;
|
|
node->pData = pData;
|
|
node->maxDepth = 0;
|
|
#ifdef DEBUGGABLE_NODES
|
|
node->pTree = this;
|
|
#endif
|
|
return node;
|
|
}
|
|
|
|
inline CUtlSphereTree::NodeRef CUtlSphereTree::CopyNode( const NodeRef &nodeToCopy )
|
|
{
|
|
// Allocate a new node off m_Nodes and init it by copying an existing node
|
|
NodeRef node = NewNode( NULL, NULL );
|
|
*node = *nodeToCopy;
|
|
return node;
|
|
}
|
|
|
|
inline void CUtlSphereTree::SetNodeChildren( NodeRef &node, const NodeRef &childA, const NodeRef &childB )
|
|
{
|
|
bool bChildADeeper = ( childA->maxDepth > childB->maxDepth );
|
|
node->deepIndex = bChildADeeper ? childA.Index() : childB.Index();
|
|
node->shallowIndex = bChildADeeper ? childB.Index() : childA.Index();
|
|
node->bounds = AddSpheres( node.Deep()->bounds, node.Shallow()->bounds );
|
|
node->maxDepth = node.Deep()->maxDepth + 1;
|
|
node->pData = NULL; // If it was a leaf, it's not any more
|
|
}
|
|
|
|
inline void CUtlSphereTree::Insert( const void *pData, const Sphere_t *bounds )
|
|
{
|
|
if ( !m_NumNodesInTree )
|
|
{
|
|
// Special case: first node inserted
|
|
NodeRef root = NewNode( pData, bounds );
|
|
Assert( root.Index() == 0 ); // The root should always be at index zero
|
|
return;
|
|
}
|
|
|
|
// Find the closest leaf to the inserted sphere, and record the path to it
|
|
float minDist = FLT_MAX;
|
|
const Vector &pos = bounds->AsVector3D();
|
|
if ( m_PrevInsertedNode >= 0 )
|
|
{
|
|
// Use the last inserted node as an upper bound on minDist (~halves the cost of FindPathToClosestLeaf_R)
|
|
NodeRef node = Ref( m_PrevInsertedNode );
|
|
Assert( node.IsLeaf() );
|
|
if ( node.IsLeaf() )
|
|
{
|
|
float dist = ( node->bounds.AsVector3D() - pos ).Length();
|
|
minDist = MAX( dist*1.1f, dist + 0.001f ); // Add slop for paranoia's sake
|
|
}
|
|
}
|
|
CUtlVector< int > path;
|
|
float nodeDist = ( Ref( 0 )->bounds.AsVector3D() - pos ).Length();
|
|
FindPathToClosestLeaf_R( Ref( 0 ), pos, nodeDist, minDist, path );
|
|
Assert( path.Count() && path[ path.Count() - 1 ] == 0 ); // All paths lead to the root!
|
|
int bestSibling = 0;
|
|
|
|
/* TODO: see if this could improve the tree noticeably, without increasing tree-building time too much (reuse the old ChooseInsertionStrategy code)
|
|
// Now determine where best to insert the sphere along the recorded path, minimizing summed node volume growth
|
|
// (if the sphere is much bigger than the leaf it was paired with, we'll insert it higher up the tree).
|
|
float minCost = FLT_MAX;
|
|
for ( int i = 0; i < path.Count(); i++ )
|
|
{
|
|
NodeRef &sibling = Ref( path[ i ] );
|
|
// NOTE: Can't rebalance the tree if you pair with a depth 3+ sibling
|
|
if ( ( sibling->maxDepth <= 2 ) && ( ComputePairingCost2( sibling, bounds, path ) < minCost ) )
|
|
bestSibling = i;
|
|
} */
|
|
|
|
// Insert alongside the chosen sibling - duplicate the sibling and add this copy plus the new node to the original sibling:
|
|
NodeRef sibling = Ref( path[ bestSibling ] );
|
|
NodeRef newSibling = CopyNode( sibling );
|
|
NodeRef newNode = NewNode( pData, bounds );
|
|
SetNodeChildren( sibling, newNode, newSibling );
|
|
RebalanceSubtrees( sibling, true );
|
|
|
|
// Remember the last node we inserted
|
|
m_PrevInsertedNode = newNode.Index();
|
|
|
|
for ( int i = ( bestSibling + 1 ); i < path.Count(); i++ )
|
|
{
|
|
// Update each node's status whose descendants were modified, and rebalance it:
|
|
NodeRef &sibling = Ref( path[ i ] );
|
|
SetNodeChildren( sibling, sibling.Deep(), sibling.Shallow() );
|
|
RebalanceSubtrees( sibling, true );
|
|
}
|
|
|
|
//ParanoidBalanceCheck_R( Ref( 0 ) ); // Use this to help debug Insert/Remove issues
|
|
}
|
|
|
|
inline void CUtlSphereTree::FindPathToClosestLeaf_R( NodeRef node, const Vector &pos, const float nodeDist, float &minDist, CUtlVector< int > &path ) const
|
|
{
|
|
float prevMinDist = minDist;
|
|
|
|
if ( node.IsLeaf() )
|
|
{
|
|
// TODO: should we use radius here? pos could be more 'central' in a larger sphere, while being closer to the centre of a smaller sphere...
|
|
if ( nodeDist >= minDist )
|
|
return;
|
|
// New closest leaf! Start the path again:
|
|
path.RemoveAll();
|
|
minDist = nodeDist;
|
|
}
|
|
else
|
|
{
|
|
float deepDist = ( node.Deep()->bounds.AsVector3D() - pos ).Length();
|
|
float shallowDist = ( node.Shallow()->bounds.AsVector3D() - pos ).Length();
|
|
if ( ( deepDist - node.Deep()->bounds.w ) < minDist )
|
|
FindPathToClosestLeaf_R( node.Deep(), pos, deepDist, minDist, path );
|
|
if ( ( shallowDist - node.Shallow()->bounds.w ) < minDist )
|
|
FindPathToClosestLeaf_R( node.Shallow(), pos, shallowDist, minDist, path );
|
|
}
|
|
|
|
if ( minDist < prevMinDist )
|
|
{
|
|
// If minDist improved, we're on the path to the closest leaf, so add ourself to the path:
|
|
path.AddToTail( node.Index() );
|
|
}
|
|
}
|
|
|
|
inline void CUtlSphereTree::Remove( const void *pData, const Sphere_t *bounds )
|
|
{
|
|
Assert( m_NumNodesInTree );
|
|
if ( !m_NumNodesInTree )
|
|
return; // Empty tree
|
|
|
|
NodeRef root = Ref( 0 );
|
|
if ( m_NumNodesInTree == 1 )
|
|
{
|
|
// Special case: the last node to remove is the root, making the tree empty
|
|
Assert( NodeHasData( root, pData, bounds ) );
|
|
if ( NodeHasData( root, pData, bounds ) )
|
|
FreelistPush( root );
|
|
return;
|
|
}
|
|
|
|
// Recurse to find the target node and remove it
|
|
int deletedNode;
|
|
deletedNode = Remove_R( root, pData, bounds );
|
|
Assert( deletedNode != 0 );
|
|
//ParanoidBalanceCheck_R( root ); // Use this to help debug Insert/Remove issues
|
|
}
|
|
|
|
inline int CUtlSphereTree::Remove_R( NodeRef &node, const void *pData, const Sphere_t *bounds )
|
|
{
|
|
// NOTE: it's ok to cache deepChild+shallowChild refs here because no items are removed from m_Nodes during this recursion
|
|
NodeRef deepChild = node.Deep(), shallowChild = node.Shallow();
|
|
|
|
// Is the data owned by one of our direct children?
|
|
for ( int i = 0; i < 2; i++ )
|
|
{
|
|
NodeRef &child = ( i ? shallowChild : deepChild ), otherChild = ( i ? deepChild : shallowChild );
|
|
if ( NodeHasData( child, pData, bounds ) )
|
|
{
|
|
// Found our target - replace node with the other child
|
|
*node = *otherChild;
|
|
FreelistPush( child );
|
|
FreelistPush( otherChild );
|
|
return child.Index();
|
|
}
|
|
}
|
|
|
|
// Recurse further to find the owner
|
|
int deletedNodeIndex = -1;
|
|
if ( !deepChild.IsLeaf() )
|
|
deletedNodeIndex = Remove_R( deepChild, pData, bounds );
|
|
if ( ( deletedNodeIndex < 0 ) && !shallowChild.IsLeaf() )
|
|
deletedNodeIndex = Remove_R( shallowChild, pData, bounds );
|
|
|
|
if ( deletedNodeIndex >= 0 )
|
|
{
|
|
// One of this node's sub-trees has changed, so update its status and rebalance the tree
|
|
SetNodeChildren( node, deepChild, shallowChild );
|
|
RebalanceSubtrees( node, false );
|
|
}
|
|
|
|
return deletedNodeIndex;
|
|
}
|
|
|
|
inline bool CUtlSphereTree::NodeHasData( const NodeRef &node, const void *pData, const Sphere_t *bounds )
|
|
{
|
|
// NOTE: this allows for pData being NULL or the same pData value being used with multiple spheres
|
|
// (though results are undefined if you insert the exact same pData/bounds pair twice)
|
|
return ( ( node->pData == pData ) && ( node->bounds == *bounds ) );
|
|
}
|
|
|
|
inline void CUtlSphereTree::RebalanceSubtrees( NodeRef &node, bool bInserting )
|
|
{
|
|
if ( IsSubtreeBalanced( node ) )
|
|
return;
|
|
|
|
// ShallowChild needs to be paired with a node from the deepChild subtree in order to rebalance the tree.
|
|
// We can either pair it with a 'nephew' (one of the children of deepChild) or a great-nephew (one of the
|
|
// children of the nephews). Not all choices result in a balanced tree, so we first determine which
|
|
// choices are valid and then we choose the one with the lowest 'cost'.
|
|
NodeRef shallowChild = node.Shallow(), deepChild = node.Deep();
|
|
Assert( IsSubtreeBalanced( deepChild ) );
|
|
|
|
// Put the shallower nephew first
|
|
NodeRef nephews[2] = { deepChild.Shallow(), deepChild.Deep() };
|
|
Assert( nephews[1]->maxDepth == ( shallowChild->maxDepth + 1 ) );
|
|
Assert( IsSubtreeBalanced( nephews[0] ) );
|
|
Assert( IsSubtreeBalanced( nephews[1] ) );
|
|
|
|
// Put the shallower great-nephew pair first, and within each pair put the shallower great-nephew first
|
|
NodeRef greatNephews[4] = { nephews[0].Shallow(), nephews[0].Deep(),
|
|
nephews[1].Shallow(), nephews[1].Deep() };
|
|
int nephewMask = 0x3, greatNephewMask = 0xF;
|
|
if ( nephews[0]->maxDepth == nephews[1]->maxDepth )
|
|
{
|
|
Assert( !bInserting );
|
|
Assert( nephews[0]->maxDepth == ( shallowChild->maxDepth + 1 ) );
|
|
|
|
// The nephews are both fair game for pairing with shallowChild. Determine which great-nephews are viable:
|
|
for ( int i = 0; i < 2; i++ )
|
|
{
|
|
// If a nephew is uneven, only the shallower great-nephew may be paired with shallowChild
|
|
if ( greatNephews[ i*2 ]->maxDepth < greatNephews[ i*2 + 1 ]->maxDepth )
|
|
greatNephewMask ^= 1 << ( i*2 + 1 );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Assert( nephews[0]->maxDepth == shallowChild->maxDepth );
|
|
|
|
// Only the shallower nephew and the deeper nephew's children may be paired with shallowChild
|
|
nephewMask = ( 1 << 0 );
|
|
greatNephewMask = ( 1 << 2 ) | ( 1 << 3 );
|
|
}
|
|
|
|
// Now use a cost metric to determine the best choice
|
|
float minCost = FLT_MAX;
|
|
int bestIndex = -1;
|
|
bool nephewBest = true;
|
|
for ( int i = 0; i < 2; i++ )
|
|
{
|
|
if ( nephewMask & (1<<i) )
|
|
{
|
|
float cost = ComputePairingCost( shallowChild, nephews[i], nephews[i^1], NULL );
|
|
if ( cost < minCost )
|
|
{
|
|
minCost = cost;
|
|
bestIndex = i;
|
|
}
|
|
}
|
|
}
|
|
for ( int i = 0; i < 4; i++ )
|
|
{
|
|
if ( greatNephewMask & (1<<i) )
|
|
{
|
|
int otherNephew = ( i >> 1 ) ^ 1;
|
|
float cost = ComputePairingCost( shallowChild, greatNephews[i], nephews[otherNephew], &greatNephews[i^1] );
|
|
if ( cost < minCost )
|
|
{
|
|
minCost = cost;
|
|
bestIndex = i;
|
|
nephewBest = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
if ( nephewBest )
|
|
{
|
|
// Pair shallowChild with one of its nephews (reuse deepChild as their parent), and pull the other nephew up the tree
|
|
SetNodeChildren( deepChild, shallowChild, nephews[ bestIndex ] );
|
|
SetNodeChildren( node, deepChild, nephews[ bestIndex ^ 1 ] );
|
|
}
|
|
else
|
|
{
|
|
// Pair shallowChild with one of its great-nephews, and pair the remaining great-nephew with the other nephew
|
|
NodeRef &nephew = nephews[ bestIndex >> 1 ];
|
|
NodeRef &otherNephew = nephews[ ( bestIndex >> 1 ) ^ 1 ];
|
|
SetNodeChildren( deepChild, otherNephew, greatNephews[ bestIndex ^ 1 ] );
|
|
SetNodeChildren( nephew, shallowChild, greatNephews[ bestIndex ] );
|
|
SetNodeChildren( node, deepChild, nephew );
|
|
}
|
|
|
|
// The tree under 'node' should now be balanced
|
|
Assert( IsSubtreeBalanced( node ) );
|
|
Assert( IsSubtreeBalanced( node.Deep() ) );
|
|
Assert( IsSubtreeBalanced( node.Shallow() ) );
|
|
}
|
|
|
|
inline bool CUtlSphereTree::IsSubtreeBalanced( const NodeRef &node ) const
|
|
{
|
|
if ( !node.IsLeaf() )
|
|
{
|
|
Assert( node.Deep()->maxDepth >= node.Shallow()->maxDepth );
|
|
if ( node.Deep()->maxDepth > ( node.Shallow()->maxDepth + 1 ) )
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
inline float CUtlSphereTree::Cost( const Sphere_t &sphere ) const
|
|
{
|
|
// We optimize the tree based on total resulting bounding sphere volume:
|
|
// - this minimizes average cost in the space occupied by the tree, i.e. the number of tests you have
|
|
// to perform to satisfy a given query; on entering a sphere, you need to do 2 tests to pick a child,
|
|
// so total cost is 'two tests' times the total volume of (non-leaf) nodes
|
|
// - NOTE: this metric works for input spheres which overlap or fully contain each other
|
|
|
|
// TODO: Volume is the best criterion for a point test ("which leaves does a point intersect?"),
|
|
// but area seems better for ray tests and radius for plane tests - are there pathological
|
|
// cases where these 3 metrics would differ enough to make Insert choose a different pairing?
|
|
// Should be easy to change this and test perf differences on real data...
|
|
return sphere.w*sphere.w*sphere.w;
|
|
}
|
|
|
|
inline Sphere_t CUtlSphereTree::AddSpheres( const Sphere_t &pA, const Sphere_t &pB ) const
|
|
{
|
|
const Vector &aPos = pA.AsVector3D(), &bPos = pB.AsVector3D();
|
|
float aRad = pA.w, bRad = pB.w;
|
|
|
|
Vector delta = aPos - bPos;
|
|
float dist = delta.NormalizeInPlace();
|
|
|
|
if ( dist < fabsf( aRad - bRad ) )
|
|
{
|
|
// One sphere entirely contains the other
|
|
return ( ( aRad > bRad ) ? pA : pB );
|
|
}
|
|
|
|
// Figure out the new sphere's centre+radius, using 'mathematics'
|
|
float newRad = ( aRad + bRad + dist ) / 2;
|
|
Vector newPos = bPos + delta*( newRad - bRad );
|
|
|
|
return Sphere_t( newPos.x, newPos.y, newPos.z, newRad );
|
|
}
|
|
|
|
inline float CUtlSphereTree::ComputePairingCost( const NodeRef &pair1A, const NodeRef &pair1B, const NodeRef &pair2A, const NodeRef *pair2B ) const
|
|
{
|
|
Sphere_t pair1Sphere = AddSpheres( pair1A->bounds, pair1B->bounds );
|
|
Sphere_t pair2Sphere = pair2B ? AddSpheres( pair2A->bounds, (*pair2B)->bounds ) : pair2A->bounds;
|
|
Sphere_t outerSphere = AddSpheres( pair1Sphere, pair2Sphere );
|
|
return ( Cost( pair1Sphere ) + Cost( pair2Sphere ) + Cost( outerSphere ) );
|
|
}
|
|
|
|
inline void CUtlSphereTree::ParanoidBalanceCheck_R( const NodeRef &node ) const
|
|
{
|
|
if ( node.IsLeaf() )
|
|
return;
|
|
Assert( IsSubtreeBalanced( node ) );
|
|
ParanoidBalanceCheck_R( node.Deep() );
|
|
ParanoidBalanceCheck_R( node.Shallow() );
|
|
}
|
|
|
|
inline void CUtlSphereTree::GetLeavesUnderNode_R( const NodeRef &node, CUtlVector< void * > &leaves, int &count ) const
|
|
{
|
|
if ( node.IsLeaf() )
|
|
{
|
|
if ( count > 0 )
|
|
leaves.AddToTail( (void *)node->pData );
|
|
count--;
|
|
}
|
|
else
|
|
{
|
|
GetLeavesUnderNode_R( node.Deep(), leaves, count );
|
|
GetLeavesUnderNode_R( node.Shallow(), leaves, count );
|
|
}
|
|
}
|
|
|
|
inline int CUtlSphereTree::IntersectWithRay( Vector &rayStart, Vector &rayDelta, CUtlVector< void * > &result, int maxResults, CUtlSphereTree::Cut *cut ) const
|
|
{
|
|
// Returns a list of the indices of the (leaf) nodes intersecting the given ray
|
|
// TODO: optionally return the spheres in intersection order, from rayStart to (rayStart+rayDelta)
|
|
|
|
result.RemoveAll();
|
|
if ( !m_NumNodesInTree )
|
|
return 0; // Empty tree
|
|
|
|
// Default to searching the whole tree
|
|
Cut completeCut( this );
|
|
if ( cut == NULL )
|
|
cut = &completeCut;
|
|
|
|
Assert( cut->m_pTree == this );
|
|
if ( cut->m_pTree != this )
|
|
return 0;
|
|
|
|
maxResults = maxResults ? maxResults : 0x7FFFFFFF;
|
|
int count = maxResults;
|
|
for ( int i = 0; i < cut->m_NodeRefs.Count(); i++ )
|
|
{
|
|
IntersectWithRay_R( Ref( cut->m_NodeRefs[ i ] ), rayStart, rayDelta, result, count );
|
|
}
|
|
return ( maxResults - count );
|
|
}
|
|
|
|
// extern int gNumCalls; // Useful for accurately measuring the cost of intersection tests (average num calls should be a low multiple of average tree depth)
|
|
|
|
inline void CUtlSphereTree::IntersectWithRay_R( const NodeRef &node, Vector &rayStart, Vector &rayDelta, CUtlVector< void * > &result, int & count ) const
|
|
{
|
|
// TODO: pull the intersection test outside the recursive call, as is done in IntersectWithSphere_R
|
|
//gNumCalls++;
|
|
|
|
float hit1, hit2;
|
|
Sphere_t &sphere = node->bounds;
|
|
// TODO: In cases where a node overlaps heavily with its children, might it be a win to skip this test
|
|
// and assume it passes? (i.e. recurse to the children and test their (similar) bounds instead)
|
|
// Might be a clearer win in a SIMD-ified version of the tree (where we can simply omit this node)
|
|
if ( !IntersectRayWithSphere( rayStart, rayDelta, sphere.AsVector3D(), sphere.w, &hit1, &hit2 ) )
|
|
{
|
|
// No intersection
|
|
return;
|
|
}
|
|
|
|
if ( node.IsLeaf() )
|
|
{
|
|
// Leaf node, add it to the result
|
|
if ( count > 0 )
|
|
result.AddToTail( (void *)node->pData );
|
|
count--;
|
|
}
|
|
else
|
|
{
|
|
// Non-leaf, recurse to child nodes
|
|
IntersectWithRay_R( node.Deep(), rayStart, rayDelta, result, count );
|
|
IntersectWithRay_R( node.Shallow(), rayStart, rayDelta, result, count );
|
|
}
|
|
}
|
|
|
|
inline int CUtlSphereTree::IntersectWithSphere( const Sphere_t &sphere, bool bPartial, CUtlVector< void * > &result, int maxResults, CUtlSphereTree::Cut *cut ) const
|
|
{
|
|
// Returns a list of the indices of the (leaf) nodes intersecting the given sphere
|
|
// TODO: optionally return the spheres in order of distance from the sphere centre
|
|
|
|
result.RemoveAll();
|
|
if ( !m_NumNodesInTree )
|
|
return 0; // Empty tree
|
|
|
|
// Default to searching the whole tree
|
|
Cut completeCut( this );
|
|
if ( cut == NULL )
|
|
cut = &completeCut;
|
|
|
|
Assert( cut->m_pTree == this );
|
|
if ( cut->m_pTree != this )
|
|
return 0;
|
|
|
|
maxResults = maxResults ? maxResults : 0x7FFFFFFF;
|
|
int count = maxResults;
|
|
float distSq;
|
|
for ( int i = 0; i < cut->m_NodeRefs.Count(); i++ )
|
|
{
|
|
if ( SpheresIntersect( sphere, Ref( cut->m_NodeRefs[ i ] )->bounds, distSq ) )
|
|
IntersectWithSphere_R( Ref( cut->m_NodeRefs[ i ] ), sphere, bPartial, result, count, distSq );
|
|
}
|
|
return ( maxResults - count );
|
|
}
|
|
|
|
inline void CUtlSphereTree::IntersectWithSphere_R( const NodeRef &node, const Sphere_t &sphere, const bool bPartial, CUtlVector< void * > &result, int &count, const float distSq ) const
|
|
{
|
|
//gNumCalls++;
|
|
if ( node.IsLeaf() )
|
|
{
|
|
// Leaf node
|
|
if ( !bPartial )
|
|
{
|
|
// sphere must fully contain bounds
|
|
Sphere_t &bounds = node->bounds;
|
|
if ( sphere.w < bounds.w )
|
|
return;
|
|
float maxDist = sphere.w - bounds.w;
|
|
if ( distSq > ( maxDist*maxDist ) )
|
|
return;
|
|
}
|
|
// Add it to the result
|
|
if ( count > 0 )
|
|
result.AddToTail( (void *)node->pData );
|
|
count--;
|
|
}
|
|
else
|
|
{
|
|
// Non-leaf, recurse to child nodes (do the sphere test here to minimize function calls)
|
|
// TODO: In cases where a node overlaps heavily with its children, might it be a win to skip this test
|
|
// and assume it passes? (i.e. recurse to the children and test their (similar) bounds instead)
|
|
// Might be a clearer win in a SIMD-ified version of the tree (where we can simply omit the parent node)
|
|
float deepDistSq, shallowDistSq;
|
|
if ( SpheresIntersect( sphere, node.Deep()->bounds, deepDistSq ) )
|
|
IntersectWithSphere_R( node.Deep(), sphere, bPartial, result, count, deepDistSq );
|
|
if ( SpheresIntersect( sphere, node.Shallow()->bounds, shallowDistSq ) )
|
|
IntersectWithSphere_R( node.Shallow(), sphere, bPartial, result, count, shallowDistSq );
|
|
}
|
|
}
|
|
|
|
inline bool CUtlSphereTree::SpheresIntersect( const Sphere_t &A, const Sphere_t &B, float &distSq ) const
|
|
{
|
|
Vector delta = A.AsVector3D() - B.AsVector3D();
|
|
float radiusSum = A.w + B.w;
|
|
distSq = delta.LengthSqr();
|
|
return ( distSq <= ( radiusSum*radiusSum ) );
|
|
}
|
|
|
|
inline void CUtlSphereTree::CutByPlanes( Cut *outputCut, const Cut *inputCut, int cutFlags, Plane_t *planes, int numPlanes ) const
|
|
{
|
|
// TODO: might be faster if this code called one of 4 variants of CutByPlanes_R (i.e. templatize to make 'cutFlags' a compile-time constant)
|
|
|
|
if ( !m_NumNodesInTree )
|
|
return; // Empty tree
|
|
|
|
Assert( outputCut );
|
|
if ( !outputCut )
|
|
return;
|
|
|
|
// Default to searching the whole tree
|
|
Cut completeCut( this );
|
|
if ( inputCut == NULL )
|
|
inputCut = &completeCut;
|
|
|
|
if ( inputCut == outputCut )
|
|
{
|
|
// Copy the input/output Cut's contents to a temporary Cut so we don't have to special-case much code
|
|
// TODO: it would be quicker to modify the input Cut in-place (given that node order is not important,
|
|
// you can process nodes in order, addtotail new items and replace removed items with the last entry)
|
|
completeCut.m_NodeRefs.RemoveAll();
|
|
completeCut.m_NodeRefs.AddMultipleToTail( inputCut->m_NodeRefs.Count(), inputCut->m_NodeRefs.Base() );
|
|
inputCut = &completeCut;
|
|
}
|
|
|
|
// Clear the output before we start
|
|
outputCut->m_NodeRefs.RemoveAll();
|
|
|
|
for ( int i = 0; i < inputCut->m_NodeRefs.Count(); i++ )
|
|
{
|
|
CutByPlanes_R( outputCut, Ref( inputCut->m_NodeRefs[ i ] ), cutFlags, planes, numPlanes );
|
|
}
|
|
}
|
|
|
|
inline void CUtlSphereTree::CutByPlanes_R( Cut *outputCut, const NodeRef &node, int cutFlags, Plane_t *planes, int numPlanes ) const
|
|
{
|
|
// When refining a Cut w.r.t a set of intersection primitives (here planes), there are two flags to control how this is done:
|
|
// PARTIAL_INTERSECTIONS - nodes partially passing the intersection test (partially in front of the plane) are included
|
|
// these are only leaf nodes; non-leaf nodes are subdivided if found to be 'partial' (since we do
|
|
// not want both a node and its parent to be present in the result)
|
|
// OR_INTERSECTIONS - in 'OR' mode, a given node needs to pass the intersection test for ANY intersection primitive
|
|
// - in 'AND' mode (the default), it must pass the intersection test for ALL intersection primitives
|
|
// (for planes, AND is useful to find 'nodes INSIDE a region' and OR to find 'nodes OUTSIDE a region')
|
|
//
|
|
// When recursing, we follow these rules for each node:
|
|
// AND:
|
|
// - fail any plane -> FAIL, end recursion
|
|
// - pass all planes -> PASS, add node
|
|
// - otherwise (partial for any planes)
|
|
// o non-leaf -> recurse to children
|
|
// o leaf -> add node if allow partial
|
|
// OR:
|
|
// - fail all planes -> FAIL, end recursion
|
|
// - pass any planes -> PASS, add node
|
|
// - partial for any planes (fail for others)
|
|
// o non-leaf -> recurse to children
|
|
// o leaf -> add node if allow partial
|
|
|
|
Cut::CutTestResult result = CutByPlanes_Test( node, cutFlags, planes, numPlanes );
|
|
|
|
switch( result )
|
|
{
|
|
case Cut::CUT_TEST_PASS:
|
|
// The node fully passes the test, add it to the output Cut
|
|
outputCut->m_NodeRefs.AddToTail( node.Index() );
|
|
return;
|
|
|
|
case Cut::CUT_TEST_FAIL:
|
|
// The node fully fails the test, stop recursion
|
|
return;
|
|
|
|
case Cut::CUT_TEST_PARTIAL:
|
|
// The node partially passes the test
|
|
if ( node.IsLeaf() )
|
|
{
|
|
// Add to the result if partial intersections are desired
|
|
if ( cutFlags & Cut::PARTIAL_INTERSECTIONS )
|
|
outputCut->m_NodeRefs.AddToTail( node.Index() );
|
|
}
|
|
else
|
|
{
|
|
// Recurse to the child nodes (since we can't know yet whether they are pass, fail or partial)
|
|
CutByPlanes_R( outputCut, node.Deep(), cutFlags, planes, numPlanes );
|
|
CutByPlanes_R( outputCut, node.Shallow(), cutFlags, planes, numPlanes );
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
|
|
inline CUtlSphereTree::Cut::CutTestResult CUtlSphereTree::CutByPlanes_Test( const NodeRef &node, int cutFlags, Plane_t *planes, int numPlanes ) const
|
|
{
|
|
Vector ¢re = node->bounds.AsVector3D();
|
|
float radius = node->bounds.w;
|
|
|
|
int numPass = 0, numFail = 0, numPartial = 0;
|
|
for ( int i = 0; i < numPlanes; i++ )
|
|
{
|
|
float dist = centre.Dot( planes[ i ].AsVector3D() ) - planes[ i ].w;
|
|
if ( dist >= radius )
|
|
{
|
|
// Node is fully in front
|
|
if ( cutFlags & Cut::OR_INTERSECTIONS )
|
|
return Cut::CUT_TEST_PASS; // A single pass is enough for 'OR' mode
|
|
numPass++;
|
|
}
|
|
else if ( dist < -radius )
|
|
{
|
|
// Node is fully behind
|
|
if ( !( cutFlags & Cut::OR_INTERSECTIONS ) )
|
|
return Cut::CUT_TEST_FAIL; // A single fail is enough for 'AND' mode
|
|
numFail++;
|
|
}
|
|
else
|
|
{
|
|
// Node is partially in front
|
|
numPartial++;
|
|
}
|
|
}
|
|
|
|
if ( cutFlags & Cut::OR_INTERSECTIONS ) // 'OR' mode
|
|
{
|
|
Assert( numPass == 0 ); // Should have earlied-out above, otherwise
|
|
return ( numPartial == 0 ) ? Cut::CUT_TEST_FAIL : Cut::CUT_TEST_PARTIAL;
|
|
}
|
|
else // 'AND' mode
|
|
{
|
|
Assert( numFail == 0 ); // Should have earlied-out above, otherwise
|
|
return ( numPass == numPlanes ) ? Cut::CUT_TEST_PASS : Cut::CUT_TEST_PARTIAL;
|
|
}
|
|
}
|
|
|
|
inline void CUtlSphereTree::MeasureTreeQuality( float &averageDepth, float &idealDepth, float &averageRedundancy, float &averageSphereTestCost, float sphereTestRadius ) const
|
|
{
|
|
int numLeaves = ( m_NumNodesInTree + 1 ) / 2;
|
|
float pathRedundancy = 0;
|
|
averageRedundancy = 0;
|
|
averageDepth = 0;
|
|
float pathProbability = 1;
|
|
averageSphereTestCost = 0;
|
|
int steps = 0;
|
|
|
|
// Measure depth and redundancy for all paths through the tree:
|
|
MeasureTreeQuality_R( Ref( 0 ), steps, averageDepth, averageRedundancy, pathRedundancy, averageSphereTestCost, pathProbability, sphereTestRadius );
|
|
|
|
// The more efficient a perfectly tree, the closer its averageRedundancy to 0.0 (1.0 is the worst case, 0.5 is normal)
|
|
// NOTE: parent-child redundancy correlates well with sibling overlap
|
|
averageRedundancy /= MAX( 1, numLeaves );
|
|
|
|
// For a well-balanced tree, averageDepth ~= idealDepth
|
|
averageDepth /= MAX( 1, numLeaves );
|
|
// Compute idealDepth for comparison:
|
|
idealDepth = ( log( m_NumNodesInTree + 1.0f ) / log( 2.0f ) ) - 1;
|
|
int baseDepth = (int)floor( idealDepth );
|
|
int numFullTreeNodes = ( 1 << baseDepth )*2 - 1;
|
|
int numHighDepthLeaves = m_NumNodesInTree - numFullTreeNodes;
|
|
int numLowDepthLeaves = numLeaves - numHighDepthLeaves;
|
|
idealDepth = ( numLowDepthLeaves*baseDepth + numHighDepthLeaves*( baseDepth + 1 ) ) / (float)numLeaves;
|
|
}
|
|
|
|
inline void CUtlSphereTree::MeasureTreeQuality_R( const NodeRef &node, const int steps, float &averageDepth, float &averageRedundancy, const float pathRedundancy, float &averageSphereTestCost, const float pathProbability, const float sphereTestRadius ) const
|
|
{
|
|
averageSphereTestCost += pathProbability;
|
|
if ( !node.IsLeaf() )
|
|
{
|
|
float deepHitProbability = ComputeIntersectionProbability( node, node.Deep(), sphereTestRadius ), shallowHitProbability = ComputeIntersectionProbability( node, node.Shallow(), sphereTestRadius );
|
|
MeasureTreeQuality_R( node.Deep(), steps+1, averageDepth, averageRedundancy, ( pathRedundancy + ComputeRedundancy( node, node.Deep() ) ), averageSphereTestCost, pathProbability*deepHitProbability, sphereTestRadius );
|
|
MeasureTreeQuality_R( node.Shallow(), steps+1, averageDepth, averageRedundancy, ( pathRedundancy + ComputeRedundancy( node, node.Shallow() ) ), averageSphereTestCost, pathProbability*shallowHitProbability, sphereTestRadius );
|
|
}
|
|
else
|
|
{
|
|
// Leaf
|
|
averageDepth += steps;
|
|
averageRedundancy += pathRedundancy / steps;
|
|
}
|
|
}
|
|
|
|
inline float CUtlSphereTree::ComputeRedundancy( const NodeRef &parent, const NodeRef &child ) const
|
|
{
|
|
const Sphere_t &parentSphere = parent->bounds, &childSphere = child->bounds;
|
|
if ( parentSphere.w == 0 )
|
|
return 1;
|
|
return ( childSphere.w*childSphere.w*childSphere.w ) / ( parentSphere.w*parentSphere.w*parentSphere.w );
|
|
}
|
|
|
|
inline float CUtlSphereTree::ComputeIntersectionProbability( const NodeRef &parent, const NodeRef &child, const float sphereTestRadius ) const
|
|
{
|
|
// The probability of a test sphere intersecting a parent's child sphere, GIVEN that it has intersected the parent sphere:
|
|
// [NOTE: this math assumes that the child sphere touches the parent sphere's boundary, which is always true for CUtlSphereTree]
|
|
float totalRadius = ( parent->bounds.w + sphereTestRadius ), hitRadius = ( child->bounds.w + sphereTestRadius );
|
|
if ( totalRadius == 0 )
|
|
return 1;
|
|
return ( hitRadius*hitRadius*hitRadius ) / ( totalRadius*totalRadius*totalRadius );
|
|
}
|
|
|
|
|
|
|
|
// ===== CUtlSphereTree_Cut implementation =================================
|
|
|
|
inline CUtlSphereTree::Cut::Cut( const CUtlSphereTree *pTree )
|
|
{
|
|
// Initialize a Cut with an entire sphere tree (i.e. the root node)
|
|
Assert( pTree );
|
|
m_pTree = pTree;
|
|
if ( pTree->m_NumNodesInTree )
|
|
m_NodeRefs.AddToTail( 0 );
|
|
}
|
|
|
|
inline CUtlSphereTree::Cut::~Cut( void )
|
|
{
|
|
}
|
|
|
|
inline int CUtlSphereTree::Cut::GetLeaves( CUtlVector< void * > &leaves, int maxResults ) const
|
|
{
|
|
leaves.RemoveAll();
|
|
maxResults = maxResults ? maxResults : 0x7FFFFFFF;
|
|
int count = maxResults;
|
|
for ( int i = 0; i < m_NodeRefs.Count(); i++ )
|
|
{
|
|
m_pTree->GetLeavesUnderNode_R( m_pTree->Ref( m_NodeRefs[ i ] ), leaves, count );
|
|
}
|
|
return ( maxResults - count );
|
|
}
|
|
|
|
#endif // _utlspheretree_H_
|