38 #ifndef HPP_FCL_HIERARCHY_TREE_H 39 #define HPP_FCL_HIERARCHY_TREE_H 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);
143 typedef typename std::vector<NodeBase<BV>*>::iterator NodeVecIterator;
145 typename std::vector<NodeBase<BV>*>::const_iterator NodeVecConstIterator;
147 struct SortByMorton {
148 bool operator()(
const Node* a,
const Node* b)
const {
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);
230 Node* removeLeaf(Node*
const leaf);
234 void fetchLeaves(Node* root, std::vector<Node*>& leaves,
int depth = -1);
236 static size_t indexOf(Node* node);
239 Node* createNode(Node* parent,
const BV& bv,
void* data);
241 Node* createNode(Node* parent,
const BV& bv1,
const BV& bv2,
void* data);
243 Node* createNode(Node* parent,
void* data);
245 void deleteNode(Node* node);
247 void recurseDeleteNode(Node* node);
249 void recurseRefit(Node* node);
273 template <
typename BV>
278 template <
typename BV>
284 template <
typename BV>
unsigned int opath
Definition: hierarchy_tree.h:256
int topdown_level
decide which topdown algorithm to use
Definition: hierarchy_tree.h:266
Main namespace.
Definition: broadphase_bruteforce.h:44
uint32_t code
morton code for current BV
Definition: node_base.h:70
void balanceTopdown()
balance the tree from top
Definition: hierarchy_tree-inl.h:222
Node * getRoot() const
get the root of the tree
Definition: hierarchy_tree-inl.h:274
int bu_threshold
decide the depth to use expensive bottom-up algorithm
Definition: hierarchy_tree.h:269
void balanceIncremental(int iterations)
balance the tree in an incremental way
Definition: hierarchy_tree-inl.h:233
void refit()
refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a botto...
Definition: hierarchy_tree-inl.h:251
NodeBase< BV > Node
Definition: hierarchy_tree.h:59
size_t size() const
number of leaves in the tree
Definition: hierarchy_tree-inl.h:268
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...
Definition: hierarchy_tree-inl.h:50
Class for hierarchy tree structure.
Definition: hierarchy_tree.h:57
double FCL_REAL
Definition: data_types.h:65
size_t getMaxHeight() const
get the max height of the tree
Definition: hierarchy_tree-inl.h:193
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...
Definition: hierarchy_tree-inl.h:124
void clear()
Clear the tree.
Definition: hierarchy_tree-inl.h:107
void balanceBottomup()
balance the tree from bottom
Definition: hierarchy_tree-inl.h:210
void extractLeaves(const Node *root, std::vector< Node *> &leaves) const
extract all the leaves of the tree
Definition: hierarchy_tree-inl.h:257
size_t getMaxDepth() const
get the max depth of the tree
Definition: hierarchy_tree-inl.h:200
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 ...
Definition: hierarchy_tree-inl.h:952
void print(Node *root, int depth)
print the tree in a recursive way
Definition: hierarchy_tree-inl.h:286
dynamic AABB tree node
Definition: node_base.h:50
Node * insert(const BV &bv, void *data)
Insest a node.
Definition: hierarchy_tree-inl.h:89
void init(std::vector< Node *> &leaves, int level=0)
Initialize the tree by a set of leaves using algorithm with a given level.
Definition: hierarchy_tree-inl.h:68
int max_lookahead_level
Definition: hierarchy_tree.h:262
size_t n_leaves
Definition: hierarchy_tree.h:254
Node * root_node
Definition: hierarchy_tree.h:252
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
~HierarchyTree()
Definition: hierarchy_tree-inl.h:62
bool empty() const
Whether the tree is empty.
Definition: hierarchy_tree-inl.h:118
Node * free_node
Definition: hierarchy_tree.h:260
bool nodeBaseLess(NodeBase< BV > *a, NodeBase< BV > *b, int d)
Compare two nodes accoording to the d-th dimension of node center.
Definition: hierarchy_tree-inl.h:930