Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/broadphase/detail/interval_tree.cpp	Lines:	157	265	59.2 %
Date:	2024-02-09 12:57:42	Branches:	66	150	44.0 %


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2016, Open Source Robotics Foundation
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** @author Jia Pan */

#ifndef HPP_FCL_INTERVAL_TREE_INL_H
#define HPP_FCL_INTERVAL_TREE_INL_H

#include "hpp/fcl/broadphase/detail/interval_tree.h"

#include <algorithm>

namespace hpp {
namespace fcl {
namespace detail {

//==============================================================================
IntervalTree::IntervalTree() {
  nil = new IntervalTreeNode;
  nil->left = nil->right = nil->parent = nil;
  nil->red = false;
  nil->key = nil->high = nil->max_high =
      -(std::numeric_limits<FCL_REAL>::max)();
  nil->stored_interval = nullptr;

  root = new IntervalTreeNode;
  root->parent = root->left = root->right = nil;
  root->key = root->high = root->max_high =
      (std::numeric_limits<FCL_REAL>::max)();
  root->red = false;
  root->stored_interval = nullptr;

  /// the following are used for the query function
  recursion_node_stack_size = 128;
  recursion_node_stack = (it_recursion_node*)malloc(recursion_node_stack_size *
                                                    sizeof(it_recursion_node));
  recursion_node_stack_top = 1;
  recursion_node_stack[0].start_node = nullptr;
}

//==============================================================================
IntervalTree::~IntervalTree() {
  IntervalTreeNode* x = root->left;
  std::deque<IntervalTreeNode*> nodes_to_free;

  if (x != nil) {
    if (x->left != nil) {
      nodes_to_free.push_back(x->left);
    }
    if (x->right != nil) {
      nodes_to_free.push_back(x->right);
    }

    delete x;
    while (nodes_to_free.size() > 0) {
      x = nodes_to_free.back();
      nodes_to_free.pop_back();
      if (x->left != nil) {
        nodes_to_free.push_back(x->left);
      }
      if (x->right != nil) {
        nodes_to_free.push_back(x->right);
      }
      delete x;
    }
  }
  delete nil;
  delete root;
  free(recursion_node_stack);
}

//==============================================================================
void IntervalTree::leftRotate(IntervalTreeNode* x) {
  IntervalTreeNode* y;

  y = x->right;
  x->right = y->left;

  if (y->left != nil) y->left->parent = x;

  y->parent = x->parent;

  if (x == x->parent->left)
    x->parent->left = y;
  else
    x->parent->right = y;

  y->left = x;
  x->parent = y;

  x->max_high =
      std::max(x->left->max_high, std::max(x->right->max_high, x->high));
  y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high));
}

//==============================================================================
void IntervalTree::rightRotate(IntervalTreeNode* y) {
  IntervalTreeNode* x;

  x = y->left;
  y->left = x->right;

  if (nil != x->right) x->right->parent = y;

  x->parent = y->parent;
  if (y == y->parent->left)
    y->parent->left = x;
  else
    y->parent->right = x;

  x->right = y;
  y->parent = x;

  y->max_high =
      std::max(y->left->max_high, std::max(y->right->max_high, y->high));
  x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high));
}

//==============================================================================
void IntervalTree::recursiveInsert(IntervalTreeNode* z) {
  IntervalTreeNode* x;
  IntervalTreeNode* y;

  z->left = z->right = nil;
  y = root;
  x = root->left;
  while (x != nil) {
    y = x;
    if (x->key > z->key)
      x = x->left;
    else
      x = x->right;
  }
  z->parent = y;
  if ((y == root) || (y->key > z->key))
    y->left = z;
  else
    y->right = z;
}

//==============================================================================
void IntervalTree::fixupMaxHigh(IntervalTreeNode* x) {
  while (x != root) {
    x->max_high =
        std::max(x->high, std::max(x->left->max_high, x->right->max_high));
    x = x->parent;
  }
}

