GCC Code Coverage Report

Directory: ./
File: src/broadphase/detail/interval_tree.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 158 268 59.0%
Branches: 66 150 44.0%
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 COAL_INTERVAL_TREE_INL_H
39 #define COAL_INTERVAL_TREE_INL_H
40
41 #include "coal/broadphase/detail/interval_tree.h"
42
43 #include <algorithm>
44
45 namespace coal {
46 namespace detail {
47
48 //==============================================================================
49 192 IntervalTree::IntervalTree() {
50
1/2
✓ Branch 2 taken 192 times.
✗ Branch 3 not taken.
 192 invalid_node = new IntervalTreeNode;
51 192 invalid_node->left = invalid_node->right = invalid_node->parent =
52 192 invalid_node;
53 192 invalid_node->red = false;
54 192 invalid_node->key = invalid_node->high = invalid_node->max_high =
55 192 -(std::numeric_limits<Scalar>::max)();
56 192 invalid_node->stored_interval = nullptr;
57
58
1/2
✓ Branch 2 taken 192 times.
✗ Branch 3 not taken.
 192 root = new IntervalTreeNode;
59 192 root->parent = root->left = root->right = invalid_node;
60 384 root->key = root->high = root->max_high =
61 192 (std::numeric_limits<Scalar>::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 189 IntervalTree::~IntervalTree() {
75 189 IntervalTreeNode* x = root->left;
76 189 std::deque<IntervalTreeNode*> nodes_to_free;
77
78
2/2
✓ Branch 0 taken 165 times.
✓ Branch 1 taken 24 times.
 189 if (x != invalid_node) {
79
1/2
✓ Branch 0 taken 165 times.
✗ Branch 1 not taken.
 165 if (x->left != invalid_node) {
80 165 nodes_to_free.push_back(x->left);
81 }
82
1/2
✓ Branch 0 taken 165 times.
✗ Branch 1 not taken.
 165 if (x->right != invalid_node) {
83 165 nodes_to_free.push_back(x->right);
84 }
85
86
1/2
✓ Branch 0 taken 165 times.
✗ Branch 1 not taken.
 165 delete x;
87
2/2
✓ Branch 1 taken 40980 times.
✓ Branch 2 taken 165 times.
 41145 while (nodes_to_free.size() > 0) {
88 40980 x = nodes_to_free.back();
89 40980 nodes_to_free.pop_back();
90
2/2
✓ Branch 0 taken 20392 times.
✓ Branch 1 taken 20588 times.
 40980 if (x->left != invalid_node) {
91 20392 nodes_to_free.push_back(x->left);
92 }
93
2/2
✓ Branch 0 taken 20258 times.
✓ Branch 1 taken 20722 times.
 40980 if (x->right != invalid_node) {
94 20258 nodes_to_free.push_back(x->right);
95 }
96
1/2
✓ Branch 0 taken 40980 times.
✗ Branch 1 not taken.
 40980 delete x;
97 }
98 }
99
1/2
✓ Branch 0 taken 189 times.
✗ Branch 1 not taken.
 189 delete invalid_node;
100
1/2
✓ Branch 0 taken 189 times.
✗ Branch 1 not taken.
 189 delete root;
101 189 free(recursion_node_stack);
102 189 }
103
104 //==============================================================================
105 23831 void IntervalTree::leftRotate(IntervalTreeNode* x) {
106 IntervalTreeNode* y;
107
108 23831 y = x->right;
109 23831 x->right = y->left;
110
111
2/2
✓ Branch 0 taken 9783 times.
✓ Branch 1 taken 14048 times.
 23831 if (y->left != invalid_node) y->left->parent = x;
112
113 23831 y->parent = x->parent;
114
115
2/2
✓ Branch 0 taken 6873 times.
✓ Branch 1 taken 16958 times.
 23831 if (x == x->parent->left)
116 6873 x->parent->left = y;
117 else
118 16958 x->parent->right = y;
119
120 23831 y->left = x;
121 23831 x->parent = y;
122
123 23831 x->max_high =
124 23831 std::max(x->left->max_high, std::max(x->right->max_high, x->high));
125 23831 y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high));
126 23831 }
127
128 //==============================================================================
129 9390 void IntervalTree::rightRotate(IntervalTreeNode* y) {
130 IntervalTreeNode* x;
131
132 9390 x = y->left;
133 9390 y->left = x->right;
134
135
2/2
✓ Branch 0 taken 2681 times.
✓ Branch 1 taken 6709 times.
 9390 if (invalid_node != x->right) x->right->parent = y;
136
137 9390 x->parent = y->parent;
138
2/2
✓ Branch 0 taken 3536 times.
✓ Branch 1 taken 5854 times.
 9390 if (y == y->parent->left)
139 3536 y->parent->left = x;
140 else
141 5854 y->parent->right = x;
142
143 9390 x->right = y;
144 9390 y->parent = x;
145
146 9390 y->max_high =
147 9390 std::max(y->left->max_high, std::max(y->right->max_high, y->high));
148 9390 x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high));
149 9390 }
150
151 //==============================================================================
152 42045 void IntervalTree::recursiveInsert(IntervalTreeNode* z) {
153 IntervalTreeNode* x;
154 IntervalTreeNode* y;
155
156 42045 z->left = z->right = invalid_node;
157 42045 y = root;
158 42045 x = root->left;
159
2/2
✓ Branch 0 taken 705590 times.
✓ Branch 1 taken 42045 times.
 747635 while (x != invalid_node) {
160 705590 y = x;
161
2/2
✓ Branch 0 taken 381862 times.
✓ Branch 1 taken 323728 times.
 705590 if (x->key > z->key)
162 381862 x = x->left;
163 else
164 323728 x = x->right;
165 }
166 42045 z->parent = y;
167
4/4
✓ Branch 0 taken 41877 times.
✓ Branch 1 taken 168 times.
✓ Branch 2 taken 13671 times.
✓ Branch 3 taken 28206 times.
 42045 if ((y == root) || (y->key > z->key))
168 13839 y->left = z;
169 else
170 28206 y->right = z;
171 42045 }
172
173 //==============================================================================
174 42045 void IntervalTree::fixupMaxHigh(IntervalTreeNode* x) {
175
2/2
✓ Branch 0 taken 705590 times.
✓ Branch 1 taken 42045 times.
 747635 while (x != root) {
176 705590 x->max_high =
177 705590 std::max(x->high, std::max(x->left->max_high, x->right->max_high));
178 705590 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
1/2
✓ Branch 2 taken 42045 times.
✗ Branch 3 not taken.
 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
2/2
✓ Branch 0 taken 90249 times.
✓ Branch 1 taken 42045 times.
 132294 while (x->parent->red) {
194 /// use sentinel instead of checking for root
195
2/2
✓ Branch 0 taken 44725 times.
✓ Branch 1 taken 45524 times.
 90249 if (x->parent == x->parent->parent->left) {
196 44725 y = x->parent->parent->right;
197
2/2
✓ Branch 0 taken 38027 times.
✓ Branch 1 taken 6698 times.
 44725 if (y->red) {
198 38027 x->parent->red = true;
199 38027 y->red = true;
200 38027 x->parent->parent->red = true;
201 38027 x = x->parent->parent;
202 } else {
203
2/2
✓ Branch 0 taken 3553 times.
✓ Branch 1 taken 3145 times.
 6698 if (x == x->parent->right) {
204 3553 x = x->parent;
205 3553 leftRotate(x);
206 }
207 6698 x->parent->red = false;
208 6698 x->parent->parent->red = true;
209 6698 rightRotate(x->parent->parent);
210 }
211 } else {
212 45524 y = x->parent->parent->left;
213
2/2
✓ Branch 0 taken 25246 times.
✓ Branch 1 taken 20278 times.
 45524 if (y->red) {
214 25246 x->parent->red = false;
215 25246 y->red = false;
216 25246 x->parent->parent->red = true;
217 25246 x = x->parent->parent;
218 } else {
219
2/2
✓ Branch 0 taken 2692 times.
✓ Branch 1 taken 17586 times.
 20278 if (x == x->parent->left) {
220 2692 x = x->parent;
221 2692 rightRotate(x);
222 }
223 20278 x->parent->red = false;
224 20278 x->parent->parent->red = true;
225 20278 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 (invalid_node != (y = x->right)) {
238 ✗ while (y->left != invalid_node) 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 invalid_node;
247 ✗ return y;
248 }
249 }
250
251 //==============================================================================
252 ✗ IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const {
253 IntervalTreeNode* y;
254
255 ✗ if (invalid_node != (y = x->left)) {
256 ✗ while (y->right != invalid_node) y = y->right;
257 ✗ return y;
258 } else {
259 ✗ y = x->parent;
260 ✗ while (x == y->left) {
261 ✗ if (y == root) return invalid_node;
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 != invalid_node) {
272 ✗ recursivePrint(x->left);
273 ✗ x->print(invalid_node, 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 != invalid_node) {
350 ✗ if (node->stored_interval == ivl) return node;
351
352 ✗ IntervalTreeNode* left = recursiveSearch(node->left, ivl);
353 ✗ if (left != invalid_node) return left;
354 ✗ IntervalTreeNode* right = recursiveSearch(node->right, ivl);
355 ✗ if (right != invalid_node) return right;
356 }
357
358 ✗ return invalid_node;
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 == invalid_node) || (z->right == invalid_node))
368 ✗ ? z
369 ✗ : getSuccessor(z);
370 ✗ x = (y->left == invalid_node) ? y->right : y->left;
371 ✗ if (root == (x->parent = y->parent)) {
372 ✗ root->left = x;
373 } else {
374 ✗ if (y == y->parent->left) {
375 ✗ y->parent->left = x;
376 } else {
377 ✗ y->parent->right = x;
378 }
379 }
380
381 /// @brief y should not be invalid_node in this case
382 /// y is the node to splice out and x is its child
383 ✗ if (y != z) {
384 ✗ y->max_high = -(std::numeric_limits<Scalar>::max)();
385 ✗ y->left = z->left;
386 ✗ y->right = z->right;
387 ✗ y->parent = z->parent;
388 ✗ z->left->parent = z->right->parent = y;
389 ✗ if (z == z->parent->left)
390 ✗ z->parent->left = y;
391 else
392 ✗ z->parent->right = y;
393
394 ✗ fixupMaxHigh(x->parent);
395 ✗ if (!(y->red)) {
396 ✗ y->red = z->red;
397 ✗ deleteFixup(x);
398 } else
399 ✗ y->red = z->red;
400 ✗ delete z;
401 } else {
402 ✗ fixupMaxHigh(x->parent);
403 ✗ if (!(y->red)) deleteFixup(x);
404 ✗ delete y;
405 }
406
407 ✗ return node_to_delete;
408 }
409
410 //==============================================================================
411 /// @brief returns 1 if the intervals overlap, and 0 otherwise
412 8180287 bool overlap(Scalar a1, Scalar a2, Scalar b1, Scalar b2) {
413
2/2
✓ Branch 0 taken 8044966 times.
✓ Branch 1 taken 135321 times.
 8180287 if (a1 <= b1) {
414 8044966 return (b1 <= a2);
415 } else {
416 135321 return (a1 <= b2);
417 }
418 }
419
420 //==============================================================================
421 19398 std::deque<SimpleInterval*> IntervalTree::query(Scalar low, Scalar high) {
422 19398 std::deque<SimpleInterval*> result_stack;
423 19398 IntervalTreeNode* x = root->left;
424 19398 bool run = (x != invalid_node);
425
426 19398 current_parent = 0;
427
428
2/2
✓ Branch 0 taken 8180287 times.
✓ Branch 1 taken 19398 times.
 8199685 while (run) {
429
3/4
✓ Branch 1 taken 8180287 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6549945 times.
✓ Branch 4 taken 1630342 times.
 8180287 if (overlap(low, high, x->key, x->high)) {
430
1/2
✓ Branch 1 taken 6549945 times.
✗ Branch 2 not taken.
 6549945 result_stack.push_back(x->stored_interval);
431 6549945 recursion_node_stack[current_parent].try_right_branch = true;
432 }
433
2/2
✓ Branch 0 taken 4360976 times.
✓ Branch 1 taken 3819311 times.
 8180287 if (x->left->max_high >= low) {
434
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 4360974 times.
 4360976 if (recursion_node_stack_top == recursion_node_stack_size) {
435 2 recursion_node_stack_size *= 2;
436 2 recursion_node_stack = (it_recursion_node*)realloc(
437 2 recursion_node_stack,
438 2 recursion_node_stack_size * sizeof(it_recursion_node));
439
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 if (recursion_node_stack == nullptr) abort();
440 }
441 4360976 recursion_node_stack[recursion_node_stack_top].start_node = x;
442 4360976 recursion_node_stack[recursion_node_stack_top].try_right_branch = false;
443 4360976 recursion_node_stack[recursion_node_stack_top].parent_index =
444 4360976 current_parent;
445 4360976 current_parent = recursion_node_stack_top++;
446 4360976 x = x->left;
447 } else
448 3819311 x = x->right;
449
450 8180287 run = (x != invalid_node);
451
4/4
✓ Branch 0 taken 4380374 times.
✓ Branch 1 taken 8160889 times.
✓ Branch 2 taken 4360976 times.
✓ Branch 3 taken 19398 times.
 12541263 while ((!run) && (recursion_node_stack_top > 1)) {
452
2/2
✓ Branch 0 taken 3701996 times.
✓ Branch 1 taken 658980 times.
 4360976 if (recursion_node_stack[--recursion_node_stack_top].try_right_branch) {
453 3701996 x = recursion_node_stack[recursion_node_stack_top].start_node->right;
454 3701996 current_parent =
455 3701996 recursion_node_stack[recursion_node_stack_top].parent_index;
456 3701996 recursion_node_stack[current_parent].try_right_branch = true;
457 3701996 run = (x != invalid_node);
458 }
459 }
460 }
461 19398 return result_stack;
462 }
463
464 } // namespace detail
465 } // namespace coal
466
467 #endif
468