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

jagua_rs/collision_detection/quadtree/
qt_traits.rs
