Skip to main content

mathverse_geometry/
spatial.rs

1//! Spatial structures: axis-aligned bounding box, oriented bounding box, quadtree, octree, frustum.
2
3use super::shapes2d::Point2;
4use super::shapes3d::Point3;
5
6// ---------------------------------------------------------------------------
7// AABB (2D)
8// ---------------------------------------------------------------------------
9
10/// 2D axis-aligned bounding box.
11#[derive(Debug, Clone, Copy, PartialEq)]
12pub struct AABB {
13    pub min: Point2,
14    pub max: Point2,
15}
16
17impl AABB {
18    pub fn new(min: Point2, max: Point2) -> Self {
19        Self { min, max }
20    }
21    pub fn from_points(pts: &[Point2]) -> Option<Self> {
22        if pts.is_empty() {
23            return None;
24        }
25        let mut minx = f64::INFINITY;
26        let mut miny = f64::INFINITY;
27        let mut maxx = f64::NEG_INFINITY;
28        let mut maxy = f64::NEG_INFINITY;
29        for p in pts {
30            if p.x < minx { minx = p.x; }
31            if p.y < miny { miny = p.y; }
32            if p.x > maxx { maxx = p.x; }
33            if p.y > maxy { maxy = p.y; }
34        }
35        Some(Self { min: Point2::new(minx, miny), max: Point2::new(maxx, maxy) })
36    }
37    pub fn width(self) -> f64 {
38        self.max.x - self.min.x
39    }
40    pub fn height(self) -> f64 {
41        self.max.y - self.min.y
42    }
43    pub fn center(self) -> Point2 {
44        Point2::new((self.min.x + self.max.x) / 2.0, (self.min.y + self.max.y) / 2.0)
45    }
46    pub fn area(self) -> f64 {
47        self.width() * self.height()
48    }
49    pub fn contains(self, p: Point2) -> bool {
50        p.x >= self.min.x && p.x <= self.max.x && p.y >= self.min.y && p.y <= self.max.y
51    }
52    pub fn intersects(self, other: AABB) -> bool {
53        self.min.x <= other.max.x
54            && self.max.x >= other.min.x
55            && self.min.y <= other.max.y
56            && self.max.y >= other.min.y
57    }
58    pub fn merge(self, other: AABB) -> AABB {
59        AABB {
60            min: Point2::new(self.min.x.min(other.min.x), self.min.y.min(other.min.y)),
61            max: Point2::new(self.max.x.max(other.max.x), self.max.y.max(other.max.y)),
62        }
63    }
64    pub fn expand(self, padding: f64) -> AABB {
65        AABB {
66            min: Point2::new(self.min.x - padding, self.min.y - padding),
67            max: Point2::new(self.max.x + padding, self.max.y + padding),
68        }
69    }
70}
71
72// ---------------------------------------------------------------------------
73// AABB3 (3D)
74// ---------------------------------------------------------------------------
75
76/// 3D axis-aligned bounding box.
77#[derive(Debug, Clone, Copy, PartialEq)]
78pub struct AABB3 {
79    pub min: Point3,
80    pub max: Point3,
81}
82
83impl AABB3 {
84    pub fn new(min: Point3, max: Point3) -> Self {
85        Self { min, max }
86    }
87    pub fn from_points(pts: &[Point3]) -> Option<Self> {
88        if pts.is_empty() {
89            return None;
90        }
91        let mut min = Point3::new(f64::INFINITY, f64::INFINITY, f64::INFINITY);
92        let mut max = Point3::new(f64::NEG_INFINITY, f64::NEG_INFINITY, f64::NEG_INFINITY);
93        for p in pts {
94            if p.x < min.x { min.x = p.x; }
95            if p.y < min.y { min.y = p.y; }
96            if p.z < min.z { min.z = p.z; }
97            if p.x > max.x { max.x = p.x; }
98            if p.y > max.y { max.y = p.y; }
99            if p.z > max.z { max.z = p.z; }
100        }
101        Some(Self { min, max })
102    }
103    pub fn center(self) -> Point3 {
104        Point3::new(
105            (self.min.x + self.max.x) / 2.0,
106            (self.min.y + self.max.y) / 2.0,
107            (self.min.z + self.max.z) / 2.0,
108        )
109    }
110    pub fn half_extents(self) -> Point3 {
111        Point3::new(
112            (self.max.x - self.min.x) / 2.0,
113            (self.max.y - self.min.y) / 2.0,
114            (self.max.z - self.min.z) / 2.0,
115        )
116    }
117    pub fn volume(self) -> f64 {
118        (self.max.x - self.min.x) * (self.max.y - self.min.y) * (self.max.z - self.min.z)
119    }
120    pub fn surface_area(self) -> f64 {
121        let dx = self.max.x - self.min.x;
122        let dy = self.max.y - self.min.y;
123        let dz = self.max.z - self.min.z;
124        2.0 * (dx * dy + dy * dz + dz * dx)
125    }
126    pub fn contains(self, p: Point3) -> bool {
127        p.x >= self.min.x && p.x <= self.max.x
128            && p.y >= self.min.y && p.y <= self.max.y
129            && p.z >= self.min.z && p.z <= self.max.z
130    }
131    pub fn intersects(self, other: AABB3) -> bool {
132        self.min.x <= other.max.x
133            && self.max.x >= other.min.x
134            && self.min.y <= other.max.y
135            && self.max.y >= other.min.y
136            && self.min.z <= other.max.z
137            && self.max.z >= other.min.z
138    }
139    pub fn merge(self, other: AABB3) -> AABB3 {
140        AABB3 {
141            min: Point3::new(
142                self.min.x.min(other.min.x),
143                self.min.y.min(other.min.y),
144                self.min.z.min(other.min.z),
145            ),
146            max: Point3::new(
147                self.max.x.max(other.max.x),
148                self.max.y.max(other.max.y),
149                self.max.z.max(other.max.z),
150            ),
151        }
152    }
153}
154
155// ---------------------------------------------------------------------------
156// OBB (2D)
157// ---------------------------------------------------------------------------
158
159/// 2D oriented bounding box: center, half-extents along local axes, rotation angle.
160#[derive(Debug, Clone, Copy, PartialEq)]
161pub struct OBB {
162    pub center: Point2,
163    pub half_w: f64,
164    pub half_h: f64,
165    pub angle: f64,
166}
167
168impl OBB {
169    pub fn new(center: Point2, half_w: f64, half_h: f64, angle: f64) -> Self {
170        Self { center, half_w, half_h, angle }
171    }
172    /// Four corner vertices.
173    pub fn corners(self) -> [Point2; 4] {
174        let (s, c) = self.angle.sin_cos();
175        let dx = Point2::new(c * self.half_w, s * self.half_w);
176        let dy = Point2::new(-s * self.half_h, c * self.half_h);
177        [
178            self.center.translate(dx.x - dy.x, dx.y - dy.y),
179            self.center.translate(dx.x + dy.x, dx.y + dy.y),
180            self.center.translate(-dx.x + dy.x, -dx.y + dy.y),
181            self.center.translate(-dx.x - dy.x, -dx.y - dy.y),
182        ]
183    }
184    pub fn area(self) -> f64 {
185        4.0 * self.half_w * self.half_h
186    }
187    /// Whether point `p` is inside this OBB (SAT with one axis).
188    pub fn contains(self, p: Point2) -> bool {
189        let (s, c) = self.angle.sin_cos();
190        let dx = p.x - self.center.x;
191        let dy = p.y - self.center.y;
192        let local_x = c * dx + s * dy;
193        let local_y = -s * dx + c * dy;
194        local_x.abs() <= self.half_w && local_y.abs() <= self.half_h
195    }
196}
197
198// ---------------------------------------------------------------------------
199// Quadtree
200// ---------------------------------------------------------------------------
201
202/// Simple quadtree for 2D point queries.
203pub struct Quadtree {
204    bounds: AABB,
205    points: Vec<Point2>,
206    capacity: usize,
207    divided: bool,
208    ne: Option<Box<Quadtree>>,
209    nw: Option<Box<Quadtree>>,
210    se: Option<Box<Quadtree>>,
211    sw: Option<Box<Quadtree>>,
212}
213
214impl Quadtree {
215    pub fn new(bounds: AABB, capacity: usize) -> Self {
216        Self {
217            bounds,
218            points: Vec::new(),
219            capacity,
220            divided: false,
221            ne: None,
222            nw: None,
223            se: None,
224            sw: None,
225        }
226    }
227    pub fn insert(&mut self, p: Point2) -> bool {
228        if !self.bounds.contains(p) {
229            return false;
230        }
231        if self.points.len() < self.capacity && !self.divided {
232            self.points.push(p);
233            return true;
234        }
235        if !self.divided {
236            self.subdivide();
237        }
238        // Safety/invariant: subdivide() always populates all four quadrants
239        self.ne.as_mut().unwrap().insert(p)
240            || self.nw.as_mut().unwrap().insert(p)
241            || self.se.as_mut().unwrap().insert(p)
242            || self.sw.as_mut().unwrap().insert(p)
243    }
244    /// Query all points within `range`.
245    pub fn query(&self, range: AABB, result: &mut Vec<Point2>) {
246        if !self.bounds.intersects(range) {
247            return;
248        }
249        for p in &self.points {
250            if range.contains(*p) {
251                result.push(*p);
252            }
253        }
254        if self.divided {
255            // Safety/invariant: subdivide() always populates all four quadrants
256            self.ne.as_ref().unwrap().query(range, result);
257            self.nw.as_ref().unwrap().query(range, result);
258            self.se.as_ref().unwrap().query(range, result);
259            self.sw.as_ref().unwrap().query(range, result);
260        }
261    }
262    fn subdivide(&mut self) {
263        let c = self.bounds.center();
264        let hx = self.bounds.width() / 2.0;
265        let hy = self.bounds.height() / 2.0;
266        let cap = self.capacity;
267        self.ne = Some(Box::new(Quadtree::new(AABB::new(Point2::new(c.x, c.y), Point2::new(c.x + hx, c.y + hy)), cap)));
268        self.nw = Some(Box::new(Quadtree::new(AABB::new(Point2::new(c.x - hx, c.y), Point2::new(c.x, c.y + hy)), cap)));
269        self.se = Some(Box::new(Quadtree::new(AABB::new(Point2::new(c.x, c.y - hy), Point2::new(c.x + hx, c.y)), cap)));
270        self.sw = Some(Box::new(Quadtree::new(AABB::new(Point2::new(c.x - hx, c.y - hy), Point2::new(c.x, c.y)), cap)));
271        self.divided = true;
272    }
273}
274
275// ---------------------------------------------------------------------------
276// Octree
277// ---------------------------------------------------------------------------
278
279/// Simple octree for 3D point queries.
280pub struct Octree {
281    bounds: AABB3,
282    points: Vec<Point3>,
283    capacity: usize,
284    divided: bool,
285    children: Option<[Box<Octree>; 8]>,
286}
287
288impl Octree {
289    pub fn new(bounds: AABB3, capacity: usize) -> Self {
290        Self { bounds, points: Vec::new(), capacity, divided: false, children: None }
291    }
292    pub fn insert(&mut self, p: Point3) -> bool {
293        if !self.bounds.contains(p) {
294            return false;
295        }
296        if self.points.len() < self.capacity && !self.divided {
297            self.points.push(p);
298            return true;
299        }
300        if !self.divided {
301            self.subdivide();
302        }
303        // Safety/invariant: subdivide() always populates all eight children
304        for child in self.children.as_mut().unwrap().iter_mut() {
305            if child.insert(p) {
306                return true;
307            }
308        }
309        false
310    }
311    pub fn query(&self, range: AABB3, result: &mut Vec<Point3>) {
312        if !self.bounds.intersects(range) {
313            return;
314        }
315        for p in &self.points {
316            if range.contains(*p) {
317                result.push(*p);
318            }
319        }
320        if let Some(children) = &self.children {
321            for child in children.iter() {
322                child.query(range, result);
323            }
324        }
325    }
326    fn subdivide(&mut self) {
327        let c = self.bounds.center();
328        let he = self.bounds.half_extents();
329        let cap = self.capacity;
330        let mut children: Vec<Box<Octree>> = Vec::with_capacity(8);
331        for iz in 0..2 {
332            for iy in 0..2 {
333                for ix in 0..2 {
334                    let min = Point3::new(
335                        c.x + (ix as f64 - 0.5) * he.x,
336                        c.y + (iy as f64 - 0.5) * he.y,
337                        c.z + (iz as f64 - 0.5) * he.z,
338                    );
339                    let max = Point3::new(
340                        min.x + he.x,
341                        min.y + he.y,
342                        min.z + he.z,
343                    );
344                    children.push(Box::new(Octree::new(AABB3::new(min, max), cap)));
345                }
346            }
347        }
348        let arr: [Box<Octree>; 8] = [
349            children.remove(0), children.remove(0), children.remove(0), children.remove(0),
350            children.remove(0), children.remove(0), children.remove(0), children.remove(0),
351        ];
352        self.children = Some(arr);
353        self.divided = true;
354    }
355}
356
357// ---------------------------------------------------------------------------
358// Frustum (3D)
359// ---------------------------------------------------------------------------
360
361/// View frustum defined by 6 planes (near, far, left, right, top, bottom).
362/// Each plane is (nx, ny, nz, d) where nx*x + ny*y + nz*z + d = 0.
363#[derive(Debug, Clone, Copy)]
364pub struct Frustum {
365    pub planes: [(f64, f64, f64, f64); 6],
366}
367
368impl Frustum {
369    pub fn new(planes: [(f64, f64, f64, f64); 6]) -> Self {
370        Self { planes }
371    }
372    /// Whether a point is inside (or on) all frustum planes.
373    pub fn contains_point(self, p: Point3) -> bool {
374        self.planes.iter().all(|(nx, ny, nz, d)| {
375            nx * p.x + ny * p.y + nz * p.z + d >= -1e-10
376        })
377    }
378    /// Whether an AABB intersects the frustum.
379    pub fn intersects_aabb(self, bb: AABB3) -> bool {
380        for (nx, ny, nz, d) in &self.planes {
381            let px = if *nx >= 0.0 { bb.max.x } else { bb.min.x };
382            let py = if *ny >= 0.0 { bb.max.y } else { bb.min.y };
383            let pz = if *nz >= 0.0 { bb.max.z } else { bb.min.z };
384            if nx * px + ny * py + nz * pz + d < -1e-10 {
385                return false;
386            }
387        }
388        true
389    }
390}
391
392#[cfg(test)]
393mod tests {
394    use super::*;
395
396    fn pt2(x: f64, y: f64) -> Point2 { Point2::new(x, y) }
397
398    #[test]
399    fn aabb_basics() {
400        let bb = AABB::new(pt2(1.0, 2.0), pt2(3.0, 4.0));
401        assert!((bb.width() - 2.0).abs() < 1e-12);
402        assert!((bb.height() - 2.0).abs() < 1e-12);
403        assert_eq!(bb.center(), pt2(2.0, 3.0));
404        assert!(bb.contains(pt2(2.0, 3.0)));
405        assert!(!bb.contains(pt2(0.0, 0.0)));
406        let other = AABB::new(pt2(2.5, 3.5), pt2(5.0, 5.0));
407        assert!(bb.intersects(other));
408        let disjoint = AABB::new(pt2(10.0, 10.0), pt2(20.0, 20.0));
409        assert!(!bb.intersects(disjoint));
410    }
411
412    #[test]
413    fn aabb_from_points() {
414        let pts = vec![pt2(1.0, 5.0), pt2(3.0, 2.0), pt2(-1.0, 4.0)];
415        let bb = AABB::from_points(&pts).unwrap();
416        assert_eq!(bb.min, pt2(-1.0, 2.0));
417        assert_eq!(bb.max, pt2(3.0, 5.0));
418    }
419
420    #[test]
421    fn obb_contains() {
422        let obb = OBB::new(pt2(0.0, 0.0), 2.0, 1.0, 0.0);
423        assert!(obb.contains(pt2(0.0, 0.0)));
424        assert!(obb.contains(pt2(1.9, 0.9)));
425        assert!(!obb.contains(pt2(2.1, 0.0)));
426    }
427
428    #[test]
429    fn quadtree_query() {
430        let bounds = AABB::new(pt2(0.0, 0.0), pt2(10.0, 10.0));
431        let mut qt = Quadtree::new(bounds, 4);
432        for i in 0..20 {
433            qt.insert(pt2(i as f64 + 0.5, i as f64 + 0.5));
434        }
435        let range = AABB::new(pt2(0.0, 0.0), pt2(5.0, 5.0));
436        let mut result = Vec::new();
437        qt.query(range, &mut result);
438        assert_eq!(result.len(), 5);
439    }
440
441    #[test]
442    fn frustum_contains() {
443        // Simple frustum: all half-spaces with normals pointing inward
444        let planes = [
445            (0.0, 0.0, 1.0, 1.0),   // near: z >= -1
446            (0.0, 0.0, -1.0, 10.0),  // far: z <= 10
447            (1.0, 0.0, 0.0, 1.0),    // left: x >= -1
448            (-1.0, 0.0, 0.0, 10.0),  // right: x <= 10
449            (0.0, 1.0, 0.0, 1.0),    // bottom: y >= -1
450            (0.0, -1.0, 0.0, 10.0),  // top: y <= 10
451        ];
452        let f = Frustum::new(planes);
453        assert!(f.contains_point(Point3::new(5.0, 5.0, 5.0)));
454        assert!(!f.contains_point(Point3::new(15.0, 5.0, 5.0)));
455    }
456}