box3d_rust/dynamic_tree/
insert.rs

1// Leaf insertion/removal, SAH sibling selection, rotations, and proxy
2// operations from dynamic_tree.c.
3// SPDX-FileCopyrightText: 2025 Erin Catto
4// SPDX-License-Identifier: MIT
5
6use super::{DynamicTree, ALLOCATED_NODE, ENLARGED_NODE, LEAF_NODE};
7use crate::aabb::{enlarge_aabb, perimeter};
8use crate::core::NULL_INDEX;
9use crate::math_functions::{
10    aabb_center, aabb_contains, aabb_union, is_valid_aabb, length_squared, min_float, sub, Aabb,
11};
12
13fn max_u16(a: u16, b: u16) -> u16 {
14    if a > b {
15        a
16    } else {
17        b
18    }
19}
20
21// Greedy algorithm for sibling selection using the SAH
22// We have three nodes A-(B,C) and want to add a leaf D, there are three choices.
23// 1: make a new parent for A and D : E-(A-(B,C), D)
24// 2: associate D with B
25//   a: B is a leaf : A-(E-(B,D), C)
26//   b: B is an internal node: A-(B{D},C)
27// 3: associate D with C
28//   a: C is a leaf : A-(B, E-(C,D))
29//   b: C is an internal node: A-(B, C{D})
30// All of these have a clear cost except when B or C is an internal node.
31// Hence we need to be greedy.
32//
33// The cost for cases 1, 2a, and 3a can be computed using the sibling cost
34// formula: cost of sibling H = area(union(H, D)) + increased area of ancestors
35//
36// Suppose B (or C) is an internal node, then the lowest cost would be one of
37// two cases:
38// case1: D becomes a sibling of B
39// case2: D becomes a descendant of B along with a new internal node of area(D).
40fn find_best_sibling(tree: &DynamicTree, box_d: Aabb) -> i32 {
41    let center_d = aabb_center(box_d);
42    let area_d = perimeter(box_d);
43
44    let nodes = &tree.nodes;
45    let root_index = tree.root;
46
47    let root_box = nodes[root_index as usize].aabb;
48
49    // Area of current node
50    let mut area_base = perimeter(root_box);
51
52    // Area of inflated node
53    let mut direct_cost = perimeter(aabb_union(root_box, box_d));
54    let mut inherited_cost = 0.0;
55
56    let mut best_sibling = root_index;
57    let mut best_cost = direct_cost;
58
59    // Descend the tree from root, following a single greedy path.
60    let mut index = root_index;
61    while !nodes[index as usize].is_leaf() {
62        let child1 = nodes[index as usize].child1;
63        let child2 = nodes[index as usize].child2;
64
65        // Cost of creating a new parent for this node and the new leaf
66        let cost = direct_cost + inherited_cost;
67
68        // Sometimes there are multiple identical costs within tolerance.
69        // This breaks the ties using the centroid distance.
70        if cost < best_cost {
71            best_sibling = index;
72            best_cost = cost;
73        }
74
75        // Inheritance cost seen by children
76        inherited_cost += direct_cost - area_base;
77
78        let leaf1 = nodes[child1 as usize].is_leaf();
79        let leaf2 = nodes[child2 as usize].is_leaf();
80
81        // Cost of descending into child 1
82        let mut lower_cost1 = f32::MAX;
83        let box1 = nodes[child1 as usize].aabb;
84        let direct_cost1 = perimeter(aabb_union(box1, box_d));
85        let mut area1 = 0.0;
86        if leaf1 {
87            // Child 1 is a leaf
88            // Cost of creating new node and increasing area of node P
89            let cost1 = direct_cost1 + inherited_cost;
90
91            // Need this here due to while condition above
92            if cost1 < best_cost {
93                best_sibling = child1;
94                best_cost = cost1;
95            }
96        } else {
97            // Child 1 is an internal node
98            area1 = perimeter(box1);
99
100            // Lower bound cost of inserting under child 1.
101            lower_cost1 = inherited_cost + direct_cost1 + min_float(area_d - area1, 0.0);
102        }
103
104        // Cost of descending into child 2
105        let mut lower_cost2 = f32::MAX;
106        let box2 = nodes[child2 as usize].aabb;
107        let direct_cost2 = perimeter(aabb_union(box2, box_d));
108        let mut area2 = 0.0;
109        if leaf2 {
110            // Child 2 is a leaf
111            // Cost of creating new node and increasing area of node P
112            let cost2 = direct_cost2 + inherited_cost;
113
114            // Need this here due to while condition above
115            if cost2 < best_cost {
116                best_sibling = child2;
117                best_cost = cost2;
118            }
119        } else {
120            // Child 2 is an internal node
121            area2 = perimeter(box2);
122
123            // Lower bound cost of inserting under child 2.
124            lower_cost2 = inherited_cost + direct_cost2 + min_float(area_d - area2, 0.0);
125        }
126
127        if leaf1 && leaf2 {
128            break;
129        }
130
131        // Can the cost possibly be decreased?
132        if best_cost <= lower_cost1 && best_cost <= lower_cost2 {
133            break;
134        }
135
136        if lower_cost1 == lower_cost2 && !leaf1 {
137            debug_assert!(lower_cost1 < f32::MAX);
138            debug_assert!(lower_cost2 < f32::MAX);
139
140            // No clear choice based on lower bound surface area. This can
141            // happen when both children fully contain D. Fall back to node
142            // distance.
143            let d1 = sub(aabb_center(box1), center_d);
144            let d2 = sub(aabb_center(box2), center_d);
145            lower_cost1 = length_squared(d1);
146            lower_cost2 = length_squared(d2);
147        }
148
149        // Descend
150        if lower_cost1 < lower_cost2 && !leaf1 {
151            index = child1;
152            area_base = area1;
153            direct_cost = direct_cost1;
154        } else {
155            index = child2;
156            area_base = area2;
157            direct_cost = direct_cost2;
158        }
159
160        debug_assert!(!nodes[index as usize].is_leaf());
161    }
162
163    best_sibling
164}
165
166/// (enum b3RotateType)
167enum RotateType {
168    None,
169    Bf,
170    Bg,
171    Cd,
172    Ce,
173}
174
175impl DynamicTree {
176    /// Perform a left or right rotation if node A is imbalanced.
177    /// (static b3RotateNodes)
178    fn rotate_nodes(&mut self, i_a: i32) {
179        debug_assert!(i_a != NULL_INDEX);
180
181        let nodes = &mut self.nodes;
182
183        if nodes[i_a as usize].is_leaf() {
184            return;
185        }
186
187        let i_b = nodes[i_a as usize].child1;
188        let i_c = nodes[i_a as usize].child2;
189        debug_assert!(0 <= i_b && (i_b as usize) < nodes.len());
190        debug_assert!(0 <= i_c && (i_c as usize) < nodes.len());
191
192        let (ia, ib, ic) = (i_a as usize, i_b as usize, i_c as usize);
193
194        let is_leaf_b = nodes[ib].is_leaf();
195        let is_leaf_c = nodes[ic].is_leaf();
196
197        if is_leaf_b && !is_leaf_c {
198            // B is a leaf and C is internal
199            let i_f = nodes[ic].child1;
200            let i_g = nodes[ic].child2;
201            let (if_, ig) = (i_f as usize, i_g as usize);
202            debug_assert!(0 <= i_f && (i_f as usize) < nodes.len());
203            debug_assert!(0 <= i_g && (i_g as usize) < nodes.len());
204
205            // Base cost
206            let cost_base = perimeter(nodes[ic].aabb);
207
208            // Cost of swapping B and F
209            let aabb_bg = aabb_union(nodes[ib].aabb, nodes[ig].aabb);
210            let cost_bf = perimeter(aabb_bg);
211
212            // Cost of swapping B and G
213            let aabb_bf = aabb_union(nodes[ib].aabb, nodes[if_].aabb);
214            let cost_bg = perimeter(aabb_bf);
215
216            if cost_base < cost_bf && cost_base < cost_bg {
217                // Rotation does not improve cost
218                return;
219            }
220
221            if cost_bf < cost_bg {
222                // Swap B and F
223                nodes[ia].child1 = i_f;
224                nodes[ic].child1 = i_b;
225
226                nodes[ib].parent = i_c;
227                nodes[if_].parent = i_a;
228
229                nodes[ic].aabb = aabb_bg;
230
231                nodes[ic].height = 1 + max_u16(nodes[ib].height, nodes[ig].height);
232                nodes[ia].height = 1 + max_u16(nodes[ic].height, nodes[if_].height);
233                nodes[ic].category_bits = nodes[ib].category_bits | nodes[ig].category_bits;
234                nodes[ia].category_bits = nodes[ic].category_bits | nodes[if_].category_bits;
235                nodes[ic].flags |= (nodes[ib].flags | nodes[ig].flags) & ENLARGED_NODE;
236                nodes[ia].flags |= (nodes[ic].flags | nodes[if_].flags) & ENLARGED_NODE;
237            } else {
238                // Swap B and G
239                nodes[ia].child1 = i_g;
240                nodes[ic].child2 = i_b;
241
242                nodes[ib].parent = i_c;
243                nodes[ig].parent = i_a;
244
245                nodes[ic].aabb = aabb_bf;
246
247                nodes[ic].height = 1 + max_u16(nodes[ib].height, nodes[if_].height);
248                nodes[ia].height = 1 + max_u16(nodes[ic].height, nodes[ig].height);
249                nodes[ic].category_bits = nodes[ib].category_bits | nodes[if_].category_bits;
250                nodes[ia].category_bits = nodes[ic].category_bits | nodes[ig].category_bits;
251                nodes[ic].flags |= (nodes[ib].flags | nodes[if_].flags) & ENLARGED_NODE;
252                nodes[ia].flags |= (nodes[ic].flags | nodes[ig].flags) & ENLARGED_NODE;
253            }
254        } else if is_leaf_c && !is_leaf_b {
255            // C is a leaf and B is internal
256            let i_d = nodes[ib].child1;
257            let i_e = nodes[ib].child2;
258            let (id, ie) = (i_d as usize, i_e as usize);
259            debug_assert!(0 <= i_d && (i_d as usize) < nodes.len());
260            debug_assert!(0 <= i_e && (i_e as usize) < nodes.len());
261
262            // Base cost
263            let cost_base = perimeter(nodes[ib].aabb);
264
265            // Cost of swapping C and D
266            let aabb_ce = aabb_union(nodes[ic].aabb, nodes[ie].aabb);
267            let cost_cd = perimeter(aabb_ce);
268
269            // Cost of swapping C and E
270            let aabb_cd = aabb_union(nodes[ic].aabb, nodes[id].aabb);
271            let cost_ce = perimeter(aabb_cd);
272
273            if cost_base < cost_cd && cost_base < cost_ce {
274                // Rotation does not improve cost
275                return;
276            }
277
278            if cost_cd < cost_ce {
279                // Swap C and D
280                nodes[ia].child2 = i_d;
281                nodes[ib].child1 = i_c;
282
283                nodes[ic].parent = i_b;
284                nodes[id].parent = i_a;
285
286                nodes[ib].aabb = aabb_ce;
287
288                nodes[ib].height = 1 + max_u16(nodes[ic].height, nodes[ie].height);
289                nodes[ia].height = 1 + max_u16(nodes[ib].height, nodes[id].height);
290                nodes[ib].category_bits = nodes[ic].category_bits | nodes[ie].category_bits;
291                nodes[ia].category_bits = nodes[ib].category_bits | nodes[id].category_bits;
292                nodes[ib].flags |= (nodes[ic].flags | nodes[ie].flags) & ENLARGED_NODE;
293                nodes[ia].flags |= (nodes[ib].flags | nodes[id].flags) & ENLARGED_NODE;
294            } else {
295                // Swap C and E
296                nodes[ia].child2 = i_e;
297                nodes[ib].child2 = i_c;
298
299                nodes[ic].parent = i_b;
300                nodes[ie].parent = i_a;
301
302                nodes[ib].aabb = aabb_cd;
303                nodes[ib].height = 1 + max_u16(nodes[ic].height, nodes[id].height);
304                nodes[ia].height = 1 + max_u16(nodes[ib].height, nodes[ie].height);
305                nodes[ib].category_bits = nodes[ic].category_bits | nodes[id].category_bits;
306                nodes[ia].category_bits = nodes[ib].category_bits | nodes[ie].category_bits;
307                nodes[ib].flags |= (nodes[ic].flags | nodes[id].flags) & ENLARGED_NODE;
308                nodes[ia].flags |= (nodes[ib].flags | nodes[ie].flags) & ENLARGED_NODE;
309            }
310        } else if !is_leaf_b && !is_leaf_c {
311            let i_d = nodes[ib].child1;
312            let i_e = nodes[ib].child2;
313            let i_f = nodes[ic].child1;
314            let i_g = nodes[ic].child2;
315
316            let (id, ie, if_, ig) = (i_d as usize, i_e as usize, i_f as usize, i_g as usize);
317
318            debug_assert!(0 <= i_d && (i_d as usize) < nodes.len());
319            debug_assert!(0 <= i_e && (i_e as usize) < nodes.len());
320            debug_assert!(0 <= i_f && (i_f as usize) < nodes.len());
321            debug_assert!(0 <= i_g && (i_g as usize) < nodes.len());
322
323            // Base cost
324            let area_b = perimeter(nodes[ib].aabb);
325            let area_c = perimeter(nodes[ic].aabb);
326            let cost_base = area_b + area_c;
327            let mut best_rotation = RotateType::None;
328            let mut best_cost = cost_base;
329
330            // Cost of swapping B and F
331            let aabb_bg = aabb_union(nodes[ib].aabb, nodes[ig].aabb);
332            let cost_bf = area_b + perimeter(aabb_bg);
333            if cost_bf < best_cost {
334                best_rotation = RotateType::Bf;
335                best_cost = cost_bf;
336            }
337
338            // Cost of swapping B and G
339            let aabb_bf = aabb_union(nodes[ib].aabb, nodes[if_].aabb);
340            let cost_bg = area_b + perimeter(aabb_bf);
341            if cost_bg < best_cost {
342                best_rotation = RotateType::Bg;
343                best_cost = cost_bg;
344            }
345
346            // Cost of swapping C and D
347            let aabb_ce = aabb_union(nodes[ic].aabb, nodes[ie].aabb);
348            let cost_cd = area_c + perimeter(aabb_ce);
349            if cost_cd < best_cost {
350                best_rotation = RotateType::Cd;
351                best_cost = cost_cd;
352            }
353
354            // Cost of swapping C and E
355            let aabb_cd = aabb_union(nodes[ic].aabb, nodes[id].aabb);
356            let cost_ce = area_c + perimeter(aabb_cd);
357            if cost_ce < best_cost {
358                best_rotation = RotateType::Ce;
359                // best_cost = cost_ce;
360            }
361
362            match best_rotation {
363                RotateType::None => {}
364
365                RotateType::Bf => {
366                    nodes[ia].child1 = i_f;
367                    nodes[ic].child1 = i_b;
368
369                    nodes[ib].parent = i_c;
370                    nodes[if_].parent = i_a;
371
372                    nodes[ic].aabb = aabb_bg;
373                    nodes[ic].height = 1 + max_u16(nodes[ib].height, nodes[ig].height);
374                    nodes[ia].height = 1 + max_u16(nodes[ic].height, nodes[if_].height);
375                    nodes[ic].category_bits = nodes[ib].category_bits | nodes[ig].category_bits;
376                    nodes[ia].category_bits = nodes[ic].category_bits | nodes[if_].category_bits;
377                    nodes[ic].flags |= (nodes[ib].flags | nodes[ig].flags) & ENLARGED_NODE;
378                    nodes[ia].flags |= (nodes[ic].flags | nodes[if_].flags) & ENLARGED_NODE;
379                }
380
381                RotateType::Bg => {
382                    nodes[ia].child1 = i_g;
383                    nodes[ic].child2 = i_b;
384
385                    nodes[ib].parent = i_c;
386                    nodes[ig].parent = i_a;
387
388                    nodes[ic].aabb = aabb_bf;
389                    nodes[ic].height = 1 + max_u16(nodes[ib].height, nodes[if_].height);
390                    nodes[ia].height = 1 + max_u16(nodes[ic].height, nodes[ig].height);
391                    nodes[ic].category_bits = nodes[ib].category_bits | nodes[if_].category_bits;
392                    nodes[ia].category_bits = nodes[ic].category_bits | nodes[ig].category_bits;
393                    nodes[ic].flags |= (nodes[ib].flags | nodes[if_].flags) & ENLARGED_NODE;
394                    nodes[ia].flags |= (nodes[ic].flags | nodes[ig].flags) & ENLARGED_NODE;
395                }
396
397                RotateType::Cd => {
398                    nodes[ia].child2 = i_d;
399                    nodes[ib].child1 = i_c;
400
401                    nodes[ic].parent = i_b;
402                    nodes[id].parent = i_a;
403
404                    nodes[ib].aabb = aabb_ce;
405                    nodes[ib].height = 1 + max_u16(nodes[ic].height, nodes[ie].height);
406                    nodes[ia].height = 1 + max_u16(nodes[ib].height, nodes[id].height);
407                    nodes[ib].category_bits = nodes[ic].category_bits | nodes[ie].category_bits;
408                    nodes[ia].category_bits = nodes[ib].category_bits | nodes[id].category_bits;
409                    nodes[ib].flags |= (nodes[ic].flags | nodes[ie].flags) & ENLARGED_NODE;
410                    nodes[ia].flags |= (nodes[ib].flags | nodes[id].flags) & ENLARGED_NODE;
411                }
412
413                RotateType::Ce => {
414                    nodes[ia].child2 = i_e;
415                    nodes[ib].child2 = i_c;
416
417                    nodes[ic].parent = i_b;
418                    nodes[ie].parent = i_a;
419
420                    nodes[ib].aabb = aabb_cd;
421                    nodes[ib].height = 1 + max_u16(nodes[ic].height, nodes[id].height);
422                    nodes[ia].height = 1 + max_u16(nodes[ib].height, nodes[ie].height);
423                    nodes[ib].category_bits = nodes[ic].category_bits | nodes[id].category_bits;
424                    nodes[ia].category_bits = nodes[ib].category_bits | nodes[ie].category_bits;
425                    nodes[ib].flags |= (nodes[ic].flags | nodes[id].flags) & ENLARGED_NODE;
426                    nodes[ia].flags |= (nodes[ib].flags | nodes[ie].flags) & ENLARGED_NODE;
427                }
428            }
429        }
430    }
431
432    /// (static b3InsertLeaf)
433    fn insert_leaf(&mut self, leaf: i32, should_rotate: bool) {
434        if self.root == NULL_INDEX {
435            self.root = leaf;
436            self.nodes[self.root as usize].parent = NULL_INDEX;
437            return;
438        }
439
440        // Stage 1: find the best sibling for this node
441        let leaf_aabb = self.nodes[leaf as usize].aabb;
442        let sibling = find_best_sibling(self, leaf_aabb);
443
444        // Stage 2: create a new parent for the leaf and sibling
445        let old_parent = self.nodes[sibling as usize].parent;
446        let new_parent = self.allocate_node();
447
448        // warning: node pointer can change after allocation
449        let nodes = &mut self.nodes;
450        let np = new_parent as usize;
451        nodes[np].parent = old_parent;
452        nodes[np].user_data = u64::MAX;
453        nodes[np].aabb = aabb_union(leaf_aabb, nodes[sibling as usize].aabb);
454        nodes[np].category_bits =
455            nodes[leaf as usize].category_bits | nodes[sibling as usize].category_bits;
456        nodes[np].height = nodes[sibling as usize].height + 1;
457
458        if old_parent != NULL_INDEX {
459            // The sibling was not the root.
460            if nodes[old_parent as usize].child1 == sibling {
461                nodes[old_parent as usize].child1 = new_parent;
462            } else {
463                nodes[old_parent as usize].child2 = new_parent;
464            }
465
466            nodes[np].child1 = sibling;
467            nodes[np].child2 = leaf;
468            nodes[sibling as usize].parent = new_parent;
469            nodes[leaf as usize].parent = new_parent;
470        } else {
471            // The sibling was the root.
472            nodes[np].child1 = sibling;
473            nodes[np].child2 = leaf;
474            nodes[sibling as usize].parent = new_parent;
475            nodes[leaf as usize].parent = new_parent;
476            self.root = new_parent;
477        }
478
479        // Stage 3: walk back up the tree fixing heights and AABBs
480        let mut index = self.nodes[leaf as usize].parent;
481        while index != NULL_INDEX {
482            let child1 = self.nodes[index as usize].child1;
483            let child2 = self.nodes[index as usize].child2;
484
485            debug_assert!(child1 != NULL_INDEX);
486            debug_assert!(child2 != NULL_INDEX);
487
488            let (c1, c2) = (child1 as usize, child2 as usize);
489            self.nodes[index as usize].aabb = aabb_union(self.nodes[c1].aabb, self.nodes[c2].aabb);
490            self.nodes[index as usize].category_bits =
491                self.nodes[c1].category_bits | self.nodes[c2].category_bits;
492            self.nodes[index as usize].height =
493                1 + max_u16(self.nodes[c1].height, self.nodes[c2].height);
494            self.nodes[index as usize].flags |=
495                (self.nodes[c1].flags | self.nodes[c2].flags) & ENLARGED_NODE;
496
497            if should_rotate {
498                self.rotate_nodes(index);
499            }
500
501            index = self.nodes[index as usize].parent;
502        }
503    }
504
505    /// (static b3RemoveLeaf)
506    pub(crate) fn remove_leaf(&mut self, leaf: i32) {
507        if leaf == self.root {
508            self.root = NULL_INDEX;
509            return;
510        }
511
512        let parent = self.nodes[leaf as usize].parent;
513        let grand_parent = self.nodes[parent as usize].parent;
514        let sibling = if self.nodes[parent as usize].child1 == leaf {
515            self.nodes[parent as usize].child2
516        } else {
517            self.nodes[parent as usize].child1
518        };
519
520        if grand_parent != NULL_INDEX {
521            // Destroy parent and connect sibling to grandParent.
522            if self.nodes[grand_parent as usize].child1 == parent {
523                self.nodes[grand_parent as usize].child1 = sibling;
524            } else {
525                self.nodes[grand_parent as usize].child2 = sibling;
526            }
527            self.nodes[sibling as usize].parent = grand_parent;
528            self.free_node(parent);
529
530            // Adjust ancestor bounds.
531            let mut index = grand_parent;
532            while index != NULL_INDEX {
533                let child1 = self.nodes[index as usize].child1 as usize;
534                let child2 = self.nodes[index as usize].child2 as usize;
535
536                self.nodes[index as usize].aabb =
537                    aabb_union(self.nodes[child1].aabb, self.nodes[child2].aabb);
538                self.nodes[index as usize].category_bits =
539                    self.nodes[child1].category_bits | self.nodes[child2].category_bits;
540                self.nodes[index as usize].height =
541                    1 + max_u16(self.nodes[child1].height, self.nodes[child2].height);
542
543                index = self.nodes[index as usize].parent;
544            }
545        } else {
546            self.root = sibling;
547            self.nodes[sibling as usize].parent = NULL_INDEX;
548            self.free_node(parent);
549        }
550    }
551
552    /// Create a proxy in the tree as a leaf node. Returns the node index.
553    /// (b3DynamicTree_CreateProxy)
554    pub fn create_proxy(&mut self, aabb: Aabb, category_bits: u64, user_data: u64) -> i32 {
555        debug_assert!(is_valid_aabb(aabb));
556
557        let proxy_id = self.allocate_node();
558        let node = &mut self.nodes[proxy_id as usize];
559
560        node.aabb = aabb;
561        node.user_data = user_data;
562        node.category_bits = category_bits;
563        node.height = 0;
564        node.flags = ALLOCATED_NODE | LEAF_NODE;
565
566        let should_rotate = true;
567        self.insert_leaf(proxy_id, should_rotate);
568
569        self.proxy_count += 1;
570
571        proxy_id
572    }
573
574    /// Destroy a proxy. This asserts if the id is invalid.
575    /// (b3DynamicTree_DestroyProxy)
576    pub fn destroy_proxy(&mut self, proxy_id: i32) {
577        debug_assert!(0 <= proxy_id && proxy_id < self.node_capacity());
578        debug_assert!(self.nodes[proxy_id as usize].is_leaf());
579
580        self.remove_leaf(proxy_id);
581        self.free_node(proxy_id);
582
583        debug_assert!(self.proxy_count > 0);
584        self.proxy_count -= 1;
585    }
586
587    /// Move a proxy to a new AABB by removing and reinserting into the tree.
588    /// (b3DynamicTree_MoveProxy)
589    pub fn move_proxy(&mut self, proxy_id: i32, aabb: Aabb) {
590        debug_assert!(is_valid_aabb(aabb));
591        debug_assert!(0 <= proxy_id && proxy_id < self.node_capacity());
592        debug_assert!(self.nodes[proxy_id as usize].is_leaf());
593
594        self.remove_leaf(proxy_id);
595
596        self.nodes[proxy_id as usize].aabb = aabb;
597
598        let should_rotate = false;
599        self.insert_leaf(proxy_id, should_rotate);
600    }
601
602    /// Enlarge a proxy and enlarge ancestors as necessary.
603    /// (b3DynamicTree_EnlargeProxy)
604    pub fn enlarge_proxy(&mut self, proxy_id: i32, aabb: Aabb) {
605        debug_assert!(is_valid_aabb(aabb));
606        debug_assert!(0 <= proxy_id && proxy_id < self.node_capacity());
607        debug_assert!(self.nodes[proxy_id as usize].is_leaf());
608
609        // Caller must ensure this
610        debug_assert!(!aabb_contains(self.nodes[proxy_id as usize].aabb, aabb));
611
612        self.nodes[proxy_id as usize].aabb = aabb;
613
614        let mut parent_index = self.nodes[proxy_id as usize].parent;
615        while parent_index != NULL_INDEX {
616            let changed = enlarge_aabb(&mut self.nodes[parent_index as usize].aabb, aabb);
617            self.nodes[parent_index as usize].flags |= ENLARGED_NODE;
618            parent_index = self.nodes[parent_index as usize].parent;
619
620            if !changed {
621                break;
622            }
623        }
624
625        while parent_index != NULL_INDEX {
626            if self.nodes[parent_index as usize].flags & ENLARGED_NODE != 0 {
627                // early out because this ancestor was previously ascended and
628                // marked as enlarged
629                break;
630            }
631
632            self.nodes[parent_index as usize].flags |= ENLARGED_NODE;
633            parent_index = self.nodes[parent_index as usize].parent;
634        }
635    }
636
637    /// Modify the category bits on a proxy. This is an expensive operation.
638    /// (b3DynamicTree_SetCategoryBits)
639    pub fn set_category_bits(&mut self, proxy_id: i32, category_bits: u64) {
640        debug_assert!(self.nodes[proxy_id as usize].is_leaf());
641
642        self.nodes[proxy_id as usize].category_bits = category_bits;
643
644        // Fix up category bits in ancestor internal nodes
645        let mut node_index = self.nodes[proxy_id as usize].parent;
646        while node_index != NULL_INDEX {
647            let child1 = self.nodes[node_index as usize].child1;
648            debug_assert!(child1 != NULL_INDEX);
649            let child2 = self.nodes[node_index as usize].child2;
650            debug_assert!(child2 != NULL_INDEX);
651            self.nodes[node_index as usize].category_bits = self.nodes[child1 as usize]
652                .category_bits
653                | self.nodes[child2 as usize].category_bits;
654
655            node_index = self.nodes[node_index as usize].parent;
656        }
657    }
658}
box3d_rust/dynamic_tree/insert.rs

box3d_rust/dynamic_tree/
insert.rs
