1use super::shapes2d::Point2;
4use super::shapes3d::Point3;
5
6#[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#[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#[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 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 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
198pub 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 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 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 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
275pub 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 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#[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 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 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 let planes = [
445 (0.0, 0.0, 1.0, 1.0), (0.0, 0.0, -1.0, 10.0), (1.0, 0.0, 0.0, 1.0), (-1.0, 0.0, 0.0, 10.0), (0.0, 1.0, 0.0, 1.0), (0.0, -1.0, 0.0, 10.0), ];
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}