interval_tree.cpp
Go to the documentation of this file.
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 
38 #ifndef HPP_FCL_INTERVAL_TREE_INL_H
39 #define HPP_FCL_INTERVAL_TREE_INL_H
40 
42 
43 #include <algorithm>
44 
45 namespace hpp {
46 namespace fcl {
47 namespace detail {
48 
49 //==============================================================================
51  nil = new IntervalTreeNode;
52  nil->left = nil->right = nil->parent = nil;
53  nil->red = false;
54  nil->key = nil->high = nil->max_high =
55  -(std::numeric_limits<FCL_REAL>::max)();
56  nil->stored_interval = nullptr;
57 
58  root = new IntervalTreeNode;
59  root->parent = root->left = root->right = nil;
60  root->key = root->high = root->max_high =
61  (std::numeric_limits<FCL_REAL>::max)();
62  root->red = false;
63  root->stored_interval = nullptr;
64 
68  sizeof(it_recursion_node));
70  recursion_node_stack[0].start_node = nullptr;
71 }
72 
73 //==============================================================================
76  std::deque<IntervalTreeNode*> nodes_to_free;
77 
78  if (x != nil) {
79  if (x->left != nil) {
80  nodes_to_free.push_back(x->left);
81  }
82  if (x->right != nil) {
83  nodes_to_free.push_back(x->right);
84  }
85 
86  delete x;
87  while (nodes_to_free.size() > 0) {
88  x = nodes_to_free.back();
89  nodes_to_free.pop_back();
90  if (x->left != nil) {
91  nodes_to_free.push_back(x->left);
92  }
93  if (x->right != nil) {
94  nodes_to_free.push_back(x->right);
95  }
96  delete x;
97  }
98  }
99  delete nil;
100  delete root;
101  free(recursion_node_stack);
102 }
103 
104 //==============================================================================
107 
108  y = x->right;
109  x->right = y->left;
110 
111  if (y->left != nil) y->left->parent = x;
112 
113  y->parent = x->parent;
114 
115  if (x == x->parent->left)
116  x->parent->left = y;
117  else
118  x->parent->right = y;
119 
120  y->left = x;
121  x->parent = y;
122 
123  x->max_high =
124  std::max(x->left->max_high, std::max(x->right->max_high, x->high));
125  y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high));
126 }
127 
128 //==============================================================================
131 
132  x = y->left;
133  y->left = x->right;
134 
135  if (nil != x->right) x->right->parent = y;
136 
137  x->parent = y->parent;
138  if (y == y->parent->left)
139  y->parent->left = x;
140  else
141  y->parent->right = x;
142 
143  x->right = y;
144  y->parent = x;
145 
146  y->max_high =
147  std::max(y->left->max_high, std::max(y->right->max_high, y->high));
148  x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high));
149 }
150 
151 //==============================================================================
155 
156  z->left = z->right = nil;
157  y = root;
158  x = root->left;
159  while (x != nil) {
160  y = x;
161  if (x->key > z->key)
162  x = x->left;
163  else
164  x = x->right;
165  }
166  z->parent = y;
167  if ((y == root) || (y->key > z->key))
168  y->left = z;
169  else
170  y->right = z;
171 }
172 
173 //==============================================================================
175  while (x != root) {
176  x->max_high =
177  std::max(x->high, std::max(x->left->max_high, x->right->max_high));
178  x = x->parent;
179  }
180 }
181 
182 //==============================================================================
186  IntervalTreeNode* new_node;
187 
188  x = new IntervalTreeNode(new_interval);
190  fixupMaxHigh(x->parent);
191  new_node = x;
192  x->red = true;
193  while (x->parent->red) {
195  if (x->parent == x->parent->parent->left) {
196  y = x->parent->parent->right;
197  if (y->red) {
198  x->parent->red = true;
199  y->red = true;
200  x->parent->parent->red = true;
201  x = x->parent->parent;
202  } else {
203  if (x == x->parent->right) {
204  x = x->parent;
205  leftRotate(x);
206  }
207  x->parent->red = false;
208  x->parent->parent->red = true;
209  rightRotate(x->parent->parent);
210  }
211  } else {
212  y = x->parent->parent->left;
213  if (y->red) {
214  x->parent->red = false;
215  y->red = false;
216  x->parent->parent->red = true;
217  x = x->parent->parent;
218  } else {
219  if (x == x->parent->left) {
220  x = x->parent;
221  rightRotate(x);
222  }
223  x->parent->red = false;
224  x->parent->parent->red = true;
225  leftRotate(x->parent->parent);
226  }
227  }
228  }
229  root->left->red = false;
230  return new_node;
231 }
232 
233 //==============================================================================
236 
237  if (nil != (y = x->right)) {
238  while (y->left != nil) y = y->left;
239  return y;
240  } else {
241  y = x->parent;
242  while (x == y->right) {
243  x = y;
244  y = y->parent;
245  }
246  if (y == root) return nil;
247  return y;
248  }
249 }
250 
251 //==============================================================================
254 
255  if (nil != (y = x->left)) {
256  while (y->right != nil) y = y->right;
257  return y;
258  } else {
259  y = x->parent;
260  while (x == y->left) {
261  if (y == root) return nil;
262  x = y;
263  y = y->parent;
264  }
265  return y;
266  }
267 }
268 
269 //==============================================================================
271  if (x != nil) {
272  recursivePrint(x->left);
273  x->print(nil, root);
274  recursivePrint(x->right);
275  }
276 }
277 
278 //==============================================================================
280 
281 //==============================================================================
283  IntervalTreeNode* w;
284  IntervalTreeNode* root_left_node = root->left;
285 
286  while ((!x->red) && (root_left_node != x)) {
287  if (x == x->parent->left) {
288  w = x->parent->right;
289  if (w->red) {
290  w->red = false;
291  x->parent->red = true;
292  leftRotate(x->parent);
293  w = x->parent->right;
294  }
295  if ((!w->right->red) && (!w->left->red)) {
296  w->red = true;
297  x = x->parent;
298  } else {
299  if (!w->right->red) {
300  w->left->red = false;
301  w->red = true;
302  rightRotate(w);
303  w = x->parent->right;
304  }
305  w->red = x->parent->red;
306  x->parent->red = false;
307  w->right->red = false;
308  leftRotate(x->parent);
309  x = root_left_node;
310  }
311  } else {
312  w = x->parent->left;
313  if (w->red) {
314  w->red = false;
315  x->parent->red = true;
316  rightRotate(x->parent);
317  w = x->parent->left;
318  }
319  if ((!w->right->red) && (!w->left->red)) {
320  w->red = true;
321  x = x->parent;
322  } else {
323  if (!w->left->red) {
324  w->right->red = false;
325  w->red = true;
326  leftRotate(w);
327  w = x->parent->left;
328  }
329  w->red = x->parent->red;
330  x->parent->red = false;
331  w->left->red = false;
332  rightRotate(x->parent);
333  x = root_left_node;
334  }
335  }
336  }
337  x->red = false;
338 }
339 
340 //==============================================================================
342  IntervalTreeNode* node = recursiveSearch(root, ivl);
343  if (node) deleteNode(node);
344 }
345 
346 //==============================================================================
348  SimpleInterval* ivl) const {
349  if (node != nil) {
350  if (node->stored_interval == ivl) return node;
351 
352  IntervalTreeNode* left = recursiveSearch(node->left, ivl);
353  if (left != nil) return left;
354  IntervalTreeNode* right = recursiveSearch(node->right, ivl);
355  if (right != nil) return right;
356  }
357 
358  return nil;
359 }
360 
361 //==============================================================================
365  SimpleInterval* node_to_delete = z->stored_interval;
366 
367  y = ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z);
368  x = (y->left == nil) ? y->right : y->left;
369  if (root == (x->parent = y->parent)) {
370  root->left = x;
371  } else {
372  if (y == y->parent->left) {
373  y->parent->left = x;
374  } else {
375  y->parent->right = x;
376  }
377  }
378 
381  if (y != z) {
382  y->max_high = -(std::numeric_limits<FCL_REAL>::max)();
383  y->left = z->left;
384  y->right = z->right;
385  y->parent = z->parent;
386  z->left->parent = z->right->parent = y;
387  if (z == z->parent->left)
388  z->parent->left = y;
389  else
390  z->parent->right = y;
391 
392  fixupMaxHigh(x->parent);
393  if (!(y->red)) {
394  y->red = z->red;
395  deleteFixup(x);
396  } else
397  y->red = z->red;
398  delete z;
399  } else {
400  fixupMaxHigh(x->parent);
401  if (!(y->red)) deleteFixup(x);
402  delete y;
403  }
404 
405  return node_to_delete;
406 }
407 
408 //==============================================================================
410 bool overlap(FCL_REAL a1, FCL_REAL a2, FCL_REAL b1, FCL_REAL b2) {
411  if (a1 <= b1) {
412  return (b1 <= a2);
413  } else {
414  return (a1 <= b2);
415  }
416 }
417 
418 //==============================================================================
419 std::deque<SimpleInterval*> IntervalTree::query(FCL_REAL low, FCL_REAL high) {
420  std::deque<SimpleInterval*> result_stack;
422  bool run = (x != nil);
423 
424  current_parent = 0;
425 
426  while (run) {
427  if (overlap(low, high, x->key, x->high)) {
428  result_stack.push_back(x->stored_interval);
430  }
431  if (x->left->max_high >= low) {
437  if (recursion_node_stack == nullptr) abort();
438  }
444  x = x->left;
445  } else
446  x = x->right;
447 
448  run = (x != nil);
449  while ((!run) && (recursion_node_stack_top > 1)) {
450  if (recursion_node_stack[--recursion_node_stack_top].try_right_branch) {
455  run = (x != nil);
456  }
457  }
458  }
459  return result_stack;
460 }
461 
462 } // namespace detail
463 } // namespace fcl
464 } // namespace hpp
465 
466 #endif
hpp::fcl::detail::IntervalTree::recursivePrint
void recursivePrint(IntervalTreeNode *node) const
recursively print a subtree
Definition: interval_tree.cpp:270
hpp::fcl::detail::IntervalTree::query
std::deque< SimpleInterval * > query(FCL_REAL low, FCL_REAL high)
Return result for a given query.
Definition: interval_tree.cpp:419
hpp::fcl::detail::it_recursion_node
Class describes the information needed when we take the right branch in searching for intervals but p...
Definition: interval_tree.h:54
hpp::fcl::detail::IntervalTree::fixupMaxHigh
void fixupMaxHigh(IntervalTreeNode *node)
Travels up to the root fixing the max_high fields after an insertion or deletion.
Definition: interval_tree.cpp:174
hpp::fcl::detail::IntervalTree::nil
IntervalTreeNode * nil
Definition: interval_tree.h:94
hpp::fcl::detail::IntervalTreeNode::max_high
FCL_REAL max_high
Definition: interval_tree_node.h:76
hpp::fcl::detail::IntervalTreeNode::key
FCL_REAL key
Definition: interval_tree_node.h:72
hpp::fcl::detail::IntervalTree::getSuccessor
IntervalTreeNode * getSuccessor(IntervalTreeNode *node) const
Get the successor of a given node.
Definition: interval_tree.cpp:234
hpp::fcl::detail::IntervalTreeNode::right
IntervalTreeNode * right
Definition: interval_tree_node.h:83
y
y
hpp::fcl::detail::IntervalTree::recursiveInsert
void recursiveInsert(IntervalTreeNode *node)
Inserts node into the tree as if it were a regular binary tree.
Definition: interval_tree.cpp:152
hpp::fcl::detail::IntervalTree::~IntervalTree
~IntervalTree()
Definition: interval_tree.cpp:74
hpp::fcl::detail::IntervalTree::IntervalTree
IntervalTree()
Definition: interval_tree.cpp:50
hpp::fcl::detail::IntervalTreeNode::parent
IntervalTreeNode * parent
Definition: interval_tree_node.h:85
hpp::fcl::detail::IntervalTree::rightRotate
void rightRotate(IntervalTreeNode *node)
right rotation of tree node
Definition: interval_tree.cpp:129
hpp::fcl::detail::IntervalTreeNode::stored_interval
SimpleInterval * stored_interval
interval stored in the node
Definition: interval_tree_node.h:70
hpp::fcl::detail::IntervalTree::print
void print() const
Print the whole interval tree.
Definition: interval_tree.cpp:279
hpp::fcl::detail::overlap
bool overlap(FCL_REAL a1, FCL_REAL a2, FCL_REAL b1, FCL_REAL b2)
returns 1 if the intervals overlap, and 0 otherwise
Definition: interval_tree.cpp:410
hpp::fcl::detail::IntervalTree::recursiveSearch
IntervalTreeNode * recursiveSearch(IntervalTreeNode *node, SimpleInterval *ivl) const
recursively find the node corresponding to the interval
Definition: interval_tree.cpp:347
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::detail::IntervalTreeNode
The node for interval tree.
Definition: interval_tree_node.h:52
hpp::fcl::detail::it_recursion_node::start_node
IntervalTreeNode * start_node
Definition: interval_tree.h:56
hpp::fcl::detail::IntervalTree::current_parent
unsigned int current_parent
Definition: interval_tree.h:121
hpp::fcl::detail::it_recursion_node::try_right_branch
bool try_right_branch
Definition: interval_tree.h:60
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
x
x
omniidl_be_python_with_docstring.run
def run(tree, args)
Definition: omniidl_be_python_with_docstring.py:140
hpp::fcl::detail::IntervalTree::recursion_node_stack_top
unsigned int recursion_node_stack_top
Definition: interval_tree.h:122
hpp::fcl::detail::IntervalTree::root
IntervalTreeNode * root
Definition: interval_tree.h:92
hpp::fcl::detail::IntervalTree::getPredecessor
IntervalTreeNode * getPredecessor(IntervalTreeNode *node) const
get the predecessor of a given node
Definition: interval_tree.cpp:252
hpp::fcl::detail::IntervalTree::leftRotate
void leftRotate(IntervalTreeNode *node)
left rotation of tree node
Definition: interval_tree.cpp:105
hpp::fcl::detail::IntervalTreeNode::red
bool red
red or black node: if red = false then the node is black
Definition: interval_tree_node.h:79
hpp::fcl::detail::IntervalTree::recursion_node_stack_size
unsigned int recursion_node_stack_size
Definition: interval_tree.h:119
interval_tree.h
hpp::fcl::detail::IntervalTree::insert
IntervalTreeNode * insert(SimpleInterval *new_interval)
Insert one node of the interval tree.
Definition: interval_tree.cpp:183
hpp::fcl::detail::SimpleInterval
Interval trees implemented using red-black-trees as described in the book Introduction_To_Algorithms_...
Definition: simple_interval.h:50
hpp::fcl::detail::IntervalTree::deleteFixup
void deleteFixup(IntervalTreeNode *node)
Definition: interval_tree.cpp:282
hpp::fcl::detail::it_recursion_node::parent_index
unsigned int parent_index
Definition: interval_tree.h:58
hpp::fcl::detail::IntervalTreeNode::high
FCL_REAL high
Definition: interval_tree_node.h:74
hpp::fcl::detail::IntervalTree::recursion_node_stack
it_recursion_node * recursion_node_stack
Definition: interval_tree.h:120
hpp::fcl::detail::IntervalTree::deleteNode
SimpleInterval * deleteNode(IntervalTreeNode *node)
Delete one node of the interval tree.
Definition: interval_tree.cpp:362
hpp::fcl::detail::IntervalTreeNode::left
IntervalTreeNode * left
Definition: interval_tree_node.h:81


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:14