box2d_rust/dynamic_tree/
insert.rs

1// Leaf insertion/removal, SAH sibling selection, rotations, and proxy
2// operations from dynamic_tree.c.
3// SPDX-FileCopyrightText: 2023 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].height > 0 {
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].height == 0;
79        let leaf2 = nodes[child2 as usize].height == 0;
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. The minimum accounts
101            // for two possibilities:
102            // 1. Child1 could be the sibling with cost1 = inheritedCost + directCost1
103            // 2. A descendant of child1 could be the sibling with the lower
104            //    bound cost of cost1 = inheritedCost + (directCost1 - area1) + areaD
105            lower_cost1 = inherited_cost + direct_cost1 + min_float(area_d - area1, 0.0);
106        }
107
108        // Cost of descending into child 2
109        let mut lower_cost2 = f32::MAX;
110        let box2 = nodes[child2 as usize].aabb;
111        let direct_cost2 = perimeter(aabb_union(box2, box_d));
112        let mut area2 = 0.0;
113        if leaf2 {
114            let cost2 = direct_cost2 + inherited_cost;
115
116            if cost2 < best_cost {
117                best_sibling = child2;
118                best_cost = cost2;
119            }
120        } else {
121            area2 = perimeter(box2);
122            lower_cost2 = inherited_cost + direct_cost2 + min_float(area_d - area2, 0.0);
123        }
124
125        if leaf1 && leaf2 {
126            break;
127        }
128
129        // Can the cost possibly be decreased?
130        if best_cost <= lower_cost1 && best_cost <= lower_cost2 {
131            break;
132        }
133
134        if lower_cost1 == lower_cost2 && !leaf1 {
135            debug_assert!(lower_cost1 < f32::MAX);
136            debug_assert!(lower_cost2 < f32::MAX);
137
138            // No clear choice based on lower bound surface area. This can
139            // happen when both children fully contain D. Fall back to node
140            // distance.
141            let d1 = sub(aabb_center(box1), center_d);
142            let d2 = sub(aabb_center(box2), center_d);
143            lower_cost1 = length_squared(d1);
144            lower_cost2 = length_squared(d2);
145        }
146
147        // Descend
148        if lower_cost1 < lower_cost2 && !leaf1 {
149            index = child1;
150            area_base = area1;
151            direct_cost = direct_cost1;
152        } else {
153            index = child2;
154            area_base = area2;
155            direct_cost = direct_cost2;
156        }
157
158        debug_assert!(nodes[index as usize].height > 0);
159    }
160
161    best_sibling
162}
163
164/// (enum b2RotateType)
165enum RotateType {
166    None,
167    Bf,
168    Bg,
169    Cd,
170    Ce,
171}
172
173impl DynamicTree {
174    /// Perform a left or right rotation if node A is imbalanced.
175    /// (static b2RotateNodes)
176    fn rotate_nodes(&mut self, i_a: i32) {
177        debug_assert!(i_a != NULL_INDEX);
178
179        let nodes = &mut self.nodes;
180
181        if nodes[i_a as usize].height < 2 {
182            return;
183        }
184
185        let i_b = nodes[i_a as usize].child1;
186        let i_c = nodes[i_a as usize].child2;
187        debug_assert!(0 <= i_b && (i_b as usize) < nodes.len());
188        debug_assert!(0 <= i_c && (i_c as usize) < nodes.len());
189
190        let (ia, ib, ic) = (i_a as usize, i_b as usize, i_c as usize);
191
192        if nodes[ib].height == 0 {
193            // B is a leaf and C is internal
194            debug_assert!(nodes[ic].height > 0);
195
196            let i_f = nodes[ic].child1;
197            let i_g = nodes[ic].child2;
198            let (if_, ig) = (i_f as usize, i_g as usize);
199            debug_assert!(0 <= i_f && (i_f as usize) < nodes.len());
200            debug_assert!(0 <= i_g && (i_g as usize) < nodes.len());
201
202            // Base cost
203            let cost_base = perimeter(nodes[ic].aabb);
204
205            // Cost of swapping B and F
206            let aabb_bg = aabb_union(nodes[ib].aabb, nodes[ig].aabb);
207            let cost_bf = perimeter(aabb_bg);
208
209            // Cost of swapping B and G
210            let aabb_bf = aabb_union(nodes[ib].aabb, nodes[if_].aabb);
211            let cost_bg = perimeter(aabb_bf);
212
213            if cost_base < cost_bf && cost_base < cost_bg {
214                // Rotation does not improve cost
215                return;
216            }
217
218            if cost_bf < cost_bg {
219                // Swap B and F
220                nodes[ia].child1 = i_f;
221                nodes[ic].child1 = i_b;
222
223                nodes[ib].parent = i_c;
224                nodes[if_].parent = i_a;
225
226                nodes[ic].aabb = aabb_bg;
227
228                nodes[ic].height = 1 + max_u16(nodes[ib].height, nodes[ig].height);
229                nodes[ia].height = 1 + max_u16(nodes[ic].height, nodes[if_].height);
230                nodes[ic].category_bits = nodes[ib].category_bits | nodes[ig].category_bits;
231                nodes[ia].category_bits = nodes[ic].category_bits | nodes[if_].category_bits;
232                nodes[ic].flags |= (nodes[ib].flags | nodes[ig].flags) & ENLARGED_NODE;
233                nodes[ia].flags |= (nodes[ic].flags | nodes[if_].flags) & ENLARGED_NODE;
234            } else {
235                // Swap B and G
236                nodes[ia].child1 = i_g;
237                nodes[ic].child2 = i_b;
238
239                nodes[ib].parent = i_c;
240                nodes[ig].parent = i_a;
241
242                nodes[ic].aabb = aabb_bf;
243
244                nodes[ic].height = 1 + max_u16(nodes[ib].height, nodes[if_].height);
245                nodes[ia].height = 1 + max_u16(nodes[ic].height, nodes[ig].height);
246                nodes[ic].category_bits = nodes[ib].category_bits | nodes[if_].category_bits;
247                nodes[ia].category_bits = nodes[ic].category_bits | nodes[ig].category_bits;
248                nodes[ic].flags |= (nodes[ib].flags | nodes[if_].flags) & ENLARGED_NODE;
249                nodes[ia].flags |= (nodes[ic].flags | nodes[ig].flags) & ENLARGED_NODE;
250            }
251        } else if nodes[ic].height == 0 {
252            // C is a leaf and B is internal
253            debug_assert!(nodes[ib].height > 0);
254
255            let i_d = nodes[ib].child1;
256            let i_e = nodes[ib].child2;
257            let (id, ie) = (i_d as usize, i_e as usize);
258            debug_assert!(0 <= i_d && (i_d as usize) < nodes.len());
259            debug_assert!(0 <= i_e && (i_e as usize) < nodes.len());
260
261            // Base cost
262            let cost_base = perimeter(nodes[ib].aabb);
263
264            // Cost of swapping C and D
265            let aabb_ce = aabb_union(nodes[ic].aabb, nodes[ie].aabb);
266            let cost_cd = perimeter(aabb_ce);
267
268            // Cost of swapping C and E
269            let aabb_cd = aabb_union(nodes[ic].aabb, nodes[id].aabb);
270            let cost_ce = perimeter(aabb_cd);
271
272            if cost_base < cost_cd && cost_base < cost_ce {
273                // Rotation does not improve cost
274                return;
275            }
276
277            if cost_cd < cost_ce {
278                // Swap C and D
279                nodes[ia].child2 = i_d;
280                nodes[ib].child1 = i_c;
281
282                nodes[ic].parent = i_b;
283                nodes[id].parent = i_a;
284
285                nodes[ib].aabb = aabb_ce;
286
287                nodes[ib].height = 1 + max_u16(nodes[ic].height, nodes[ie].height);
288                nodes[ia].height = 1 + max_u16(nodes[ib].height, nodes[id].height);
289                nodes[ib].category_bits = nodes[ic].category_bits | nodes[ie].category_bits;
290                nodes[ia].category_bits = nodes[ib].category_bits | nodes[id].category_bits;
291                nodes[ib].flags |= (nodes[ic].flags | nodes[ie].flags) & ENLARGED_NODE;
292                nodes[ia].flags |= (nodes[ib].flags | nodes[id].flags) & ENLARGED_NODE;
293            } else {
294                // Swap C and E
295                nodes[ia].child2 = i_e;
296                nodes[ib].child2 = i_c;
297
298                nodes[ic].parent = i_b;
299                nodes[ie].parent = i_a;
300
301                nodes[ib].aabb = aabb_cd;
302                nodes[ib].height = 1 + max_u16(nodes[ic].height, nodes[id].height);
303                nodes[ia].height = 1 + max_u16(nodes[ib].height, nodes[ie].height);
304                nodes[ib].category_bits = nodes[ic].category_bits | nodes[id].category_bits;
305                nodes[ia].category_bits = nodes[ib].category_bits | nodes[ie].category_bits;
306                nodes[ib].flags |= (nodes[ic].flags | nodes[id].flags) & ENLARGED_NODE;
307                nodes[ia].flags |= (nodes[ib].flags | nodes[ie].flags) & ENLARGED_NODE;
308            }
309        } else {
310            let i_d = nodes[ib].child1;
311            let i_e = nodes[ib].child2;
312            let i_f = nodes[ic].child1;
313            let i_g = nodes[ic].child2;
314
315            let (id, ie, if_, ig) = (i_d as usize, i_e as usize, i_f as usize, i_g as usize);
316
317            debug_assert!(0 <= i_d && (i_d as usize) < nodes.len());
318            debug_assert!(0 <= i_e && (i_e as usize) < nodes.len());
319            debug_assert!(0 <= i_f && (i_f as usize) < nodes.len());
320            debug_assert!(0 <= i_g && (i_g as usize) < nodes.len());
321
322            // Base cost
323            let area_b = perimeter(nodes[ib].aabb);
324            let area_c = perimeter(nodes[ic].aabb);
325            let cost_base = area_b + area_c;
326            let mut best_rotation = RotateType::None;
327            let mut best_cost = cost_base;
328
329            // Cost of swapping B and F
330            let aabb_bg = aabb_union(nodes[ib].aabb, nodes[ig].aabb);
331            let cost_bf = area_b + perimeter(aabb_bg);
332            if cost_bf < best_cost {
333                best_rotation = RotateType::Bf;
334                best_cost = cost_bf;
335            }
336
337            // Cost of swapping B and G
338            let aabb_bf = aabb_union(nodes[ib].aabb, nodes[if_].aabb);
339            let cost_bg = area_b + perimeter(aabb_bf);
340            if cost_bg < best_cost {
341                best_rotation = RotateType::Bg;
342                best_cost = cost_bg;
343            }
344
345            // Cost of swapping C and D
346            let aabb_ce = aabb_union(nodes[ic].aabb, nodes[ie].aabb);
347            let cost_cd = area_c + perimeter(aabb_ce);
348            if cost_cd < best_cost {
349                best_rotation = RotateType::Cd;
350                best_cost = cost_cd;
351            }
352
353            // Cost of swapping C and E
354            let aabb_cd = aabb_union(nodes[ic].aabb, nodes[id].aabb);
355            let cost_ce = area_c + perimeter(aabb_cd);
356            if cost_ce < best_cost {
357                best_rotation = RotateType::Ce;
358                // best_cost = cost_ce;
359            }
360
361            match best_rotation {
362                RotateType::None => {}
363
364                RotateType::Bf => {
365                    nodes[ia].child1 = i_f;
366                    nodes[ic].child1 = i_b;
367
368                    nodes[ib].parent = i_c;
369                    nodes[if_].parent = i_a;
370
371                    nodes[ic].aabb = aabb_bg;
372                    nodes[ic].height = 1 + max_u16(nodes[ib].height, nodes[ig].height);
373                    nodes[ia].height = 1 + max_u16(nodes[ic].height, nodes[if_].height);
374                    nodes[ic].category_bits = nodes[ib].category_bits | nodes[ig].category_bits;
375                    nodes[ia].category_bits = nodes[ic].category_bits | nodes[if_].category_bits;
376                    nodes[ic].flags |= (nodes[ib].flags | nodes[ig].flags) & ENLARGED_NODE;
377                    nodes[ia].flags |= (nodes[ic].flags | nodes[if_].flags) & ENLARGED_NODE;
378                }
379
380                RotateType::Bg => {
381                    nodes[ia].child1 = i_g;
382                    nodes[ic].child2 = i_b;
383
384                    nodes[ib].parent = i_c;
385                    nodes[ig].parent = i_a;
386
387                    nodes[ic].aabb = aabb_bf;
388                    nodes[ic].height = 1 + max_u16(nodes[ib].height, nodes[if_].height);
389                    nodes[ia].height = 1 + max_u16(nodes[ic].height, nodes[ig].height);
390                    nodes[ic].category_bits = nodes[ib].category_bits | nodes[if_].category_bits;
391                    nodes[ia].category_bits = nodes[ic].category_bits | nodes[ig].category_bits;
392                    nodes[ic].flags |= (nodes[ib].flags | nodes[if_].flags) & ENLARGED_NODE;
393                    nodes[ia].flags |= (nodes[ic].flags | nodes[ig].flags) & ENLARGED_NODE;
394                }
395
396                RotateType::Cd => {
397                    nodes[ia].child2 = i_d;
398                    nodes[ib].child1 = i_c;
399
400                    nodes[ic].parent = i_b;
401                    nodes[id].parent = i_a;
402
403                    nodes[ib].aabb = aabb_ce;
404                    nodes[ib].height = 1 + max_u16(nodes[ic].height, nodes[ie].height);
405                    nodes[ia].height = 1 + max_u16(nodes[ib].height, nodes[id].height);
406                    nodes[ib].category_bits = nodes[ic].category_bits | nodes[ie].category_bits;
407                    nodes[ia].category_bits = nodes[ib].category_bits | nodes[id].category_bits;
408                    nodes[ib].flags |= (nodes[ic].flags | nodes[ie].flags) & ENLARGED_NODE;
409                    nodes[ia].flags |= (nodes[ib].flags | nodes[id].flags) & ENLARGED_NODE;
410                }
411
412                RotateType::Ce => {
413                    nodes[ia].child2 = i_e;
414                    nodes[ib].child2 = i_c;
415
416                    nodes[ic].parent = i_b;
417                    nodes[ie].parent = i_a;
418
419                    nodes[ib].aabb = aabb_cd;
420                    nodes[ib].height = 1 + max_u16(nodes[ic].height, nodes[id].height);
421                    nodes[ia].height = 1 + max_u16(nodes[ib].height, nodes[ie].height);
422                    nodes[ib].category_bits = nodes[ic].category_bits | nodes[id].category_bits;
423                    nodes[ia].category_bits = nodes[ib].category_bits | nodes[ie].category_bits;
424                    nodes[ib].flags |= (nodes[ic].flags | nodes[id].flags) & ENLARGED_NODE;
425                    nodes[ia].flags |= (nodes[ib].flags | nodes[ie].flags) & ENLARGED_NODE;
426                }
427            }
428        }
429    }
430
431    /// (static b2InsertLeaf)
432    fn insert_leaf(&mut self, leaf: i32, should_rotate: bool) {
433        if self.root == NULL_INDEX {
434            self.root = leaf;
435            self.nodes[self.root as usize].parent = NULL_INDEX;
436            return;
437        }
438
439        // Stage 1: find the best sibling for this node
440        let leaf_aabb = self.nodes[leaf as usize].aabb;
441        let sibling = find_best_sibling(self, leaf_aabb);
442
443        // Stage 2: create a new parent for the leaf and sibling
444        let old_parent = self.nodes[sibling as usize].parent;
445        let new_parent = self.allocate_node();
446
447        let nodes = &mut self.nodes;
448        let np = new_parent as usize;
449        nodes[np].parent = old_parent;
450        nodes[np].user_data = u64::MAX;
451        nodes[np].aabb = aabb_union(leaf_aabb, nodes[sibling as usize].aabb);
452        nodes[np].category_bits =
453            nodes[leaf as usize].category_bits | nodes[sibling as usize].category_bits;
454        nodes[np].height = nodes[sibling as usize].height + 1;
455        nodes[np].child1 = sibling;
456        nodes[np].child2 = leaf;
457        nodes[sibling as usize].parent = new_parent;
458        nodes[leaf as usize].parent = new_parent;
459
460        // Fix grandparent links
461        if old_parent != NULL_INDEX {
462            // The sibling was not the root
463            if nodes[old_parent as usize].child1 == sibling {
464                nodes[old_parent as usize].child1 = new_parent;
465            } else {
466                debug_assert!(nodes[old_parent as usize].child2 == sibling);
467                nodes[old_parent as usize].child2 = new_parent;
468            }
469        } else {
470            // The sibling was the root
471            self.root = new_parent;
472        }
473
474        // Stage 3: walk back up the tree fixing heights and AABBs
475        let mut index = self.nodes[leaf as usize].parent;
476        while index != NULL_INDEX {
477            let child1 = self.nodes[index as usize].child1;
478            let child2 = self.nodes[index as usize].child2;
479
480            debug_assert!(child1 != NULL_INDEX);
481            debug_assert!(child2 != NULL_INDEX);
482
483            let (c1, c2) = (child1 as usize, child2 as usize);
484            self.nodes[index as usize].aabb = aabb_union(self.nodes[c1].aabb, self.nodes[c2].aabb);
485            self.nodes[index as usize].category_bits =
486                self.nodes[c1].category_bits | self.nodes[c2].category_bits;
487            self.nodes[index as usize].height =
488                1 + max_u16(self.nodes[c1].height, self.nodes[c2].height);
489            self.nodes[index as usize].flags |=
490                (self.nodes[c1].flags | self.nodes[c2].flags) & ENLARGED_NODE;
491
492            if should_rotate {
493                self.rotate_nodes(index);
494            }
495
496            index = self.nodes[index as usize].parent;
497        }
498    }
499
500    /// (static b2RemoveLeaf)
501    pub(crate) fn remove_leaf(&mut self, leaf: i32) {
502        if leaf == self.root {
503            self.root = NULL_INDEX;
504            return;
505        }
506
507        let parent = self.nodes[leaf as usize].parent;
508        let grand_parent = self.nodes[parent as usize].parent;
509        let sibling = if self.nodes[parent as usize].child1 == leaf {
510            self.nodes[parent as usize].child2
511        } else {
512            self.nodes[parent as usize].child1
513        };
514
515        if grand_parent != NULL_INDEX {
516            // Destroy parent and connect sibling to grandParent.
517            if self.nodes[grand_parent as usize].child1 == parent {
518                self.nodes[grand_parent as usize].child1 = sibling;
519            } else {
520                self.nodes[grand_parent as usize].child2 = sibling;
521            }
522            self.nodes[sibling as usize].parent = grand_parent;
523            self.free_node(parent);
524
525            // Adjust ancestor bounds.
526            let mut index = grand_parent;
527            while index != NULL_INDEX {
528                let child1 = self.nodes[index as usize].child1 as usize;
529                let child2 = self.nodes[index as usize].child2 as usize;
530
531                self.nodes[index as usize].aabb =
532                    aabb_union(self.nodes[child1].aabb, self.nodes[child2].aabb);
533                self.nodes[index as usize].category_bits =
534                    self.nodes[child1].category_bits | self.nodes[child2].category_bits;
535                self.nodes[index as usize].height =
536                    1 + max_u16(self.nodes[child1].height, self.nodes[child2].height);
537
538                index = self.nodes[index as usize].parent;
539            }
540        } else {
541            self.root = sibling;
542            self.nodes[sibling as usize].parent = NULL_INDEX;
543            self.free_node(parent);
544        }
545    }
546
547    /// Create a proxy in the tree as a leaf node. Returns the node index.
548    /// (b2DynamicTree_CreateProxy)
549    pub fn create_proxy(&mut self, aabb: Aabb, category_bits: u64, user_data: u64) -> i32 {
550        debug_assert!(is_valid_aabb(aabb));
551
552        let proxy_id = self.allocate_node();
553        let node = &mut self.nodes[proxy_id as usize];
554
555        node.aabb = aabb;
556        node.user_data = user_data;
557        node.category_bits = category_bits;
558        node.height = 0;
559        node.flags = ALLOCATED_NODE | LEAF_NODE;
560
561        let should_rotate = true;
562        self.insert_leaf(proxy_id, should_rotate);
563
564        self.proxy_count += 1;
565
566        proxy_id
567    }
568
569    /// Destroy a proxy. This asserts if the id is invalid.
570    /// (b2DynamicTree_DestroyProxy)
571    pub fn destroy_proxy(&mut self, proxy_id: i32) {
572        debug_assert!(0 <= proxy_id && proxy_id < self.node_capacity());
573        debug_assert!(self.nodes[proxy_id as usize].is_leaf());
574
575        self.remove_leaf(proxy_id);
576        self.free_node(proxy_id);
577
578        debug_assert!(self.proxy_count > 0);
579        self.proxy_count -= 1;
580    }
581
582    /// Move a proxy to a new AABB by removing and reinserting into the tree.
583    /// (b2DynamicTree_MoveProxy)
584    pub fn move_proxy(&mut self, proxy_id: i32, aabb: Aabb) {
585        debug_assert!(is_valid_aabb(aabb));
586        debug_assert!(aabb.upper_bound.x - aabb.lower_bound.x < crate::constants::huge());
587        debug_assert!(aabb.upper_bound.y - aabb.lower_bound.y < crate::constants::huge());
588        debug_assert!(0 <= proxy_id && proxy_id < self.node_capacity());
589        debug_assert!(self.nodes[proxy_id as usize].is_leaf());
590
591        self.remove_leaf(proxy_id);
592
593        self.nodes[proxy_id as usize].aabb = aabb;
594
595        let should_rotate = false;
596        self.insert_leaf(proxy_id, should_rotate);
597    }
598
599    /// Enlarge a proxy and enlarge ancestors as necessary.
600    /// (b2DynamicTree_EnlargeProxy)
601    pub fn enlarge_proxy(&mut self, proxy_id: i32, aabb: Aabb) {
602        debug_assert!(is_valid_aabb(aabb));
603        debug_assert!(aabb.upper_bound.x - aabb.lower_bound.x < crate::constants::huge());
604        debug_assert!(aabb.upper_bound.y - aabb.lower_bound.y < crate::constants::huge());
605        debug_assert!(0 <= proxy_id && proxy_id < self.node_capacity());
606        debug_assert!(self.nodes[proxy_id as usize].is_leaf());
607
608        // Caller must ensure this
609        debug_assert!(!aabb_contains(self.nodes[proxy_id as usize].aabb, aabb));
610
611        self.nodes[proxy_id as usize].aabb = aabb;
612
613        let mut parent_index = self.nodes[proxy_id as usize].parent;
614        while parent_index != NULL_INDEX {
615            let changed = enlarge_aabb(&mut self.nodes[parent_index as usize].aabb, aabb);
616            self.nodes[parent_index as usize].flags |= ENLARGED_NODE;
617            parent_index = self.nodes[parent_index as usize].parent;
618
619            if !changed {
620                break;
621            }
622        }
623
624        while parent_index != NULL_INDEX {
625            if self.nodes[parent_index as usize].flags & ENLARGED_NODE != 0 {
626                // early out because this ancestor was previously ascended and
627                // marked as enlarged
628                break;
629            }
630
631            self.nodes[parent_index as usize].flags |= ENLARGED_NODE;
632            parent_index = self.nodes[parent_index as usize].parent;
633        }
634    }
635
636    /// Modify the category bits on a proxy. This is an expensive operation.
637    /// (b2DynamicTree_SetCategoryBits)
638    pub fn set_category_bits(&mut self, proxy_id: i32, category_bits: u64) {
639        let p = proxy_id as usize;
640        debug_assert!(self.nodes[p].child1 == NULL_INDEX);
641        debug_assert!(self.nodes[p].child2 == NULL_INDEX);
642        debug_assert!(self.nodes[p].flags & LEAF_NODE == LEAF_NODE);
643
644        self.nodes[p].category_bits = category_bits;
645
646        // Fix up category bits in ancestor internal nodes
647        let mut node_index = self.nodes[p].parent;
648        while node_index != NULL_INDEX {
649            let child1 = self.nodes[node_index as usize].child1;
650            debug_assert!(child1 != NULL_INDEX);
651            let child2 = self.nodes[node_index as usize].child2;
652            debug_assert!(child2 != NULL_INDEX);
653            self.nodes[node_index as usize].category_bits = self.nodes[child1 as usize]
654                .category_bits
655                | self.nodes[child2 as usize].category_bits;
656
657            node_index = self.nodes[node_index as usize].parent;
658        }
659    }
660}
box2d_rust/dynamic_tree/insert.rs

box2d_rust/dynamic_tree/
insert.rs
