38 #ifndef HPP_FCL_HIERARCHY_TREE_H 39 #define HPP_FCL_HIERARCHY_TREE_H 45 #include "hpp/fcl/warning.hh" 56 template <
typename BV>
66 HierarchyTree(
int bu_threshold_ = 16,
int topdown_level_ = 0);
72 void init(std::vector<Node*>& leaves,
int level = 0);
75 Node*
insert(
const BV& bv,
void* data);
78 void remove(Node* leaf);
97 void update(Node* leaf,
int lookahead_level = -1);
101 bool update(Node* leaf,
const BV& bv);
107 bool update(Node* leaf,
const BV& bv,
const Vec3f& vel);
129 void extractLeaves(
const Node* root, std::vector<Node*>& leaves)
const;
140 void print(Node* root,
int depth);
154 void bottomup(
const NodeVecIterator lbeg,
const NodeVecIterator lend);
157 Node*
topdown(
const NodeVecIterator lbeg,
const NodeVecIterator lend);
163 void getMaxDepth(Node* node,
size_t depth,
size_t& max_depth)
const;
170 Node*
topdown_0(
const NodeVecIterator lbeg,
const NodeVecIterator lend);
178 Node*
topdown_1(
const NodeVecIterator lbeg,
const NodeVecIterator lend);
182 void init_0(std::vector<Node*>& leaves);
188 void init_1(std::vector<Node*>& leaves);
194 void init_2(std::vector<Node*>& leaves);
199 void init_3(std::vector<Node*>& leaves);
201 Node*
mortonRecurse_0(
const NodeVecIterator lbeg,
const NodeVecIterator lend,
202 const uint32_t& split,
int bits);
204 Node*
mortonRecurse_1(
const NodeVecIterator lbeg,
const NodeVecIterator lend,
205 const uint32_t& split,
int bits);
207 Node*
mortonRecurse_2(
const NodeVecIterator lbeg,
const NodeVecIterator lend);
210 void update_(Node* leaf,
const BV& bv);
213 Node*
sort(Node* n, Node*&
r);
221 void insertLeaf(Node*
const sub_root, Node*
const leaf);
234 void fetchLeaves(Node* root, std::vector<Node*>& leaves,
int depth = -1);
236 static size_t indexOf(Node* node);
241 Node*
createNode(Node* parent,
const BV& bv1,
const BV& bv2,
void* data);
273 template <
typename BV>
278 template <
typename BV>
284 template <
typename BV>
Node * createNode(Node *parent, const BV &bv, void *data)
create one node (leaf or internal)
void init_2(std::vector< Node *> &leaves)
init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more tha...
std::vector< NodeBase< BV > * >::iterator NodeVecIterator
int topdown_level
decide which topdown algorithm to use
uint32_t code
morton code for current BV
void balanceTopdown()
balance the tree from top
Node * mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend)
Node * topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend)
construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. During construction...
static size_t indexOf(Node *node)
Node * getRoot() const
get the root of the tree
void fetchLeaves(Node *root, std::vector< Node *> &leaves, int depth=-1)
Delete all internal nodes and return all leaves nodes with given depth from root. ...
Node * mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t &split, int bits)
int bu_threshold
decide the depth to use expensive bottom-up algorithm
void balanceIncremental(int iterations)
balance the tree in an incremental way
void refit()
refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a botto...
void recurseDeleteNode(Node *node)
size_t size() const
number of leaves in the tree
HierarchyTree(int bu_threshold_=16, int topdown_level_=0)
Create hierarchy tree with suitable setting. bu_threshold decides the height of tree node to start bo...
Class for hierarchy tree structure.
size_t getMaxHeight() const
get the max height of the tree
bool operator()(const Node *a, const Node *b) const
void update(Node *leaf, int lookahead_level=-1)
Updates a leaf node. A use case is when the bounding volume of an object changes. Ensure every parent...
void clear()
Clear the tree.
Node * topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend)
construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. During construction...
std::vector< NodeBase< BV > * >::const_iterator NodeVecConstIterator
void balanceBottomup()
balance the tree from bottom
Node * mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t &split, int bits)
void extractLeaves(const Node *root, std::vector< Node *> &leaves) const
extract all the leaves of the tree
void deleteNode(Node *node)
Node * sort(Node *n, Node *&r)
sort node n and its parent according to their memory position
void insertLeaf(Node *const sub_root, Node *const leaf)
Insert a leaf node and also update its ancestors. Maintain the tree as a full binary tree (every inte...
void recurseRefit(Node *node)
size_t getMaxDepth() const
get the max depth of the tree
size_t select(const NodeBase< BV > &query, const NodeBase< BV > &node1, const NodeBase< BV > &node2)
select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2 ...
Node * topdown(const NodeVecIterator lbeg, const NodeVecIterator lend)
construct a tree for a set of leaves from top
void print(Node *root, int depth)
print the tree in a recursive way
void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend)
construct a tree for a set of leaves from bottom – very heavy way
void init_3(std::vector< Node *> &leaves)
init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the ...
Node * insert(const BV &bv, void *data)
Insest a node.
void init(std::vector< Node *> &leaves, int level=0)
Initialize the tree by a set of leaves using algorithm with a given level.
Node * removeLeaf(Node *const leaf)
Remove a leaf. Maintain the tree as a full binary tree (every interior node has exactly two children)...
void init_1(std::vector< Node *> &leaves)
init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more tha...
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
void update_(Node *leaf, const BV &bv)
update one leaf node's bounding volume
bool empty() const
Whether the tree is empty.
bool nodeBaseLess(NodeBase< BV > *a, NodeBase< BV > *b, int d)
Compare two nodes accoording to the d-th dimension of node center.
void init_0(std::vector< Node *> &leaves)
init tree from leaves in the topdown manner (topdown_0 or topdown_1)