box2d_rust/dynamic_tree/
mod.rs

1// Port of box2d-cpp-reference/src/dynamic_tree.c and the tree group of
2// include/box2d/collision.h — a dynamic AABB tree broad-phase inspired by
3// Nathanael Presson's btDbvt.
4//
5// Split to satisfy the 800-line file limit:
6// - insert.rs   — SAH sibling selection, rotations, insert/remove, proxy ops
7// - query.rs    — AABB query, ray cast, box cast traversals
8// - rebuild.rs  — median-split partial/full rebuild
9// - validate.rs — structure/metrics validation for tests
10//
11// The C `#else` SAH build heuristic (B2_TREE_HEURISTIC == 1) is compiled out
12// upstream and is not ported; only the median-split heuristic is live.
13//
14// The C node uses two unions: {children | userData} and {parent | next}. The
15// Rust node stores all four fields separately; the code writes them at exactly
16// the points the C writes the corresponding union view, so observable behavior
17// matches. (The C default node's children view {-1,-1} reads as u64::MAX
18// through the userData view, which is reproduced explicitly.)
19//
20// SPDX-FileCopyrightText: 2023 Erin Catto
21// SPDX-License-Identifier: MIT
22
23mod insert;
24mod query;
25mod rebuild;
26mod validate;
27
28use crate::core::NULL_INDEX;
29use crate::math_functions::{Aabb, Vec2, VEC2_ZERO};
30
31/// types.h: B2_DEFAULT_CATEGORY_BITS
32pub const DEFAULT_CATEGORY_BITS: u64 = 1;
33/// types.h: B2_DEFAULT_MASK_BITS
34pub const DEFAULT_MASK_BITS: u64 = u64::MAX;
35
36pub(crate) const TREE_STACK_SIZE: usize = 1024;
37
38// Tree node flags (enum b2TreeNodeFlags)
39pub(crate) const ALLOCATED_NODE: u16 = 0x0001;
40pub(crate) const ENLARGED_NODE: u16 = 0x0002;
41pub(crate) const LEAF_NODE: u16 = 0x0004;
42
43/// A node in the dynamic tree. For internal usage. (b2TreeNode)
44#[derive(Debug, Clone, Copy)]
45pub struct TreeNode {
46    /// The node bounding box
47    pub aabb: Aabb,
48    /// Category bits for collision filtering
49    pub category_bits: u64,
50    /// Child node index 1 (internal nodes)
51    pub child1: i32,
52    /// Child node index 2 (internal nodes)
53    pub child2: i32,
54    /// User data (leaf nodes)
55    pub user_data: u64,
56    /// The node parent index (allocated nodes)
57    pub parent: i32,
58    /// The node freelist next index (free nodes)
59    pub next: i32,
60    pub height: u16,
61    pub flags: u16,
62}
63
64impl TreeNode {
65    /// static b2_defaultTreeNode
66    pub(crate) fn default_node() -> TreeNode {
67        TreeNode {
68            aabb: Aabb {
69                lower_bound: VEC2_ZERO,
70                upper_bound: VEC2_ZERO,
71            },
72            category_bits: DEFAULT_CATEGORY_BITS,
73            child1: NULL_INDEX,
74            child2: NULL_INDEX,
75            // The C children/userData union: {-1, -1} reads as u64::MAX.
76            user_data: u64::MAX,
77            parent: NULL_INDEX,
78            next: NULL_INDEX,
79            height: 0,
80            flags: ALLOCATED_NODE,
81        }
82    }
83
84    /// Zeroed storage matching the C memset of the node pool.
85    fn zeroed() -> TreeNode {
86        TreeNode {
87            aabb: Aabb {
88                lower_bound: VEC2_ZERO,
89                upper_bound: VEC2_ZERO,
90            },
91            category_bits: 0,
92            child1: 0,
93            child2: 0,
94            user_data: 0,
95            parent: 0,
96            next: 0,
97            height: 0,
98            flags: 0,
99        }
100    }
101
102    /// (static b2IsLeaf)
103    pub(crate) fn is_leaf(&self) -> bool {
104        self.flags & LEAF_NODE != 0
105    }
106
107    /// (static b2IsAllocated)
108    pub(crate) fn is_allocated(&self) -> bool {
109        self.flags & ALLOCATED_NODE != 0
110    }
111}
112
113/// These are performance results returned by dynamic tree queries. (b2TreeStats)
114#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
115pub struct TreeStats {
116    /// Number of internal nodes visited during the query
117    pub node_visits: i32,
118    /// Number of leaf nodes visited during the query
119    pub leaf_visits: i32,
120}
121
122/// Input for casting an AABB through a dynamic tree. (b2BoxCastInput)
123#[derive(Debug, Clone, Copy, PartialEq, Default)]
124pub struct BoxCastInput {
125    /// The AABB to cast, in the tree's frame.
126    pub box_: Aabb,
127    /// The sweep translation.
128    pub translation: Vec2,
129    /// The maximum fraction of the translation to consider, typically 1.
130    pub max_fraction: f32,
131}
132
133/// The dynamic tree structure. (b2DynamicTree)
134///
135/// A dynamic AABB tree broad-phase. Leaf nodes are proxies with an AABB, used
136/// to hold a user collision object. Nodes are pooled and relocatable, so node
137/// indices are used rather than pointers.
138#[derive(Debug, Clone, Default)]
139pub struct DynamicTree {
140    /// The tree nodes. `nodes.len()` is the node capacity.
141    pub(crate) nodes: Vec<TreeNode>,
142    /// The root index
143    pub(crate) root: i32,
144    /// The number of allocated nodes
145    pub(crate) node_count: i32,
146    /// Node free list
147    pub(crate) free_list: i32,
148    /// Number of proxies created
149    pub(crate) proxy_count: i32,
150    /// Leaf indices for rebuild
151    pub(crate) leaf_indices: Vec<i32>,
152    /// Leaf bounding box centers for rebuild
153    pub(crate) leaf_centers: Vec<Vec2>,
154    /// Allocated space for rebuilding
155    pub(crate) rebuild_capacity: i32,
156}
157
158impl DynamicTree {
159    /// Constructing the tree initializes the node pool. (b2DynamicTree_Create)
160    pub fn new(proxy_capacity: i32) -> DynamicTree {
161        let capacity = crate::math_functions::max_int(proxy_capacity, 16);
162
163        // maximum node count for a full binary tree is 2 * leafCount - 1
164        let node_capacity = (2 * capacity - 1) as usize;
165        let mut nodes = vec![TreeNode::zeroed(); node_capacity];
166
167        // Build a linked list for the free list.
168        for (i, node) in nodes.iter_mut().enumerate().take(node_capacity - 1) {
169            node.next = i as i32 + 1;
170        }
171        nodes[node_capacity - 1].next = NULL_INDEX;
172
173        DynamicTree {
174            nodes,
175            root: NULL_INDEX,
176            node_count: 0,
177            free_list: 0,
178            proxy_count: 0,
179            leaf_indices: Vec::new(),
180            leaf_centers: Vec::new(),
181            rebuild_capacity: 0,
182        }
183    }
184
185    /// Destroy the tree, freeing the node pool. (b2DynamicTree_Destroy)
186    ///
187    /// Rust would drop the storage automatically; this mirrors the C function
188    /// (which leaves a zeroed struct) so ported call sites and tests read the
189    /// same.
190    pub fn destroy(&mut self) {
191        *self = DynamicTree {
192            nodes: Vec::new(),
193            root: 0,
194            node_count: 0,
195            free_list: 0,
196            proxy_count: 0,
197            leaf_indices: Vec::new(),
198            leaf_centers: Vec::new(),
199            rebuild_capacity: 0,
200        };
201    }
202
203    pub(crate) fn node_capacity(&self) -> i32 {
204        self.nodes.len() as i32
205    }
206
207    /// The number of allocated nodes.
208    pub fn node_count(&self) -> i32 {
209        self.node_count
210    }
211
212    /// Allocate a node from the pool. Grow the pool if necessary.
213    /// (static b2AllocateNode)
214    pub(crate) fn allocate_node(&mut self) -> i32 {
215        // Expand the node pool as needed.
216        if self.free_list == NULL_INDEX {
217            debug_assert!(self.node_count == self.node_capacity());
218
219            // The free list is empty. Rebuild a bigger pool.
220            let old_capacity = self.nodes.len();
221            let new_capacity = old_capacity + (old_capacity >> 1);
222            self.nodes.resize(new_capacity, TreeNode::zeroed());
223
224            // Build a linked list for the free list. The parent pointer
225            // becomes the "next" pointer.
226            for i in self.node_count as usize..new_capacity - 1 {
227                self.nodes[i].next = i as i32 + 1;
228            }
229            self.nodes[new_capacity - 1].next = NULL_INDEX;
230            self.free_list = self.node_count;
231        }
232
233        // Peel a node off the free list.
234        let node_index = self.free_list;
235        self.free_list = self.nodes[node_index as usize].next;
236        self.nodes[node_index as usize] = TreeNode::default_node();
237        self.node_count += 1;
238        node_index
239    }
240
241    /// Return a node to the pool. (static b2FreeNode)
242    pub(crate) fn free_node(&mut self, node_id: i32) {
243        debug_assert!(0 <= node_id && node_id < self.node_capacity());
244        debug_assert!(0 < self.node_count);
245        self.nodes[node_id as usize].next = self.free_list;
246        self.nodes[node_id as usize].flags = 0;
247        self.free_list = node_id;
248        self.node_count -= 1;
249    }
250
251    /// Get the number of proxies created. (b2DynamicTree_GetProxyCount)
252    pub fn proxy_count(&self) -> i32 {
253        self.proxy_count
254    }
255
256    /// Get the category bits on a proxy. (b2DynamicTree_GetCategoryBits)
257    pub fn category_bits(&self, proxy_id: i32) -> u64 {
258        debug_assert!(0 <= proxy_id && proxy_id < self.node_capacity());
259        self.nodes[proxy_id as usize].category_bits
260    }
261
262    /// Get the height of the binary tree. (b2DynamicTree_GetHeight)
263    pub fn height(&self) -> i32 {
264        if self.root == NULL_INDEX {
265            return 0;
266        }
267
268        self.nodes[self.root as usize].height as i32
269    }
270
271    /// Get the ratio of the sum of the node areas to the root area.
272    /// (b2DynamicTree_GetAreaRatio)
273    pub fn area_ratio(&self) -> f32 {
274        use crate::aabb::perimeter;
275
276        if self.root == NULL_INDEX {
277            return 0.0;
278        }
279
280        let root = &self.nodes[self.root as usize];
281        let root_area = perimeter(root.aabb);
282
283        let mut total_area = 0.0;
284        for (i, node) in self.nodes.iter().enumerate() {
285            if !node.is_allocated() || node.is_leaf() || i as i32 == self.root {
286                continue;
287            }
288
289            total_area += perimeter(node.aabb);
290        }
291
292        total_area / root_area
293    }
294
295    /// Get the bounding box that contains the entire tree.
296    /// (b2DynamicTree_GetRootBounds)
297    pub fn root_bounds(&self) -> Aabb {
298        if self.root != NULL_INDEX {
299            return self.nodes[self.root as usize].aabb;
300        }
301
302        Aabb {
303            lower_bound: VEC2_ZERO,
304            upper_bound: VEC2_ZERO,
305        }
306    }
307
308    /// Get the number of bytes used by this tree. (b2DynamicTree_GetByteCount)
309    pub fn byte_count(&self) -> i32 {
310        let size = core::mem::size_of::<DynamicTree>()
311            + core::mem::size_of::<TreeNode>() * self.nodes.len()
312            + self.rebuild_capacity as usize
313                * (core::mem::size_of::<i32>()
314                    + core::mem::size_of::<Aabb>()
315                    + core::mem::size_of::<Vec2>()
316                    + core::mem::size_of::<i32>());
317
318        size as i32
319    }
320
321    /// Get proxy user data. (b2DynamicTree_GetUserData)
322    pub fn user_data(&self, proxy_id: i32) -> u64 {
323        debug_assert!(0 <= proxy_id && proxy_id < self.node_capacity());
324        self.nodes[proxy_id as usize].user_data
325    }
326
327    /// Get the AABB of a proxy. (b2DynamicTree_GetAABB)
328    pub fn aabb(&self, proxy_id: i32) -> Aabb {
329        debug_assert!(0 <= proxy_id && proxy_id < self.node_capacity());
330        self.nodes[proxy_id as usize].aabb
331    }
332}
box2d_rust/dynamic_tree/mod.rs

box2d_rust/dynamic_tree/
mod.rs
