maplibre/util/
math.rs

1use std::{cmp::Ordering, fmt};
2
3use cgmath::{
4    ulps_eq, BaseFloat, BaseNum, EuclideanSpace, InnerSpace, Point2, Point3, Vector3, Zero,
5};
6
7/// A 3-dimensional plane formed from the equation: `A*x + B*y + C*z - D = 0`.
8///
9/// # Fields
10///
11/// - `n`: a unit vector representing the normal of the plane where:
12///   - `n.x`: corresponds to `A` in the plane equation
13///   - `n.y`: corresponds to `B` in the plane equation
14///   - `n.z`: corresponds to `C` in the plane equation
15/// - `d`: the distance value, corresponding to `D` in the plane equation
16///
17/// # Notes
18///
19/// The `A*x + B*y + C*z - D = 0` form is preferred over the other common
20/// alternative, `A*x + B*y + C*z + D = 0`, because it tends to avoid
21/// superfluous negations (see _Real Time Collision Detection_, p. 55).
22///
23/// Copied from: https://github.com/rustgd/collision-rs
24pub struct Plane<S> {
25    /// Plane normal
26    pub n: Vector3<S>,
27    /// Plane distance value
28    pub d: S,
29}
30
31impl<S: BaseFloat> Plane<S> {
32    /// Construct a plane from a normal vector and a scalar distance. The
33    /// plane will be perpendicular to `n`, and `d` units offset from the
34    /// origin.
35    pub fn new(n: Vector3<S>, d: S) -> Plane<S> {
36        Plane { n, d }
37    }
38
39    /// Constructs a plane that passes through the the three points `a`, `b` and `c`
40    pub fn from_points(a: Point3<S>, b: Point3<S>, c: Point3<S>) -> Option<Plane<S>> {
41        // create two vectors that run parallel to the plane
42        let v0 = b - a;
43        let v1 = c - a;
44
45        // find the normal vector that is perpendicular to v1 and v2
46        let n = v0.cross(v1);
47        if ulps_eq!(n, &Vector3::zero()) {
48            None
49        } else {
50            // compute the normal and the distance to the plane
51            let n = n.normalize();
52            let d = -a.dot(n);
53
54            Some(Plane::new(n, d))
55        }
56    }
57
58    /// Construct a plane from a point and a normal vector.
59    /// The plane will contain the point `p` and be perpendicular to `n`.
60    pub fn from_point_normal(p: Point3<S>, n: Vector3<S>) -> Plane<S> {
61        Plane { n, d: p.dot(n) }
62    }
63
64    fn intersection_distance_ray(
65        &self,
66        ray_origin: &Vector3<S>,
67        ray_direction: &Vector3<S>,
68    ) -> Option<S> {
69        let vd: S =
70            self.n.x * ray_direction.x + self.n.y * ray_direction.y + self.n.z * ray_direction.z;
71        if vd == S::zero() {
72            return None;
73        }
74        let t: S =
75            -(self.n.x * ray_origin.x + self.n.y * ray_origin.y + self.n.z * ray_origin.z + self.d)
76                / vd;
77
78        Some(t)
79    }
80
81    /// Returns unsorted intersection points with an Aabb3
82    /// Adopted from: https://www.asawicki.info/news_1428_finding_polygon_of_plane-aabb_intersection
83    /// Inspired by: https://godotengine.org/qa/54688/camera-frustum-intersection-with-plane
84    pub fn intersection_points_aabb3(&self, aabb: &Aabb3<S>) -> Vec<Vector3<S>> {
85        let mut out_points: Vec<Vector3<S>> = Vec::new();
86        let aabb_min: Vector3<S> = aabb.min.to_vec();
87        let aabb_max: Vector3<S> = aabb.max.to_vec();
88
89        // Test edges along X axis, pointing right.
90        let mut dir: Vector3<S> = Vector3::new(aabb_max.x - aabb_min.x, S::zero(), S::zero());
91        let mut orig = aabb_min;
92        if let Some(t) = self.intersection_distance_ray(&orig, &dir) {
93            if t >= S::zero() && t <= S::one() {
94                out_points.push(orig + dir * t);
95            }
96        }
97
98        orig = Vector3::new(aabb_min.x, aabb_max.y, aabb_min.z);
99        if let Some(t) = self.intersection_distance_ray(&orig, &dir) {
100            if t >= S::zero() && t <= S::one() {
101                out_points.push(orig + dir * t);
102            }
103        }
104
105        orig = Vector3::new(aabb_min.x, aabb_min.y, aabb_max.z);
106        if let Some(t) = self.intersection_distance_ray(&orig, &dir) {
107            if t >= S::zero() && t <= S::one() {
108                out_points.push(orig + dir * t);
109            }
110        }
111
112        orig = Vector3::new(aabb_min.x, aabb_max.y, aabb_max.z);
113        if let Some(t) = self.intersection_distance_ray(&orig, &dir) {
114            if t >= S::zero() && t <= S::one() {
115                out_points.push(orig + dir * t);
116            }
117        }
118
119        // Test edges along Y axis, pointing up.
120        dir = Vector3::new(S::zero(), aabb_max.y - aabb_min.y, S::zero());
121        orig = Vector3::new(aabb_min.x, aabb_min.y, aabb_min.z);
122        if let Some(t) = self.intersection_distance_ray(&orig, &dir) {
123            if t >= S::zero() && t <= S::one() {
124                out_points.push(orig + dir * t);
125            }
126        }
127
128        orig = Vector3::new(aabb_max.x, aabb_min.y, aabb_min.z);
129        if let Some(t) = self.intersection_distance_ray(&orig, &dir) {
130            if t >= S::zero() && t <= S::one() {
131                out_points.push(orig + dir * t);
132            }
133        }
134
135        orig = Vector3::new(aabb_min.x, aabb_min.y, aabb_max.z);
136        if let Some(t) = self.intersection_distance_ray(&orig, &dir) {
137            if t >= S::zero() && t <= S::one() {
138                out_points.push(orig + dir * t);
139            }
140        }
141
142        orig = Vector3::new(aabb_max.x, aabb_min.y, aabb_max.z);
143        if let Some(t) = self.intersection_distance_ray(&orig, &dir) {
144            if t >= S::zero() && t <= S::one() {
145                out_points.push(orig + dir * t);
146            }
147        }
148
149        // Test edges along Z axis, pointing forward.
150        dir = Vector3::new(S::zero(), S::zero(), aabb_max.z - aabb_min.z);
151        orig = Vector3::new(aabb_min.x, aabb_min.y, aabb_min.z);
152        if let Some(t) = self.intersection_distance_ray(&orig, &dir) {
153            if t >= S::zero() && t <= S::one() {
154                out_points.push(orig + dir * t);
155            }
156        }
157
158        orig = Vector3::new(aabb_max.x, aabb_min.y, aabb_min.z);
159        if let Some(t) = self.intersection_distance_ray(&orig, &dir) {
160            if t >= S::zero() && t <= S::one() {
161                out_points.push(orig + dir * t);
162            }
163        }
164
165        orig = Vector3::new(aabb_min.x, aabb_max.y, aabb_min.z);
166        if let Some(t) = self.intersection_distance_ray(&orig, &dir) {
167            if t >= S::zero() && t <= S::one() {
168                out_points.push(orig + dir * t);
169            }
170        }
171
172        orig = Vector3::new(aabb_max.x, aabb_max.y, aabb_min.z);
173        if let Some(t) = self.intersection_distance_ray(&orig, &dir) {
174            if t >= S::zero() && t <= S::one() {
175                out_points.push(orig + dir * t);
176            }
177        }
178
179        out_points
180    }
181
182    pub fn intersection_polygon_aabb3(&self, aabb: &Aabb3<S>) -> Vec<Vector3<S>> {
183        let mut points = self.intersection_points_aabb3(aabb);
184
185        if points.is_empty() {
186            return points;
187        };
188
189        let plane_normal = Vector3::new(self.n.x, self.n.y, self.n.z);
190        let origin = points[0];
191
192        points.sort_by(|a, b| {
193            let cmp = (a - origin).cross(b - origin).dot(plane_normal);
194            if cmp < S::zero() {
195                Ordering::Less
196            } else if cmp == S::zero() {
197                Ordering::Equal
198            } else {
199                Ordering::Greater
200            }
201        });
202
203        points
204    }
205}
206
207impl<S: BaseFloat> fmt::Debug for Plane<S> {
208    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
209        write!(
210            f,
211            "{:?}x + {:?}y + {:?}z - {:?} = 0",
212            self.n.x, self.n.y, self.n.z, self.d
213        )
214    }
215}
216
217pub(crate) fn min<S: PartialOrd + Copy>(lhs: S, rhs: S) -> S {
218    match lhs.partial_cmp(&rhs) {
219        Some(Ordering::Less) | Some(Ordering::Equal) | None => lhs,
220        _ => rhs,
221    }
222}
223
224pub(crate) fn max<S: PartialOrd + Copy>(lhs: S, rhs: S) -> S {
225    match lhs.partial_cmp(&rhs) {
226        Some(Ordering::Greater) | Some(Ordering::Equal) | None => lhs,
227        _ => rhs,
228    }
229}
230
231/// A two-dimensional AABB, aka a rectangle.
232pub struct Aabb2<S> {
233    /// Minimum point of the AABB
234    pub min: Point2<S>,
235    /// Maximum point of the AABB
236    pub max: Point2<S>,
237}
238
239impl<S: BaseNum> Aabb2<S> {
240    /// Construct a new axis-aligned bounding box from two points.
241    #[inline]
242    pub fn new(p1: Point2<S>, p2: Point2<S>) -> Aabb2<S> {
243        Aabb2 {
244            min: Point2::new(min(p1.x, p2.x), min(p1.y, p2.y)),
245            max: Point2::new(max(p1.x, p2.x), max(p1.y, p2.y)),
246        }
247    }
248
249    /// Compute corners.
250    #[inline]
251    pub fn to_corners(&self) -> [Point2<S>; 4] {
252        [
253            self.min,
254            Point2::new(self.max.x, self.min.y),
255            Point2::new(self.min.x, self.max.y),
256            self.max,
257        ]
258    }
259}
260
261impl<S: BaseNum> fmt::Debug for Aabb2<S> {
262    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
263        write!(f, "[{:?} - {:?}]", self.min, self.max)
264    }
265}
266
267/// A three-dimensional AABB, aka a rectangular prism.
268pub struct Aabb3<S> {
269    /// Minimum point of the AABB
270    pub min: Point3<S>,
271    /// Maximum point of the AABB
272    pub max: Point3<S>,
273}
274
275impl<S: BaseNum> Aabb3<S> {
276    /// Construct a new axis-aligned bounding box from two points.
277    #[inline]
278    pub fn new(p1: Point3<S>, p2: Point3<S>) -> Aabb3<S> {
279        Aabb3 {
280            min: Point3::new(min(p1.x, p2.x), min(p1.y, p2.y), min(p1.z, p2.z)),
281            max: Point3::new(max(p1.x, p2.x), max(p1.y, p2.y), max(p1.z, p2.z)),
282        }
283    }
284
285    /// Compute corners.
286    #[inline]
287    pub fn to_corners(&self) -> [Point3<S>; 8] {
288        [
289            self.min,
290            Point3::new(self.max.x, self.min.y, self.min.z),
291            Point3::new(self.min.x, self.max.y, self.min.z),
292            Point3::new(self.max.x, self.max.y, self.min.z),
293            Point3::new(self.min.x, self.min.y, self.max.z),
294            Point3::new(self.max.x, self.min.y, self.max.z),
295            Point3::new(self.min.x, self.max.y, self.max.z),
296            self.max,
297        ]
298    }
299}
300
301impl<S: BaseNum> fmt::Debug for Aabb3<S> {
302    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
303        write!(f, "[{:?} - {:?}]", self.min, self.max)
304    }
305}
306
307pub fn bounds_from_points<P, T>(points: impl Iterator<Item = P>) -> Option<([T; 2], [T; 2])>
308where
309    P: Into<[T; 2]>,
310    T: PartialOrd + Copy,
311{
312    let mut min: Option<[T; 2]> = None;
313    let mut max: Option<[T; 2]> = None;
314
315    for point in points {
316        let [x, y] = point.into();
317
318        if let Some([min_x, min_y]) = &mut min {
319            if x < *min_x {
320                *min_x = x;
321            }
322            if y < *min_y {
323                *min_y = y;
324            }
325        } else {
326            min = Some([x, y])
327        }
328
329        if let Some([max_x, max_y]) = &mut max {
330            if x > *max_x {
331                *max_x = x;
332            }
333            if y > *max_y {
334                *max_y = y;
335            }
336        } else {
337            max = Some([x, y])
338        }
339    }
340
341    if let (Some(min), Some(max)) = (min, max) {
342        Some((min, max))
343    } else {
344        None
345    }
346}
347
348/// A wrapper type that enables ordering floats. This is a work around for the famous "rust float
349/// ordering" problem. By using it, you acknowledge that sorting NaN is undefined according to spec.
350/// This implementation treats NaN as the "smallest" float.
351#[derive(Debug, Copy, Clone, PartialOrd)]
352pub struct FloatOrd(pub f32);
353
354impl PartialEq for FloatOrd {
355    fn eq(&self, other: &Self) -> bool {
356        if self.0.is_nan() && other.0.is_nan() {
357            true
358        } else {
359            self.0 == other.0
360        }
361    }
362}
363
364impl Eq for FloatOrd {}
365
366#[allow(clippy::derive_ord_xor_partial_ord)]
367impl Ord for FloatOrd {
368    fn cmp(&self, other: &Self) -> Ordering {
369        self.0.partial_cmp(&other.0).unwrap_or_else(|| {
370            if self.0.is_nan() && !other.0.is_nan() {
371                Ordering::Less
372            } else if !self.0.is_nan() && other.0.is_nan() {
373                Ordering::Greater
374            } else {
375                Ordering::Equal
376            }
377        })
378    }
379}
380
381pub const fn div_away(lhs: i32, rhs: i32) -> i32 {
382    if rhs < 0 {
383        panic!("rhs must be positive")
384    }
385
386    if lhs < 0 {
387        div_floor(lhs, rhs)
388    } else {
389        div_ceil(lhs, rhs)
390    }
391}
392
393pub const fn div_ceil(lhs: i32, rhs: i32) -> i32 {
394    let d = lhs / rhs;
395    let r = lhs % rhs;
396    if (r > 0 && rhs > 0) || (r < 0 && rhs < 0) {
397        d + 1
398    } else {
399        d
400    }
401}
402
403pub const fn div_floor(lhs: i32, rhs: i32) -> i32 {
404    let d = lhs / rhs;
405    let r = lhs % rhs;
406    if (r > 0 && rhs < 0) || (r < 0 && rhs > 0) {
407        d - 1
408    } else {
409        d
410    }
411}
412
413#[cfg(test)]
414mod tests {
415    use crate::{coords::EXTENT_SINT, util::math::div_ceil};
416
417    #[test]
418    pub fn test_div_floor() {
419        assert_eq!(div_ceil(7000, EXTENT_SINT), 2);
420        assert_eq!(div_ceil(-7000, EXTENT_SINT), -1);
421    }
422}
maplibre/util/math.rs

maplibre/util/
math.rs