//==============================================================================
IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval) {
  IntervalTreeNode* y;
  IntervalTreeNode* x;
  IntervalTreeNode* new_node;

  x = new IntervalTreeNode(new_interval);
  recursiveInsert(x);
  fixupMaxHigh(x->parent);
  new_node = x;
  x->red = true;
  while (x->parent->red) {
    /// use sentinel instead of checking for root
    if (x->parent == x->parent->parent->left) {
      y = x->parent->parent->right;
      if (y->red) {
        x->parent->red = true;
        y->red = true;
        x->parent->parent->red = true;
        x = x->parent->parent;
      } else {
        if (x == x->parent->right) {
          x = x->parent;
          leftRotate(x);
        }
        x->parent->red = false;
        x->parent->parent->red = true;
        rightRotate(x->parent->parent);
      }
    } else {
      y = x->parent->parent->left;
      if (y->red) {
        x->parent->red = false;
        y->red = false;
        x->parent->parent->red = true;
        x = x->parent->parent;
      } else {
        if (x == x->parent->left) {
          x = x->parent;
          rightRotate(x);
        }
        x->parent->red = false;
        x->parent->parent->red = true;
        leftRotate(x->parent->parent);
      }
    }
  }
  root->left->red = false;
  return new_node;
}

//==============================================================================
IntervalTreeNode* IntervalTree::getSuccessor(IntervalTreeNode* x) const {
  IntervalTreeNode* y;

  if (nil != (y = x->right)) {
    while (y->left != nil) y = y->left;
    return y;
  } else {
    y = x->parent;
    while (x == y->right) {
      x = y;
      y = y->parent;
    }
    if (y == root) return nil;
    return y;
  }
}

//==============================================================================
IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const {
  IntervalTreeNode* y;

  if (nil != (y = x->left)) {
    while (y->right != nil) y = y->right;
    return y;
  } else {
    y = x->parent;
    while (x == y->left) {
      if (y == root) return nil;
      x = y;
      y = y->parent;
    }
    return y;
  }
}

//==============================================================================
void IntervalTree::recursivePrint(IntervalTreeNode* x) const {
  if (x != nil) {
    recursivePrint(x->left);
    x->print(nil, root);
    recursivePrint(x->right);
  }
}

//==============================================================================
void IntervalTree::print() const { recursivePrint(root->left); }

//==============================================================================
void IntervalTree::deleteFixup(IntervalTreeNode* x) {
  IntervalTreeNode* w;
  IntervalTreeNode* root_left_node = root->left;

  while ((!x->red) && (root_left_node != x)) {
    if (x == x->parent->left) {
      w = x->parent->right;
      if (w->red) {
        w->red = false;
        x->parent->red = true;
        leftRotate(x->parent);
        w = x->parent->right;
      }
      if ((!w->right->red) && (!w->left->red)) {
        w->red = true;
        x = x->parent;
      } else {
        if (!w->right->red) {
          w->left->red = false;
          w->red = true;
          rightRotate(w);
          w = x->parent->right;
        }
        w->red = x->parent->red;
        x->parent->red = false;
        w->right->red = false;
        leftRotate(x->parent);
        x = root_left_node;
      }
    } else {
      w = x->parent->left;
      if (w->red) {
        w->red = false;
        x->parent->red = true;
        rightRotate(x->parent);
        w = x->parent->left;
      }
      if ((!w->right->red) && (!w->left->red)) {
        w->red = true;
        x = x->parent;
      } else {
        if (!w->left->red) {
          w->right->red = false;
          w->red = true;
          leftRotate(w);
          w = x->parent->left;
        }
        w->red = x->parent->red;
        x->parent->red = false;
        w->left->red = false;
        rightRotate(x->parent);
        x = root_left_node;
      }
    }
  }
  x->red = false;
}

//==============================================================================
void IntervalTree::deleteNode(SimpleInterval* ivl) {
  IntervalTreeNode* node = recursiveSearch(root, ivl);
  if (node) deleteNode(node);
}

//==============================================================================
IntervalTreeNode* IntervalTree::recursiveSearch(IntervalTreeNode* node,
                                                SimpleInterval* ivl) const {
  if (node != nil) {
    if (node->stored_interval == ivl) return node;

    IntervalTreeNode* left = recursiveSearch(node->left, ivl);
    if (left != nil) return left;
    IntervalTreeNode* right = recursiveSearch(node->right, ivl);
    if (right != nil) return right;
  }

  return nil;
}

