GCC Code Coverage Report

Line	Branch	Exec	Source
1			/*
2			* Software License Agreement (BSD License)
3			*
4			* Copyright (c) 2011-2014, Willow Garage, Inc.
5			* Copyright (c) 2014-2016, Open Source Robotics Foundation
6			* All rights reserved.
7			*
8			* Redistribution and use in source and binary forms, with or without
9			* modification, are permitted provided that the following conditions
10			* are met:
11			*
12			* * Redistributions of source code must retain the above copyright
13			* notice, this list of conditions and the following disclaimer.
14			* * Redistributions in binary form must reproduce the above
15			* copyright notice, this list of conditions and the following
16			* disclaimer in the documentation and/or other materials provided
17			* with the distribution.
18			* * Neither the name of Open Source Robotics Foundation nor the names of its
19			* contributors may be used to endorse or promote products derived
20			* from this software without specific prior written permission.
21			*
22			* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23			* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24			* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25			* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26			* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27			* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28			* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29			* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30			* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31			* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32			* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33			* POSSIBILITY OF SUCH DAMAGE.
34			*/
35
36			/** @author Jia Pan */
37
38			#ifndef COAL_HIERARCHY_TREE_H
39			#define COAL_HIERARCHY_TREE_H
40
41			#include <vector>
42			#include <map>
43			#include <functional>
44			#include <iostream>
45			#include "coal/warning.hh"
46			#include "coal/BV/AABB.h"
47			#include "coal/broadphase/detail/morton.h"
48			#include "coal/broadphase/detail/node_base.h"
49
50			namespace coal {
51
52			namespace detail {
53
54			/// @brief Class for hierarchy tree structure
55			template <typename BV>
56			class HierarchyTree {
57			public:
58			typedef NodeBase<BV> Node;
59
60			/// @brief Create hierarchy tree with suitable setting.
61			/// bu_threshold decides the height of tree node to start bottom-up
62			/// construction / optimization; topdown_level decides different methods to
63			/// construct tree in topdown manner. lower level method constructs tree with
64			/// better quality but is slower.
65			HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
66
67			~HierarchyTree();
68
69			/// @brief Initialize the tree by a set of leaves using algorithm with a given
70			/// level.
71			void init(std::vector<Node*>& leaves, int level = 0);
72
73			/// @brief Insest a node
74			Node* insert(const BV& bv, void* data);
75
76			/// @brief Remove a leaf node
77			void remove(Node* leaf);
78
79			/// @brief Clear the tree
80			void clear();
81
82			/// @brief Whether the tree is empty
83			bool empty() const;
84
85			/// @brief Updates a `leaf` node. A use case is when the bounding volume
86			/// of an object changes. Ensure every parent node has its bounding volume
87			/// expand or shrink to fit the bounding volumes of its children.
88			/// @note Strangely the structure of the tree may change even if the
89			/// bounding volume of the `leaf` node does not change. It is just
90			/// another valid tree of the same set of objects. This is because
91			/// update() works by first removing `leaf` and then inserting `leaf`
92			/// back. The structural change could be as simple as switching the
93			/// order of two leaves if the sibling of the `leaf` is also a leaf.
94			/// Or it could be more complicated if the sibling is an internal
95			/// node.
96			void update(Node* leaf, int lookahead_level = -1);
97
98			/// @brief update the tree when the bounding volume of a given leaf has
99			/// changed
100			bool update(Node* leaf, const BV& bv);
101
102			/// @brief update one leaf's bounding volume, with prediction
103			bool update(Node* leaf, const BV& bv, const Vec3s& vel, Scalar margin);
104
105			/// @brief update one leaf's bounding volume, with prediction
106			bool update(Node* leaf, const BV& bv, const Vec3s& vel);
107
108			/// @brief get the max height of the tree
109			size_t getMaxHeight() const;
110
111			/// @brief get the max depth of the tree
112			size_t getMaxDepth() const;
113
114			/// @brief balance the tree from bottom
115			void balanceBottomup();
116
117			/// @brief balance the tree from top
118			void balanceTopdown();
119
120			/// @brief balance the tree in an incremental way
121			void balanceIncremental(int iterations);
122
123			/// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change,
124			/// update the entire tree in a bottom-up manner
125			void refit();
126
127			/// @brief extract all the leaves of the tree
128			void extractLeaves(const Node* root, std::vector<Node*>& leaves) const;
129
130			/// @brief number of leaves in the tree
131			size_t size() const;
132
133			/// @brief get the root of the tree
134			Node* getRoot() const;
135
136			Node*& getRoot();
137
138			/// @brief print the tree in a recursive way
139			void print(Node* root, int depth);
140
141			private:
142			typedef typename std::vector<NodeBase<BV>*>::iterator NodeVecIterator;
143			typedef
144			typename std::vector<NodeBase<BV>*>::const_iterator NodeVecConstIterator;
145
146			struct SortByMorton {
147		208655	bool operator()(const Node* a, const Node* b) const {
148		208655	return a->code < b->code;
149			}
150			};
151
152			/// @brief construct a tree for a set of leaves from bottom -- very heavy way
153			void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend);
154
155			/// @brief construct a tree for a set of leaves from top
156			Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend);
157
158			/// @brief compute the maximum height of a subtree rooted from a given node
159			size_t getMaxHeight(Node* node) const;
160
161			/// @brief compute the maximum depth of a subtree rooted from a given node
162			void getMaxDepth(Node* node, size_t depth, size_t& max_depth) const;
163
164			/// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
165			/// topdown manner. During construction, first compute the best split axis as
166			/// the axis along with the longest AABB edge. Then compute the median of all
167			/// nodes' center projection onto the axis and using it as the split
168			/// threshold.
169			Node* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend);
170
171			/// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
172			/// topdown manner. During construction, first compute the best split
173			/// thresholds for different axes as the average of all nodes' center. Then
174			/// choose the split axis as the axis whose threshold can divide the nodes
175			/// into two parts with almost similar size. This construction is more
176			/// expensive then topdown_0, but also can provide tree with better quality.
177			Node* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend);
178
179			/// @brief init tree from leaves in the topdown manner (topdown_0 or
180			/// topdown_1)
181			void init_0(std::vector<Node*>& leaves);
182
183			/// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
184			/// for nodes which is of depth more than the maximum bits of the morton code,
185			/// we use bottomup method to construct the subtree, which is slow but can
186			/// construct tree with high quality.
187			void init_1(std::vector<Node*>& leaves);
188
189			/// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
190			/// for nodes which is of depth more than the maximum bits of the morton code,
191			/// we split the leaves into two parts with the same size simply using the
192			/// node index.
193			void init_2(std::vector<Node*>& leaves);
194
195			/// @brief init tree from leaves using morton code. It uses morton_2, i.e.,
196			/// for all nodes, we simply divide the leaves into parts with the same size
197			/// simply using the node index.
198			void init_3(std::vector<Node*>& leaves);
199
200			Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend,
201			const uint32_t& split, int bits);
202
203			Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend,
204			const uint32_t& split, int bits);
205
206			Node* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend);
207
208			/// @brief update one leaf node's bounding volume
209			void update_(Node* leaf, const BV& bv);
210
211			/// @brief sort node n and its parent according to their memory position
212			Node* sort(Node* n, Node*& r);
213
214			/// @brief Insert a leaf node and also update its ancestors. Maintain the
215			/// tree as a full binary tree (every interior node has exactly two children).
216			/// Furthermore, adjust the BV of interior nodes so that each parent's BV
217			/// tightly fits its children's BVs.
218			/// @param sub_root The root of the subtree into which we will insert the
219			// leaf node.
220			void insertLeaf(Node* const sub_root, Node* const leaf);
221
222			/// @brief Remove a leaf. Maintain the tree as a full binary tree (every
223			/// interior node has exactly two children). Furthermore, adjust the BV of
224			/// interior nodes so that each parent's BV tightly fits its children's BVs.
225			/// @note The leaf node itself is not deleted yet, but all the unnecessary
226			/// internal nodes are deleted.
227			/// @returns the root of the subtree containing the nodes whose BVs were
228			// adjusted.
229			Node* removeLeaf(Node* const leaf);
230
231			/// @brief Delete all internal nodes and return all leaves nodes with given
232			/// depth from root
233			void fetchLeaves(Node* root, std::vector<Node*>& leaves, int depth = -1);
234
235			static size_t indexOf(Node* node);
236
237			/// @brief create one node (leaf or internal)
238			Node* createNode(Node* parent, const BV& bv, void* data);
239
240			Node* createNode(Node* parent, const BV& bv1, const BV& bv2, void* data);
241
242			Node* createNode(Node* parent, void* data);
243
244			void deleteNode(Node* node);
245
246			void recurseDeleteNode(Node* node);
247
248			void recurseRefit(Node* node);
249
250			protected:
251			Node* root_node;
252
253			size_t n_leaves;
254
255			unsigned int opath;
256
257			/// This is a one Node cache, the reason is that we need to remove a node and
258			/// then add it again frequently.
259			Node* free_node;
260
261			int max_lookahead_level;
262
263			public:
264			/// @brief decide which topdown algorithm to use
265			int topdown_level;
266
267			/// @brief decide the depth to use expensive bottom-up algorithm
268			int bu_threshold;
269			};
270
271			/// @brief Compare two nodes accoording to the d-th dimension of node center
272			template <typename BV>
273			bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d);
274
275			/// @brief select from node1 and node2 which is close to a given query. 0 for
276			/// node1 and 1 for node2
277			template <typename BV>
278			size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
279			const NodeBase<BV>& node2);
280
281			/// @brief select from node1 and node2 which is close to a given query bounding
282			/// volume. 0 for node1 and 1 for node2
283			template <typename BV>
284			size_t select(const BV& query, const NodeBase<BV>& node1,
285			const NodeBase<BV>& node2);
286
287			} // namespace detail
288			} // namespace coal
289
290			#include "coal/broadphase/detail/hierarchy_tree-inl.h"
291
292			#endif
293