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 00038 #ifndef FCL_INTERVAL_TREE_H 00039 #define FCL_INTERVAL_TREE_H 00040 00041 #include <deque> 00042 #include <limits> 00043 00044 namespace fcl 00045 { 00046 00050 struct SimpleInterval 00051 { 00052 public: 00053 virtual ~SimpleInterval() {} 00054 00055 virtual void print() {} 00056 00058 double low, high; 00059 }; 00060 00062 class IntervalTreeNode 00063 { 00064 friend class IntervalTree; 00065 public: 00067 void print(IntervalTreeNode* left, IntervalTreeNode* right) const; 00068 00070 IntervalTreeNode(); 00071 00073 IntervalTreeNode(SimpleInterval* new_interval); 00074 00075 ~IntervalTreeNode(); 00076 00077 protected: 00079 SimpleInterval* stored_interval; 00080 00081 double key; 00082 00083 double high; 00084 00085 double max_high; 00086 00088 bool red; 00089 00090 IntervalTreeNode* left; 00091 00092 IntervalTreeNode* right; 00093 00094 IntervalTreeNode* parent; 00095 }; 00096 00097 struct it_recursion_node; 00098 00100 class IntervalTree 00101 { 00102 public: 00103 00104 IntervalTree(); 00105 00106 ~IntervalTree(); 00107 00109 void print() const; 00110 00112 SimpleInterval* deleteNode(IntervalTreeNode* node); 00113 00115 void deleteNode(SimpleInterval* ivl); 00116 00118 IntervalTreeNode* insert(SimpleInterval* new_interval); 00119 00121 IntervalTreeNode* getPredecessor(IntervalTreeNode* node) const; 00122 00124 IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const; 00125 00127 std::deque<SimpleInterval*> query(double low, double high); 00128 00129 protected: 00130 00131 IntervalTreeNode* root; 00132 00133 IntervalTreeNode* nil; 00134 00136 void leftRotate(IntervalTreeNode* node); 00137 00139 void rightRotate(IntervalTreeNode* node); 00140 00142 void recursiveInsert(IntervalTreeNode* node); 00143 00145 void recursivePrint(IntervalTreeNode* node) const; 00146 00148 IntervalTreeNode* recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const; 00149 00151 void fixupMaxHigh(IntervalTreeNode* node); 00152 00153 void deleteFixup(IntervalTreeNode* node); 00154 00155 private: 00156 unsigned int recursion_node_stack_size; 00157 it_recursion_node* recursion_node_stack; 00158 unsigned int current_parent; 00159 unsigned int recursion_node_stack_top; 00160 }; 00161 00162 } 00163 00164 #endif 00165 00166