//==============================================================================
SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) {
  IntervalTreeNode* y;
  IntervalTreeNode* x;
  SimpleInterval* node_to_delete = z->stored_interval;

  y = ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z);
  x = (y->left == nil) ? y->right : y->left;
  if (root == (x->parent = y->parent)) {
    root->left = x;
  } else {
    if (y == y->parent->left) {
      y->parent->left = x;
    } else {
      y->parent->right = x;
    }
  }

  /// @brief y should not be nil in this case
  /// y is the node to splice out and x is its child
  if (y != z) {
    y->max_high = -(std::numeric_limits<FCL_REAL>::max)();
    y->left = z->left;
    y->right = z->right;
    y->parent = z->parent;
    z->left->parent = z->right->parent = y;
    if (z == z->parent->left)
      z->parent->left = y;
    else
      z->parent->right = y;

    fixupMaxHigh(x->parent);
    if (!(y->red)) {
      y->red = z->red;
      deleteFixup(x);
    } else
      y->red = z->red;
    delete z;
  } else {
    fixupMaxHigh(x->parent);
    if (!(y->red)) deleteFixup(x);
    delete y;
  }

  return node_to_delete;
}

//==============================================================================
/// @brief returns 1 if the intervals overlap, and 0 otherwise
bool overlap(FCL_REAL a1, FCL_REAL a2, FCL_REAL b1, FCL_REAL b2) {
  if (a1 <= b1) {
    return (b1 <= a2);
  } else {
    return (a1 <= b2);
  }
}

//==============================================================================
std::deque<SimpleInterval*> IntervalTree::query(FCL_REAL low, FCL_REAL high) {
  std::deque<SimpleInterval*> result_stack;
  IntervalTreeNode* x = root->left;
  bool run = (x != nil);

  current_parent = 0;

  while (run) {
    if (overlap(low, high, x->key, x->high)) {
      result_stack.push_back(x->stored_interval);
      recursion_node_stack[current_parent].try_right_branch = true;
    }
    if (x->left->max_high >= low) {
      if (recursion_node_stack_top == recursion_node_stack_size) {
        recursion_node_stack_size *= 2;
        recursion_node_stack = (it_recursion_node*)realloc(
            recursion_node_stack,
            recursion_node_stack_size * sizeof(it_recursion_node));
        if (recursion_node_stack == nullptr) abort();
      }
      recursion_node_stack[recursion_node_stack_top].start_node = x;
      recursion_node_stack[recursion_node_stack_top].try_right_branch = false;
      recursion_node_stack[recursion_node_stack_top].parent_index =
          current_parent;
      current_parent = recursion_node_stack_top++;
      x = x->left;
    } else
      x = x->right;

    run = (x != nil);
    while ((!run) && (recursion_node_stack_top > 1)) {
      if (recursion_node_stack[--recursion_node_stack_top].try_right_branch) {
        x = recursion_node_stack[recursion_node_stack_top].start_node->right;
        current_parent =
            recursion_node_stack[recursion_node_stack_top].parent_index;
        recursion_node_stack[current_parent].try_right_branch = true;
        run = (x != nil);
      }
    }
  }
  return result_stack;
}

}  // namespace detail
}  // namespace fcl
}  // namespace hpp

#endif


