interval_tree-inl.h
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 FCL_INTERVAL_TREE_INL_H
39 #define FCL_INTERVAL_TREE_INL_H
40 
42 
43 #include <algorithm>
44 
45 namespace fcl {
46 namespace detail {
47 
48 //==============================================================================
49 extern template
50 class IntervalTree<double>;
51 
52 //==============================================================================
53 template <typename S>
55 {
56  nil = new IntervalTreeNode<S>;
57  nil->left = nil->right = nil->parent = nil;
58  nil->red = false;
59  nil->key = nil->high = nil->max_high = -std::numeric_limits<double>::max();
60  nil->stored_interval = nullptr;
61 
62  root = new IntervalTreeNode<S>;
63  root->parent = root->left = root->right = nil;
64  root->key = root->high = root->max_high = std::numeric_limits<double>::max();
65  root->red = false;
66  root->stored_interval = nullptr;
67 
69  recursion_node_stack_size = 128;
70  recursion_node_stack = (it_recursion_node<S>*)malloc(recursion_node_stack_size*sizeof(it_recursion_node<S>));
71  recursion_node_stack_top = 1;
72  recursion_node_stack[0].start_node = nullptr;
73 }
74 
75 //==============================================================================
76 template <typename S>
78 {
79  IntervalTreeNode<S>* x = root->left;
80  std::deque<IntervalTreeNode<S>*> nodes_to_free;
81 
82  if(x != nil)
83  {
84  if(x->left != nil)
85  {
86  nodes_to_free.push_back(x->left);
87  }
88  if(x->right != nil)
89  {
90  nodes_to_free.push_back(x->right);
91  }
92 
93  delete x;
94  while( nodes_to_free.size() > 0)
95  {
96  x = nodes_to_free.back();
97  nodes_to_free.pop_back();
98  if(x->left != nil)
99  {
100  nodes_to_free.push_back(x->left);
101  }
102  if(x->right != nil)
103  {
104  nodes_to_free.push_back(x->right);
105  }
106  delete x;
107  }
108  }
109  delete nil;
110  delete root;
111  free(recursion_node_stack);
112 }
113 
114 //==============================================================================
115 template <typename S>
117 {
119 
120  y = x->right;
121  x->right = y->left;
122 
123  if(y->left != nil) y->left->parent = x;
124 
125  y->parent = x->parent;
126 
127  if(x == x->parent->left)
128  x->parent->left = y;
129  else
130  x->parent->right = y;
131 
132  y->left = x;
133  x->parent = y;
134 
136  y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high));
137 }
138 
139 //==============================================================================
140 template <typename S>
142 {
144 
145  x = y->left;
146  y->left = x->right;
147 
148  if(nil != x->right) x->right->parent = y;
149 
150  x->parent = y->parent;
151  if(y == y->parent->left)
152  y->parent->left = x;
153  else
154  y->parent->right = x;
155 
156  x->right = y;
157  y->parent = x;
158 
160  x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high));
161 }
162 
163 //==============================================================================
164 template <typename S>
166 {
169 
170  z->left = z->right = nil;
171  y = root;
172  x = root->left;
173  while(x != nil)
174  {
175  y = x;
176  if(x->key > z->key)
177  x = x->left;
178  else
179  x = x->right;
180  }
181  z->parent = y;
182  if((y == root) || (y->key > z->key))
183  y->left = z;
184  else
185  y->right = z;
186 }
187 
188 //==============================================================================
189 template <typename S>
191 {
192  while(x != root)
193  {
195  x = x->parent;
196  }
197 }
198 
199 //==============================================================================
200 template <typename S>
202 {
205  IntervalTreeNode<S>* new_node;
206 
207  x = new IntervalTreeNode<S>(new_interval);
208  recursiveInsert(x);
209  fixupMaxHigh(x->parent);
210  new_node = x;
211  x->red = true;
212  while(x->parent->red)
213  {
215  if(x->parent == x->parent->parent->left)
216  {
217  y = x->parent->parent->right;
218  if(y->red)
219  {
220  x->parent->red = true;
221  y->red = true;
222  x->parent->parent->red = true;
223  x = x->parent->parent;
224  }
225  else
226  {
227  if(x == x->parent->right)
228  {
229  x = x->parent;
230  leftRotate(x);
231  }
232  x->parent->red = false;
233  x->parent->parent->red = true;
234  rightRotate(x->parent->parent);
235  }
236  }
237  else
238  {
239  y = x->parent->parent->left;
240  if(y->red)
241  {
242  x->parent->red = false;
243  y->red = false;
244  x->parent->parent->red = true;
245  x = x->parent->parent;
246  }
247  else
248  {
249  if(x == x->parent->left)
250  {
251  x = x->parent;
252  rightRotate(x);
253  }
254  x->parent->red = false;
255  x->parent->parent->red = true;
256  leftRotate(x->parent->parent);
257  }
258  }
259  }
260  root->left->red = false;
261  return new_node;
262 }
263 
264 //==============================================================================
265 template <typename S>
267 {
269 
270  if(nil != (y = x->right))
271  {
272  while(y->left != nil)
273  y = y->left;
274  return y;
275  }
276  else
277  {
278  y = x->parent;
279  while(x == y->right)
280  {
281  x = y;
282  y = y->parent;
283  }
284  if(y == root) return nil;
285  return y;
286  }
287 }
288 
289 //==============================================================================
290 template <typename S>
292 {
294 
295  if(nil != (y = x->left))
296  {
297  while(y->right != nil)
298  y = y->right;
299  return y;
300  }
301  else
302  {
303  y = x->parent;
304  while(x == y->left)
305  {
306  if(y == root) return nil;
307  x = y;
308  y = y->parent;
309  }
310  return y;
311  }
312 }
313 
314 //==============================================================================
315 template <typename S>
317 {
318  if(x != nil)
319  {
320  recursivePrint(x->left);
321  x->print(nil,root);
322  recursivePrint(x->right);
323  }
324 }
325 
326 //==============================================================================
327 template <typename S>
329 {
330  recursivePrint(root->left);
331 }
332 
333 //==============================================================================
334 template <typename S>
336 {
338  IntervalTreeNode<S>* root_left_node = root->left;
339 
340  while((!x->red) && (root_left_node != x))
341  {
342  if(x == x->parent->left)
343  {
344  w = x->parent->right;
345  if(w->red)
346  {
347  w->red = false;
348  x->parent->red = true;
349  leftRotate(x->parent);
350  w = x->parent->right;
351  }
352  if((!w->right->red) && (!w->left->red))
353  {
354  w->red = true;
355  x = x->parent;
356  }
357  else
358  {
359  if(!w->right->red)
360  {
361  w->left->red = false;
362  w->red = true;
363  rightRotate(w);
364  w = x->parent->right;
365  }
366  w->red = x->parent->red;
367  x->parent->red = false;
368  w->right->red = false;
369  leftRotate(x->parent);
370  x = root_left_node;
371  }
372  }
373  else
374  {
375  w = x->parent->left;
376  if(w->red)
377  {
378  w->red = false;
379  x->parent->red = true;
380  rightRotate(x->parent);
381  w = x->parent->left;
382  }
383  if((!w->right->red) && (!w->left->red))
384  {
385  w->red = true;
386  x = x->parent;
387  }
388  else
389  {
390  if(!w->left->red)
391  {
392  w->right->red = false;
393  w->red = true;
394  leftRotate(w);
395  w = x->parent->left;
396  }
397  w->red = x->parent->red;
398  x->parent->red = false;
399  w->left->red = false;
400  rightRotate(x->parent);
401  x = root_left_node;
402  }
403  }
404  }
405  x->red = false;
406 }
407 
408 //==============================================================================
409 template <typename S>
411 {
412  IntervalTreeNode<S>* node = recursiveSearch(root, ivl);
413  if(node)
414  deleteNode(node);
415 }
416 
417 //==============================================================================
418 template <typename S>
420 {
421  if(node != nil)
422  {
423  if(node->stored_interval == ivl)
424  return node;
425 
426  IntervalTreeNode<S>* left = recursiveSearch(node->left, ivl);
427  if(left != nil) return left;
428  IntervalTreeNode<S>* right = recursiveSearch(node->right, ivl);
429  if(right != nil) return right;
430  }
431 
432  return nil;
433 }
434 
435 //==============================================================================
436 template <typename S>
438 {
441  SimpleInterval<S>* node_to_delete = z->stored_interval;
442 
443  y= ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z);
444  x= (y->left == nil) ? y->right : y->left;
445  if(root == (x->parent = y->parent))
446  {
447  root->left = x;
448  }
449  else
450  {
451  if(y == y->parent->left)
452  {
453  y->parent->left = x;
454  }
455  else
456  {
457  y->parent->right = x;
458  }
459  }
460 
463  if(y != z)
464  {
466  y->left = z->left;
467  y->right = z->right;
468  y->parent = z->parent;
469  z->left->parent = z->right->parent = y;
470  if(z == z->parent->left)
471  z->parent->left = y;
472  else
473  z->parent->right = y;
474 
475  fixupMaxHigh(x->parent);
476  if(!(y->red))
477  {
478  y->red = z->red;
479  deleteFixup(x);
480  }
481  else
482  y->red = z->red;
483  delete z;
484  }
485  else
486  {
487  fixupMaxHigh(x->parent);
488  if(!(y->red)) deleteFixup(x);
489  delete y;
490  }
491 
492  return node_to_delete;
493 }
494 
495 //==============================================================================
497 template <typename S>
498 bool overlap(S a1, S a2, S b1, S b2)
499 {
500  if(a1 <= b1)
501  {
502  return (b1 <= a2);
503  }
504  else
505  {
506  return (a1 <= b2);
507  }
508 }
509 
510 //==============================================================================
511 template <typename S>
512 std::deque<SimpleInterval<S>*> IntervalTree<S>::query(S low, S high)
513 {
514  std::deque<SimpleInterval<S>*> result_stack;
515  IntervalTreeNode<S>* x = root->left;
516  bool run = (x != nil);
517 
518  current_parent = 0;
519 
520  while(run)
521  {
522  if(overlap(low,high,x->key,x->high))
523  {
524  result_stack.push_back(x->stored_interval);
525  recursion_node_stack[current_parent].try_right_branch = true;
526  }
527  if(x->left->max_high >= low)
528  {
529  if(recursion_node_stack_top == recursion_node_stack_size)
530  {
531  recursion_node_stack_size *= 2;
532  recursion_node_stack = (it_recursion_node<S> *)realloc(recursion_node_stack, recursion_node_stack_size * sizeof(it_recursion_node<S>));
533  if(recursion_node_stack == nullptr)
534  abort();
535  }
536  recursion_node_stack[recursion_node_stack_top].start_node = x;
537  recursion_node_stack[recursion_node_stack_top].try_right_branch = false;
538  recursion_node_stack[recursion_node_stack_top].parent_index = current_parent;
539  current_parent = recursion_node_stack_top++;
540  x = x->left;
541  }
542  else
543  x = x->right;
544 
545  run = (x != nil);
546  while((!run) && (recursion_node_stack_top > 1))
547  {
548  if(recursion_node_stack[--recursion_node_stack_top].try_right_branch)
549  {
550  x=recursion_node_stack[recursion_node_stack_top].start_node->right;
551  current_parent=recursion_node_stack[recursion_node_stack_top].parent_index;
552  recursion_node_stack[current_parent].try_right_branch = true;
553  run = (x != nil);
554  }
555  }
556  }
557  return result_stack;
558 }
559 
560 } // namespace detail
561 } // namespace fcl
562 
563 #endif
fcl::detail::SimpleInterval
Interval trees implemented using red-black-trees as described in the book Introduction_To_Algorithms_...
Definition: simple_interval.h:52
fcl::detail::IntervalTree::deleteFixup
void deleteFixup(IntervalTreeNode< S > *node)
Definition: interval_tree-inl.h:335
fcl::detail::IntervalTree::leftRotate
void leftRotate(IntervalTreeNode< S > *node)
left rotation of tree node
Definition: interval_tree-inl.h:116
fcl::detail::IntervalTree::~IntervalTree
~IntervalTree()
Definition: interval_tree-inl.h:77
fcl::detail::IntervalTree::fixupMaxHigh
void fixupMaxHigh(IntervalTreeNode< S > *node)
Travels up to the root fixing the max_high fields after an insertion or deletion.
Definition: interval_tree-inl.h:190
fcl::detail::IntervalTree::getSuccessor
IntervalTreeNode< S > * getSuccessor(IntervalTreeNode< S > *node) const
Get the successor of a given node.
Definition: interval_tree-inl.h:266
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::detail::IntervalTreeNode::right
IntervalTreeNode * right
Definition: interval_tree_node.h:90
fcl::detail::IntervalTree::IntervalTree
IntervalTree()
Definition: interval_tree-inl.h:54
fcl::detail::overlap
bool overlap(S a1, S a2, S b1, S b2)
returns 1 if the intervals overlap, and 0 otherwise
Definition: interval_tree-inl.h:498
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
fcl::detail::IntervalTree::insert
IntervalTreeNode< S > * insert(SimpleInterval< S > *new_interval)
Insert one node of the interval tree.
Definition: interval_tree-inl.h:201
fcl::detail::IntervalTreeNode::parent
IntervalTreeNode * parent
Definition: interval_tree_node.h:92
fcl::detail::IntervalTreeNode::print
void print(IntervalTreeNode *left, IntervalTreeNode *right) const
Print the interval node information: set left = nil and right = root.
Definition: interval_tree_node-inl.h:79
fcl::detail::IntervalTree::deleteNode
SimpleInterval< S > * deleteNode(IntervalTreeNode< S > *node)
Delete one node of the interval tree.
Definition: interval_tree-inl.h:437
fcl::detail::IntervalTreeNode::left
IntervalTreeNode * left
Definition: interval_tree_node.h:88
fcl::detail::IntervalTreeNode::key
S key
Definition: interval_tree_node.h:79
fcl::detail::IntervalTree::recursivePrint
void recursivePrint(IntervalTreeNode< S > *node) const
recursively print a subtree
Definition: interval_tree-inl.h:316
fcl::detail::IntervalTreeNode::high
S high
Definition: interval_tree_node.h:81
fcl::detail::IntervalTree::recursiveSearch
IntervalTreeNode< S > * recursiveSearch(IntervalTreeNode< S > *node, SimpleInterval< S > *ivl) const
recursively find the node corresponding to the interval
Definition: interval_tree-inl.h:419
fcl::detail::IntervalTreeNode::stored_interval
SimpleInterval< S > * stored_interval
interval stored in the node
Definition: interval_tree_node.h:77
fcl::detail::IntervalTreeNode
The node for interval tree.
Definition: interval_tree_node.h:55
fcl::detail::IntervalTreeNode::max_high
S max_high
Definition: interval_tree_node.h:83
fcl::detail::IntervalTree::getPredecessor
IntervalTreeNode< S > * getPredecessor(IntervalTreeNode< S > *node) const
get the predecessor of a given node
Definition: interval_tree-inl.h:291
interval_tree.h
fcl::detail::IntervalTreeNode::red
bool red
red or black node: if red = false then the node is black
Definition: interval_tree_node.h:86
fcl::detail::IntervalTree::print
void print() const
Print the whole interval tree.
Definition: interval_tree-inl.h:328
fcl::detail::IntervalTree::rightRotate
void rightRotate(IntervalTreeNode< S > *node)
right rotation of tree node
Definition: interval_tree-inl.h:141
fcl::detail::IntervalTree::query
std::deque< SimpleInterval< S > * > query(S low, S high)
Return result for a given query.
Definition: interval_tree-inl.h:512
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::IntervalTree::recursiveInsert
void recursiveInsert(IntervalTreeNode< S > *node)
Inserts node into the tree as if it were a regular binary tree.
Definition: interval_tree-inl.h:165


fcl
Author(s):
autogenerated on Tue Jun 22 2021 07:27:51