interval_tree.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00037 #include "fcl/broadphase/interval_tree.h"
00038 #include <iostream>
00039 #include <cstdlib>
00040 
00041 
00042 namespace fcl
00043 {
00044 
00045 IntervalTreeNode::IntervalTreeNode(){}
00046 
00047 IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) :
00048   stored_interval (new_interval),
00049   key(new_interval->low),
00050   high(new_interval->high),
00051   max_high(high) {}
00052 
00053 IntervalTreeNode::~IntervalTreeNode() {}
00054 
00055 
00059 struct it_recursion_node
00060 {
00061 public:
00062   IntervalTreeNode* start_node;
00063 
00064   unsigned int parent_index;
00065 
00066   bool try_right_branch;
00067 };
00068 
00069 
00070 IntervalTree::IntervalTree()
00071 {
00072   nil = new IntervalTreeNode;
00073   nil->left = nil->right = nil->parent = nil;
00074   nil->red = false;
00075   nil->key = nil->high = nil->max_high = -std::numeric_limits<double>::max();
00076   nil->stored_interval = NULL;
00077 
00078   root = new IntervalTreeNode;
00079   root->parent = root->left = root->right = nil;
00080   root->key = root->high = root->max_high = std::numeric_limits<double>::max();
00081   root->red = false;
00082   root->stored_interval = NULL;
00083 
00085   recursion_node_stack_size = 128;
00086   recursion_node_stack = (it_recursion_node*)malloc(recursion_node_stack_size*sizeof(it_recursion_node));
00087   recursion_node_stack_top = 1;
00088   recursion_node_stack[0].start_node = NULL;
00089 }
00090 
00091 IntervalTree::~IntervalTree()
00092 {
00093   IntervalTreeNode* x = root->left;
00094   std::deque<IntervalTreeNode*> nodes_to_free;
00095 
00096   if(x != nil)
00097   {
00098     if(x->left != nil)
00099     {
00100       nodes_to_free.push_back(x->left);
00101     }
00102     if(x->right != nil)
00103     {
00104       nodes_to_free.push_back(x->right);
00105     }
00106 
00107     delete x;
00108     while( nodes_to_free.size() > 0)
00109     {
00110       x = nodes_to_free.back();
00111       nodes_to_free.pop_back();
00112       if(x->left != nil)
00113       {
00114         nodes_to_free.push_back(x->left);
00115       }
00116       if(x->right != nil)
00117       {
00118         nodes_to_free.push_back(x->right);
00119       }
00120       delete x;
00121     }
00122   }
00123   delete nil;
00124   delete root;
00125   free(recursion_node_stack);
00126 }
00127 
00128 
00129 void IntervalTree::leftRotate(IntervalTreeNode* x)
00130 {
00131   IntervalTreeNode* y;
00132 
00133   y = x->right;
00134   x->right = y->left;
00135 
00136   if(y->left != nil) y->left->parent = x;
00137 
00138   y->parent = x->parent;
00139 
00140   if(x == x->parent->left)
00141     x->parent->left = y;
00142   else
00143     x->parent->right = y;
00144 
00145   y->left = x;
00146   x->parent = y;
00147 
00148   x->max_high = std::max(x->left->max_high, std::max(x->right->max_high, x->high));
00149   y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high));
00150 }
00151 
00152 
00153 void IntervalTree::rightRotate(IntervalTreeNode* y)
00154 {
00155   IntervalTreeNode* x;
00156 
00157   x = y->left;
00158   y->left = x->right;
00159 
00160   if(nil != x->right)  x->right->parent = y;
00161 
00162   x->parent = y->parent;
00163   if(y == y->parent->left)
00164     y->parent->left = x;
00165   else
00166     y->parent->right = x;
00167 
00168   x->right = y;
00169   y->parent = x;
00170 
00171   y->max_high = std::max(y->left->max_high, std::max(y->right->max_high, y->high));
00172   x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high));
00173 }
00174 
00175 
00176 
00178 void IntervalTree::recursiveInsert(IntervalTreeNode* z)
00179 {
00180   IntervalTreeNode* x;
00181   IntervalTreeNode* y;
00182 
00183   z->left = z->right = nil;
00184   y = root;
00185   x = root->left;
00186   while(x != nil)
00187   {
00188     y = x;
00189     if(x->key > z->key)
00190       x = x->left;
00191     else
00192       x = x->right;
00193   }
00194   z->parent = y;
00195   if((y == root) || (y->key > z->key))
00196     y->left = z;
00197   else
00198     y->right = z;
00199 }
00200 
00201 
00202 void IntervalTree::fixupMaxHigh(IntervalTreeNode* x)
00203 {
00204   while(x != root)
00205   {
00206     x->max_high = std::max(x->high, std::max(x->left->max_high, x->right->max_high));
00207     x = x->parent;
00208   }
00209 }
00210 
00211 IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval)
00212 {
00213   IntervalTreeNode* y;
00214   IntervalTreeNode* x;
00215   IntervalTreeNode* new_node;
00216 
00217   x = new IntervalTreeNode(new_interval);
00218   recursiveInsert(x);
00219   fixupMaxHigh(x->parent);
00220   new_node = x;
00221   x->red = true;
00222   while(x->parent->red)
00223   {
00225     if(x->parent == x->parent->parent->left)
00226     {
00227       y = x->parent->parent->right;
00228       if(y->red)
00229       {
00230         x->parent->red = true;
00231         y->red = true;
00232         x->parent->parent->red = true;
00233         x = x->parent->parent;
00234       }
00235       else
00236       {
00237         if(x == x->parent->right)
00238         {
00239           x = x->parent;
00240           leftRotate(x);
00241         }
00242         x->parent->red = false;
00243         x->parent->parent->red = true;
00244         rightRotate(x->parent->parent);
00245       }
00246     }
00247     else
00248     {
00249       y = x->parent->parent->left;
00250       if(y->red)
00251       {
00252         x->parent->red = false;
00253         y->red = false;
00254         x->parent->parent->red = true;
00255         x = x->parent->parent;
00256       }
00257       else
00258       {
00259         if(x == x->parent->left)
00260         {
00261           x = x->parent;
00262           rightRotate(x);
00263         }
00264         x->parent->red = false;
00265         x->parent->parent->red = true;
00266         leftRotate(x->parent->parent);
00267       }
00268     }
00269   }
00270   root->left->red = false;
00271   return new_node;
00272 }
00273 
00274 IntervalTreeNode* IntervalTree::getSuccessor(IntervalTreeNode* x) const
00275 {
00276   IntervalTreeNode* y;
00277 
00278   if(nil != (y = x->right))
00279   {
00280     while(y->left != nil)
00281       y = y->left;
00282     return y;
00283   }
00284   else
00285   {
00286     y = x->parent;
00287     while(x == y->right)
00288     {
00289       x = y;
00290       y = y->parent;
00291     }
00292     if(y == root) return nil;
00293     return y;
00294   }
00295 }
00296 
00297 
00298 IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const
00299 {
00300   IntervalTreeNode* y;
00301 
00302   if(nil != (y = x->left))
00303   {
00304     while(y->right != nil)
00305       y = y->right;
00306     return y;
00307   }
00308   else
00309   {
00310     y = x->parent;
00311     while(x == y->left)
00312     {
00313       if(y == root) return nil;
00314       x = y;
00315       y = y->parent;
00316     }
00317     return y;
00318   }
00319 }
00320 
00321 void IntervalTreeNode::print(IntervalTreeNode* nil, IntervalTreeNode* root) const
00322 {
00323   stored_interval->print();
00324   std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high;
00325   std::cout << "  l->key = ";
00326   if(left == nil) std::cout << "NULL"; else std::cout << left->key;
00327   std::cout << "  r->key = ";
00328   if(right == nil) std::cout << "NULL"; else std::cout << right->key;
00329   std::cout << "  p->key = ";
00330   if(parent == root) std::cout << "NULL"; else std::cout << parent->key;
00331   std::cout << "  red = " << (int)red << std::endl;
00332 }
00333 
00334 void IntervalTree::recursivePrint(IntervalTreeNode* x) const
00335 {
00336   if(x != nil)
00337   {
00338     recursivePrint(x->left);
00339     x->print(nil,root);
00340     recursivePrint(x->right);
00341   }
00342 }
00343 
00344 
00345 void IntervalTree::print() const
00346 {
00347   recursivePrint(root->left);
00348 }
00349 
00350 void IntervalTree::deleteFixup(IntervalTreeNode* x)
00351 {
00352   IntervalTreeNode* w;
00353   IntervalTreeNode* root_left_node = root->left;
00354 
00355   while((!x->red) && (root_left_node != x))
00356   {
00357     if(x == x->parent->left)
00358     {
00359       w = x->parent->right;
00360       if(w->red)
00361       {
00362         w->red = false;
00363         x->parent->red = true;
00364         leftRotate(x->parent);
00365         w = x->parent->right;
00366       }
00367       if((!w->right->red) && (!w->left->red))
00368       {
00369         w->red = true;
00370         x = x->parent;
00371       }
00372       else
00373       {
00374         if(!w->right->red)
00375         {
00376           w->left->red = false;
00377           w->red = true;
00378           rightRotate(w);
00379           w = x->parent->right;
00380         }
00381         w->red = x->parent->red;
00382         x->parent->red = false;
00383         w->right->red = false;
00384         leftRotate(x->parent);
00385         x = root_left_node;
00386       }
00387     }
00388     else
00389     {
00390       w = x->parent->left;
00391       if(w->red)
00392       {
00393         w->red = false;
00394         x->parent->red = true;
00395         rightRotate(x->parent);
00396         w = x->parent->left;
00397       }
00398       if((!w->right->red) && (!w->left->red))
00399       {
00400         w->red = true;
00401         x = x->parent;
00402       }
00403       else
00404       {
00405         if(!w->left->red)
00406         {
00407           w->right->red = false;
00408           w->red = true;
00409           leftRotate(w);
00410           w = x->parent->left;
00411         }
00412         w->red = x->parent->red;
00413         x->parent->red = false;
00414         w->left->red = false;
00415         rightRotate(x->parent);
00416         x = root_left_node;
00417       }
00418     }
00419   }
00420   x->red = false;
00421 }
00422 
00423 void IntervalTree::deleteNode(SimpleInterval* ivl)
00424 {
00425   IntervalTreeNode* node = recursiveSearch(root, ivl);
00426   if(node)
00427     deleteNode(node);
00428 }
00429 
00430 IntervalTreeNode* IntervalTree::recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const
00431 {
00432   if(node != nil)
00433   {
00434     if(node->stored_interval == ivl)
00435       return node;
00436     
00437     IntervalTreeNode* left = recursiveSearch(node->left, ivl);
00438     if(left != nil) return left;
00439     IntervalTreeNode* right = recursiveSearch(node->right, ivl);
00440     if(right != nil) return right;
00441   } 
00442   
00443   return nil;
00444 }
00445 
00446 SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z)
00447 {
00448   IntervalTreeNode* y;
00449   IntervalTreeNode* x;
00450   SimpleInterval* node_to_delete = z->stored_interval;
00451 
00452   y= ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z);
00453   x= (y->left == nil) ? y->right : y->left;
00454   if(root == (x->parent = y->parent))
00455   {
00456     root->left = x;
00457   }
00458   else
00459   {
00460     if(y == y->parent->left)
00461     {
00462       y->parent->left = x;
00463     }
00464     else
00465     {
00466       y->parent->right = x;
00467     }
00468   }
00469 
00472   if(y != z)
00473   {
00474     y->max_high = -std::numeric_limits<double>::max();
00475     y->left = z->left;
00476     y->right = z->right;
00477     y->parent = z->parent;
00478     z->left->parent = z->right->parent = y;
00479     if(z == z->parent->left)
00480       z->parent->left = y;
00481     else
00482       z->parent->right = y;
00483 
00484     fixupMaxHigh(x->parent);
00485     if(!(y->red))
00486     {
00487       y->red = z->red;
00488       deleteFixup(x);
00489     }
00490     else
00491       y->red = z->red;
00492     delete z;
00493   }
00494   else
00495   {
00496     fixupMaxHigh(x->parent);
00497     if(!(y->red)) deleteFixup(x);
00498     delete y;
00499   }
00500 
00501   return node_to_delete;
00502 }
00503 
00505 bool overlap(double a1, double a2, double b1, double b2)
00506 {
00507   if(a1 <= b1)
00508   {
00509     return (b1 <= a2);
00510   }
00511   else
00512   {
00513     return (a1 <= b2);
00514   }
00515 }
00516 
00517 std::deque<SimpleInterval*> IntervalTree::query(double low, double high)
00518 {
00519   std::deque<SimpleInterval*> result_stack;
00520   IntervalTreeNode* x = root->left;
00521   bool run = (x != nil);
00522 
00523   current_parent = 0;
00524 
00525   while(run)
00526   {
00527     if(overlap(low,high,x->key,x->high))
00528     {
00529       result_stack.push_back(x->stored_interval);
00530       recursion_node_stack[current_parent].try_right_branch = true;
00531     }
00532     if(x->left->max_high >= low)
00533     {
00534       if(recursion_node_stack_top == recursion_node_stack_size)
00535       {
00536         recursion_node_stack_size *= 2;
00537         recursion_node_stack = (it_recursion_node *)realloc(recursion_node_stack, recursion_node_stack_size * sizeof(it_recursion_node));
00538         if(recursion_node_stack == NULL)
00539           exit(1);
00540       }
00541       recursion_node_stack[recursion_node_stack_top].start_node = x;
00542       recursion_node_stack[recursion_node_stack_top].try_right_branch = false;
00543       recursion_node_stack[recursion_node_stack_top].parent_index = current_parent;
00544       current_parent = recursion_node_stack_top++;
00545       x = x->left;
00546     }
00547     else
00548       x = x->right;
00549 
00550     run = (x != nil);
00551     while((!run) && (recursion_node_stack_top > 1))
00552     {
00553       if(recursion_node_stack[--recursion_node_stack_top].try_right_branch)
00554       {
00555         x=recursion_node_stack[recursion_node_stack_top].start_node->right;
00556         current_parent=recursion_node_stack[recursion_node_stack_top].parent_index;
00557         recursion_node_stack[current_parent].try_right_branch = true;
00558         run = (x != nil);
00559       }
00560     }
00561   }
00562   return result_stack;
00563 }
00564 
00565 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:30