Generated by: GCOVR (Version 4.2)

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 HPP_FCL_INTERVAL_TREE_INL_H
39			#define HPP_FCL_INTERVAL_TREE_INL_H
40
41			#include "hpp/fcl/broadphase/detail/interval_tree.h"
42
43			#include <algorithm>
44
45			namespace hpp {
46			namespace fcl {
47			namespace detail {
48
49			//==============================================================================
50		192	IntervalTree::IntervalTree() {
51	✓✗	192	nil = new IntervalTreeNode;
52		192	nil->left = nil->right = nil->parent = nil;
53		192	nil->red = false;
54		192	nil->key = nil->high = nil->max_high =
55		192	-(std::numeric_limits<FCL_REAL>::max)();
56		192	nil->stored_interval = nullptr;
57
58	✓✗	192	root = new IntervalTreeNode;
59		192	root->parent = root->left = root->right = nil;
60		384	root->key = root->high = root->max_high =
61		192	(std::numeric_limits<FCL_REAL>::max)();
62		192	root->red = false;
63		192	root->stored_interval = nullptr;
64
65			/// the following are used for the query function
66		192	recursion_node_stack_size = 128;
67		192	recursion_node_stack = (it_recursion_node)malloc(recursion_node_stack_size
68			sizeof(it_recursion_node));
69		192	recursion_node_stack_top = 1;
70		192	recursion_node_stack[0].start_node = nullptr;
71		192	}
72
73			//==============================================================================
74		378	IntervalTree::~IntervalTree() {
75		189	IntervalTreeNode* x = root->left;
76		189	std::deque<IntervalTreeNode*> nodes_to_free;
77
78	✓✓	189	if (x != nil) {
79	✓✗	165	if (x->left != nil) {
80		165	nodes_to_free.push_back(x->left);
81			}
82	✓✗	165	if (x->right != nil) {
83		165	nodes_to_free.push_back(x->right);
84			}
85
86	✓✗	165	delete x;
87	✓✓	41145	while (nodes_to_free.size() > 0) {
88		40980	x = nodes_to_free.back();
89		40980	nodes_to_free.pop_back();
90	✓✓	40980	if (x->left != nil) {
91		20373	nodes_to_free.push_back(x->left);
92			}
93	✓✓	40980	if (x->right != nil) {
94		20277	nodes_to_free.push_back(x->right);
95			}
96	✓✗	40980	delete x;
97			}
98			}
99	✓✗	189	delete nil;
100	✓✗	189	delete root;
101		189	free(recursion_node_stack);
102		189	}
103
104			//==============================================================================
105		23856	void IntervalTree::leftRotate(IntervalTreeNode* x) {
106			IntervalTreeNode* y;
107
108		23856	y = x->right;
109		23856	x->right = y->left;
110
111	✓✓	23856	if (y->left != nil) y->left->parent = x;
112
113		23856	y->parent = x->parent;
114
115	✓✓	23856	if (x == x->parent->left)
116		6877	x->parent->left = y;
117			else
118		16979	x->parent->right = y;
119
120		23856	y->left = x;
121		23856	x->parent = y;
122
123		23856	x->max_high =
124		23856	std::max(x->left->max_high, std::max(x->right->max_high, x->high));
125		23856	y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high));
126		23856	}
127
128			//==============================================================================
129		9445	void IntervalTree::rightRotate(IntervalTreeNode* y) {
130			IntervalTreeNode* x;
131
132		9445	x = y->left;
133		9445	y->left = x->right;
134
135	✓✓	9445	if (nil != x->right) x->right->parent = y;
136
137		9445	x->parent = y->parent;
138	✓✓	9445	if (y == y->parent->left)
139		3549	y->parent->left = x;
140			else
141		5896	y->parent->right = x;
142
143		9445	x->right = y;
144		9445	y->parent = x;
145
146		9445	y->max_high =
147		9445	std::max(y->left->max_high, std::max(y->right->max_high, y->high));
148		9445	x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high));
149		9445	}
150
151			//==============================================================================
152		42045	void IntervalTree::recursiveInsert(IntervalTreeNode* z) {
153			IntervalTreeNode* x;
154			IntervalTreeNode* y;
155
156		42045	z->left = z->right = nil;
157		42045	y = root;
158		42045	x = root->left;
159	✓✓	752286	while (x != nil) {
160		710241	y = x;
161	✓✓	710241	if (x->key > z->key)
162		389317	x = x->left;
163			else
164		320924	x = x->right;
165			}
166		42045	z->parent = y;
167	✓✓✓✓	42045	if ((y == root) \|\| (y->key > z->key))
168		13795	y->left = z;
169			else
170		28250	y->right = z;
171		42045	}
172
173			//==============================================================================
174		752286	void IntervalTree::fixupMaxHigh(IntervalTreeNode* x) {
175	✓✓	752286	while (x != root) {
176		710241	x->max_high =
177		710241	std::max(x->high, std::max(x->left->max_high, x->right->max_high));
178		710241	x = x->parent;
179			}
180		42045	}
181
182			//==============================================================================
183		42045	IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval) {
184			IntervalTreeNode* y;
185			IntervalTreeNode* x;
186			IntervalTreeNode* new_node;
187
188	✓✗	42045	x = new IntervalTreeNode(new_interval);
189		42045	recursiveInsert(x);
190		42045	fixupMaxHigh(x->parent);
191		42045	new_node = x;
192		42045	x->red = true;
193	✓✓	132561	while (x->parent->red) {
194			/// use sentinel instead of checking for root
195	✓✓	90516	if (x->parent == x->parent->parent->left) {
196		45080	y = x->parent->parent->right;
197	✓✓	45080	if (y->red) {
198		38308	x->parent->red = true;
199		38308	y->red = true;
200		38308	x->parent->parent->red = true;
201		38308	x = x->parent->parent;
202			} else {
203	✓✓	6772	if (x == x->parent->right) {
204		3611	x = x->parent;
205		3611	leftRotate(x);
206			}
207		6772	x->parent->red = false;
208		6772	x->parent->parent->red = true;
209		6772	rightRotate(x->parent->parent);
210			}
211			} else {
212		45436	y = x->parent->parent->left;
213	✓✓	45436	if (y->red) {
214		25191	x->parent->red = false;
215		25191	y->red = false;
216		25191	x->parent->parent->red = true;
217		25191	x = x->parent->parent;
218			} else {
219	✓✓	20245	if (x == x->parent->left) {
220		2673	x = x->parent;
221		2673	rightRotate(x);
222			}
223		20245	x->parent->red = false;
224		20245	x->parent->parent->red = true;
225		20245	leftRotate(x->parent->parent);
226			}
227			}
228			}
229		42045	root->left->red = false;
230		42045	return new_node;
231			}
232
233			//==============================================================================
234			IntervalTreeNode* IntervalTree::getSuccessor(IntervalTreeNode* x) const {
235			IntervalTreeNode* y;
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			//==============================================================================
252			IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const {
253			IntervalTreeNode* y;
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			//==============================================================================
270			void IntervalTree::recursivePrint(IntervalTreeNode* x) const {
271			if (x != nil) {
272			recursivePrint(x->left);
273			x->print(nil, root);
274			recursivePrint(x->right);
275			}
276			}
277
278			//==============================================================================
279			void IntervalTree::print() const { recursivePrint(root->left); }
280
281			//==============================================================================
282			void IntervalTree::deleteFixup(IntervalTreeNode* x) {
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			//==============================================================================
341			void IntervalTree::deleteNode(SimpleInterval* ivl) {
342			IntervalTreeNode* node = recursiveSearch(root, ivl);
343			if (node) deleteNode(node);
344			}
345
346			//==============================================================================
347			IntervalTreeNode* IntervalTree::recursiveSearch(IntervalTreeNode* node,
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			//==============================================================================
362			SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) {
363			IntervalTreeNode* y;
364			IntervalTreeNode* x;
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
379			/// @brief y should not be nil in this case
380			/// y is the node to splice out and x is its child
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			//==============================================================================
409			/// @brief returns 1 if the intervals overlap, and 0 otherwise
410		8164273	bool overlap(FCL_REAL a1, FCL_REAL a2, FCL_REAL b1, FCL_REAL b2) {
411	✓✓	8164273	if (a1 <= b1) {
412		8041528	return (b1 <= a2);
413			} else {
414		122745	return (a1 <= b2);
415			}
416			}
417
418			//==============================================================================
419		19391	std::deque<SimpleInterval*> IntervalTree::query(FCL_REAL low, FCL_REAL high) {
420		19391	std::deque<SimpleInterval*> result_stack;
421		19391	IntervalTreeNode* x = root->left;
422		19391	bool run = (x != nil);
423
424		19391	current_parent = 0;
425
426	✓✓	8183664	while (run) {
427	✓✗✓✓	8164273	if (overlap(low, high, x->key, x->high)) {
428	✓✗	6543197	result_stack.push_back(x->stored_interval);
429		6543197	recursion_node_stack[current_parent].try_right_branch = true;
430			}
431	✓✓	8164273	if (x->left->max_high >= low) {
432	✓✓	4354702	if (recursion_node_stack_top == recursion_node_stack_size) {
433		2	recursion_node_stack_size *= 2;
434		2	recursion_node_stack = (it_recursion_node*)realloc(
435		2	recursion_node_stack,
436		2	recursion_node_stack_size * sizeof(it_recursion_node));
437	✗✓	2	if (recursion_node_stack == nullptr) abort();
438			}
439		4354702	recursion_node_stack[recursion_node_stack_top].start_node = x;
440		4354702	recursion_node_stack[recursion_node_stack_top].try_right_branch = false;
441		4354702	recursion_node_stack[recursion_node_stack_top].parent_index =
442		4354702	current_parent;
443		4354702	current_parent = recursion_node_stack_top++;
444		4354702	x = x->left;
445			} else
446		3809571	x = x->right;
447
448		8164273	run = (x != nil);
449	✓✓✓✓	12518975	while ((!run) && (recursion_node_stack_top > 1)) {
450	✓✓	4354702	if (recursion_node_stack[--recursion_node_stack_top].try_right_branch) {
451		3695648	x = recursion_node_stack[recursion_node_stack_top].start_node->right;
452		3695648	current_parent =
453		3695648	recursion_node_stack[recursion_node_stack_top].parent_index;
454		3695648	recursion_node_stack[current_parent].try_right_branch = true;
455		3695648	run = (x != nil);
456			}
457			}
458			}
459		19391	return result_stack;
460			}
461
462			} // namespace detail
463			} // namespace fcl
464			} // namespace hpp
465
466			#endif