jagua_rs/collision_detection/quadtree/
qt_traits.rs

1use crate::geometry::geo_traits::CollidesWith;
2use crate::geometry::primitives::Rect;
3use crate::geometry::primitives::{Circle, Edge, Point};
4use std::cmp::Ordering;
5
6/// Common trait for all geometric primitives that can be directly queried in the quadtree
7/// for collisions with the edges of the registered hazards. These include: [Rect], [Edge] and [Circle].
8pub trait QTQueryable: CollidesWith<Edge> + CollidesWith<Rect> {
9    /// Checks
10    fn collides_with_quadrants(&self, _r: &Rect, qs: [&Rect; 4]) -> [bool; 4] {
11        debug_assert!(_r.quadrants().iter().zip(qs.iter()).all(|(q, r)| *q == **r));
12        qs.map(|q| self.collides_with(q))
13    }
14}
15
16impl QTQueryable for Circle {}
17impl QTQueryable for Rect {}
18
19impl QTQueryable for Edge {
20    fn collides_with_quadrants(&self, r: &Rect, qs: [&Rect; 4]) -> [bool; 4] {
21        debug_assert!(r.quadrants().iter().zip(qs.iter()).all(|(q, r)| *q == **r));
22        let e_x_min = self.x_min();
23        let e_x_max = self.x_max();
24        let e_y_min = self.y_min();
25        let e_y_max = self.y_max();
26
27        let [mut c_q0, mut c_q1, mut c_q2, mut c_q3] = [0, 1, 2, 3].map(|idx| {
28            let q = qs[idx];
29
30            let x_no_overlap = e_x_min.max(q.x_min) > e_x_max.min(q.x_max);
31            let y_no_overlap = e_y_min.max(q.y_min) > e_y_max.min(q.y_max);
32
33            if x_no_overlap || y_no_overlap {
34                // Edge is completely outside the x- or y-range of the quadrant
35                Some(false)
36            } else if q.collides_with(&self.start) || q.collides_with(&self.end) {
37                // Edge has at least one end point in the quadrant
38                Some(true)
39            } else {
40                // Undetermined, we need to check for intersections with the sides of the quadrants
41                None
42            }
43        });
44
45        // If all quadrants are already determined, we can return early
46        if let (Some(c_q0), Some(c_q1), Some(c_q2), Some(c_q3)) = (c_q0, c_q1, c_q2, c_q3) {
47            return [c_q0, c_q1, c_q2, c_q3];
48        }
49
50        // Otherwise, we need to check for intersections with the sides of the quadrants
51        // We can exploit the fact that the quadrants have a fixed layout, and share edges.
52
53        let c = r.centroid();
54
55        let [top, left, bottom, right] = r.sides();
56
57        let h_bisect = Edge {
58            start: Point(r.x_min, c.1),
59            end: Point(r.x_max, c.1),
60        };
61        let v_bisect = Edge {
62            start: Point(c.0, r.y_min),
63            end: Point(c.0, r.y_max),
64        };
65
66        //  1    0
67        //  2    3
68
69        half_intersect(self, &left, [&mut c_q1], [&mut c_q2]);
70        half_intersect(self, &right, [&mut c_q3], [&mut c_q0]);
71        half_intersect(self, &top, [&mut c_q0], [&mut c_q1]);
72        half_intersect(self, &bottom, [&mut c_q2], [&mut c_q3]);
73        half_intersect(
74            self,
75            &h_bisect,
76            [&mut c_q1, &mut c_q2],
77            [&mut c_q0, &mut c_q3],
78        );
79        half_intersect(
80            self,
81            &v_bisect,
82            [&mut c_q2, &mut c_q3],
83            [&mut c_q0, &mut c_q1],
84        );
85
86        let [c_q0, c_q1, c_q2, c_q3] = [c_q0, c_q1, c_q2, c_q3].map(|c| c.unwrap_or(false));
87        debug_assert!(
88            {
89                // make sure all quadrants which are colliding according to the individual collision check are at least
90                // also caught by the quadrant collision check
91                qs.map(|q| self.collides_with(q))
92                    .iter()
93                    .zip([c_q0, c_q1, c_q2, c_q3].iter())
94                    .all(|(&i_c, &q_c)| !i_c || q_c)
95            },
96            "{:?}, {:?}, {:?}, {:?}, {:?}",
97            self,
98            r,
99            qs,
100            [c_q0, c_q1, c_q2, c_q3],
101            qs.map(|q| self.collides_with(q))
102        );
103
104        [c_q0, c_q1, c_q2, c_q3]
105    }
106}
107
108/// If e1 intersects with e2 in the first half of e2, it sets all bools in `fst_qs` to true,
109/// if e1 intersects with e2 the second half of e2, it sets all bools in `sec_qs` to true.
110fn half_intersect<const N: usize>(
111    e1: &Edge,
112    e2: &Edge,
113    fst_qs: [&mut Option<bool>; N],
114    sec_qs: [&mut Option<bool>; N],
115) {
116    if fst_qs.iter().chain(sec_qs.iter()).any(|t| t.is_none())
117        && let Some((_, e2_col_loc)) = edge_intersection_half(e1, e2)
118    {
119        match e2_col_loc {
120            CollisionHalf::FirstHalf => {
121                for c in fst_qs {
122                    *c = Some(true);
123                }
124            }
125            CollisionHalf::Halfway => {
126                for c in fst_qs {
127                    *c = Some(true);
128                }
129                for c in sec_qs {
130                    *c = Some(true);
131                }
132            }
133            CollisionHalf::SecondHalf => {
134                for c in sec_qs {
135                    *c = Some(true);
136                }
137            }
138        }
139    }
140}
141
142#[inline(always)]
143// Similar to `edge_intersection`, but in case of an intersection, it returns in which half for both edge the intersection occurs.
144fn edge_intersection_half(e1: &Edge, e2: &Edge) -> Option<(CollisionHalf, CollisionHalf)> {
145    let Point(x1, y1) = e1.start;
146    let Point(x2, y2) = e1.end;
147    let Point(x3, y3) = e2.start;
148    let Point(x4, y4) = e2.end;
149
150    //based on: https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line_segment
151    let t_nom = (x2 - x4) * (y4 - y3) - (y2 - y4) * (x4 - x3);
152    let t_denom = (x2 - x1) * (y4 - y3) - (y2 - y1) * (x4 - x3);
153    let u_nom = (x2 - x4) * (y2 - y1) - (y2 - y4) * (x2 - x1);
154    let u_denom = (x2 - x1) * (y4 - y3) - (y2 - y1) * (x4 - x3);
155    if t_denom == 0.0 || u_denom == 0.0 {
156        //parallel edges
157        return None;
158    }
159
160    let t = t_nom / t_denom; //refers to the position along e1
161    let u = u_nom / u_denom; //refers to the position along e2
162    if (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&u) {
163        let e1_loc = match t.partial_cmp(&0.5).unwrap() {
164            Ordering::Greater => CollisionHalf::FirstHalf,
165            Ordering::Less => CollisionHalf::SecondHalf,
166            Ordering::Equal => CollisionHalf::Halfway,
167        };
168        let e2_loc = match u.partial_cmp(&0.5).unwrap() {
169            Ordering::Greater => CollisionHalf::FirstHalf,
170            Ordering::Less => CollisionHalf::SecondHalf,
171            Ordering::Equal => CollisionHalf::Halfway,
172        };
173
174        return Some((e1_loc, e2_loc));
175    }
176    None
177}
178
179pub enum CollisionHalf {
180    FirstHalf,
181    Halfway,
182    SecondHalf,
183}
jagua_rs/collision_detection/quadtree/qt_traits.rs

jagua_rs/collision_detection/quadtree/
qt_traits.rs
