rs_math3d/
queries.rs

1// Copyright 2020-Present (c) Raja Lehtihet & Wael El Oraiby
2//
3// Redistribution and use in source and binary forms, with or without
4// modification, are permitted provided that the following conditions are met:
5//
6// 1. Redistributions of source code must retain the above copyright notice,
7// this list of conditions and the following disclaimer.
8//
9// 2. Redistributions in binary form must reproduce the above copyright notice,
10// this list of conditions and the following disclaimer in the documentation
11// and/or other materials provided with the distribution.
12//
13// 3. Neither the name of the copyright holder nor the names of its contributors
14// may be used to endorse or promote products derived from this software without
15// specific prior written permission.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27// POSSIBILITY OF SUCH DAMAGE.
28//! Intersection and distance query traits and implementations.
29//!
30//! Boolean intersection tests are boundary-inclusive unless stated otherwise:
31//! touching counts as intersecting.
32
33use crate::primitives::*;
34use crate::scalar::*;
35use crate::vector::*;
36use num_traits::{One, Zero};
37
38////////////////////////////////////////////////////////////////////////////////
39/// Query Traits
40////////////////////////////////////////////////////////////////////////////////
41pub trait Distance<T, Other> {
42    /// Computes the distance to another value, or `None` if undefined.
43    fn distance(&self, other: &Other) -> Option<T>;
44}
45
46/// Trait for boolean intersection tests.
47pub trait Intersect<T> {
48    /// Returns `true` when two objects intersect.
49    ///
50    /// Unless an implementation documents otherwise, touching at the boundary
51    /// counts as an intersection.
52    fn intersect(&self, other: &T) -> bool;
53}
54
55/// Trait for producing intersection results.
56pub trait Intersection<T, Other> {
57    /// Returns the intersection result, or `None` if there is no intersection.
58    fn intersection(&self, other: &Other) -> Option<T>;
59}
60
61////////////////////////////////////////////////////////////////////////////////
62/// Distance Queries
63////////////////////////////////////////////////////////////////////////////////
64impl<T: FloatScalar> Distance<T, Vector3<T>> for Plane<T> {
65    fn distance(&self, other: &Vector3<T>) -> Option<T> {
66        let n = self.normal();
67        let nom = Vector3::dot(other, &n) + self.constant();
68        let denom = Vector3::dot(&n, &n);
69        if denom <= T::epsilon() {
70            return None;
71        }
72        Some(T::tabs(nom) / denom)
73    }
74}
75
76impl<T: FloatScalar> Distance<T, Vector3<T>> for Segment<T, Vector3<T>> {
77    fn distance(&self, other: &Vector3<T>) -> Option<T> {
78        Segment::distance(self, other, T::epsilon())
79    }
80}
81
82////////////////////////////////////////////////////////////////////////////////
83/// Intersect Queries
84////////////////////////////////////////////////////////////////////////////////
85impl<T: FloatScalar> Intersect<Ray<T, Vector3<T>>> for Box3<T> {
86    fn intersect(&self, other: &Ray<T, Vector3<T>>) -> bool {
87        // From the paper: An Efficient and Robust Ray–Box Intersection Algorithm by A. Williams et. al.
88        // "... Note also that since IEEE arithmetic guarantees that a positive number divided by zero
89        // is +\infinity and a negative number divided by zero is -\infinity, the code works for vertical
90        // and horizontal line ..."
91
92        let mut tmin = -T::infinity();
93        let mut tmax = T::infinity();
94
95        let mut t0 = (self.min.x - other.start.x) / other.direction.x;
96        let mut t1 = (self.max.x - other.start.x) / other.direction.x;
97
98        tmin = T::max(tmin, T::min(t0, t1));
99        tmax = T::min(tmax, T::max(t0, t1));
100
101        t0 = (self.min.y - other.start.y) / other.direction.y;
102        t1 = (self.max.y - other.start.y) / other.direction.y;
103
104        tmin = T::max(tmin, T::min(t0, t1));
105        tmax = T::min(tmax, T::max(t0, t1));
106
107        t0 = (self.min.z - other.start.z) / other.direction.z;
108        t1 = (self.max.z - other.start.z) / other.direction.z;
109
110        tmin = T::max(tmin, T::min(t0, t1));
111        tmax = T::min(tmax, T::max(t0, t1));
112
113        tmax >= T::max(tmin, <T as Zero>::zero())
114    }
115}
116
117impl<T: FloatScalar> Intersect<Sphere3<T>> for Box3<T> {
118    fn intersect(&self, other: &Sphere3<T>) -> bool {
119        let mut dist = <T as Zero>::zero();
120
121        let c = other.center;
122        let r = other.radius;
123
124        if c.x < self.min.x {
125            dist += T::squared(c.x - self.min.x)
126        } else if c.x > self.max.x {
127            dist += T::squared(c.x - self.max.x)
128        }
129
130        if c.y < self.min.y {
131            dist += T::squared(c.y - self.min.y)
132        } else if c.y > self.max.y {
133            dist += T::squared(c.y - self.max.y)
134        }
135
136        if c.z < self.min.z {
137            dist += T::squared(c.z - self.min.z)
138        } else if c.z > self.max.z {
139            dist += T::squared(c.z - self.max.z)
140        }
141
142        r * r >= dist
143    }
144}
145
146fn is_in_0_to_1_range<T: FloatScalar>(x: T) -> bool {
147    x >= <T as Zero>::zero() && x <= <T as One>::one()
148}
149
150impl<T: FloatScalar> Intersect<Tri3<T>> for Sphere3<T> {
151    fn intersect(&self, tri: &Tri3<T>) -> bool {
152        let uvw = tri.barycentric_coordinates(&self.center);
153        let verts = tri.vertices();
154        let v0 = verts[0];
155        let v1 = verts[1];
156        let v2 = verts[2];
157        if is_in_0_to_1_range(uvw.x) && is_in_0_to_1_range(uvw.y) && is_in_0_to_1_range(uvw.z) {
158            Plane::try_from_tri(&v0, &v1, &v2, T::epsilon()).is_some_and(|p| {
159                p.distance(&self.center)
160                    .is_some_and(|dist| dist <= self.radius)
161            })
162        } else {
163            let d0 = Segment::new(&v0, &v1).distance(&self.center, T::epsilon());
164            let d1 = Segment::new(&v1, &v2).distance(&self.center, T::epsilon());
165            let d2 = Segment::new(&v2, &v0).distance(&self.center, T::epsilon());
166
167            match (d0, d1, d2) {
168                (Some(d0), Some(d1), Some(d2)) => {
169                    let m = T::min(d0, T::min(d1, d2));
170                    m <= self.radius
171                }
172                _ => false,
173            }
174        }
175    }
176}
177
178///
179/// Ray-Triangle Intersection Test Routines
180/// Different optimizations of my and Ben Trumbore's
181/// code from journals of graphics tools (JGT)
182/// <http://www.acm.org/jgt/>
183/// by Tomas Moller, May 2000
184///
185impl<T: FloatScalar> Intersection<(T, Vector3<T>), Tri3<T>> for Ray<T, Vector3<T>> {
186    fn intersection(&self, tri: &Tri3<T>) -> Option<(T, Vector3<T>)> {
187        let verts = tri.vertices();
188        let v0 = verts[0];
189        let v1 = verts[1];
190        let v2 = verts[2];
191        // find vectors for two edges sharing vert0
192        let edge1 = v1 - v0;
193        let edge2 = v2 - v0;
194
195        // begin calculating determinant - also used to calculate U parameter
196        let pvec = Vector3::cross(&self.direction, &edge2);
197
198        // if determinant is near zero, ray lies in plane of triangle
199        let det = Vector3::dot(&edge1, &pvec);
200
201        if det > -T::epsilon() && det < T::epsilon() {
202            return None; // Parallel
203        }
204
205        // calculate distance from vert0 to ray origin
206        let tvec = self.start - v0;
207
208        let qvec = Vector3::cross(&tvec, &edge1);
209
210        let u = Vector3::dot(&tvec, &pvec) / det;
211
212        if u < -T::epsilon() || u > <T as One>::one() + T::epsilon() {
213            return None; // NoIntersection
214        }
215
216        // calculate V parameter and test bounds
217        let v = Vector3::dot(&self.direction, &qvec) / det;
218        if v < -T::epsilon() || u + v > <T as One>::one() + T::two() * T::epsilon() {
219            return None; // NoIntersection
220        }
221
222        let t = Vector3::dot(&edge2, &qvec) / det;
223        let out = self.start + (self.direction * t);
224        Some((t, out)) //Intersect
225    }
226}
227
228///
229/// Building an Orthonormal Basis from a Unit Vector
230/// John. F. Hughes & Thomas Moller
231/// Journal of Graphics Tools, 4:4, 33-35 (DOI: 10.1080/10867651.1999.10487513)
232///
233/// The original implementation has issues around edge cases (any of x,y,z = 0)
234/// I attempt to fix using the following without proof
235/// TODO: do the proof one day!
236///
237pub fn try_basis_from_unit<T: FloatScalar>(
238    unit: &Vector3<T>,
239    epsilon: T,
240) -> Option<[Vector3<T>; 3]> {
241    let u = unit.try_normalize(epsilon)?;
242    let x = u.x;
243    let y = u.y;
244    let z = u.z;
245
246    let v = if x.tabs() <= y.tabs() && x.tabs() <= z.tabs() {
247        Vector3::new(<T as Zero>::zero(), -z, y)
248    } else if y.tabs() <= x.tabs() && y.tabs() <= z.tabs() {
249        Vector3::new(-z, <T as Zero>::zero(), x)
250    } else {
251        // z.abs() < x.abs() && z.abs() <= y.abs()
252        Vector3::new(-y, x, <T as Zero>::zero())
253    };
254    let v = v.try_normalize(epsilon)?;
255    let w = Vector3::cross(&u, &v).try_normalize(epsilon)?;
256    Some([u, w, v])
257}
258
259/// Builds an orthonormal basis from a non-zero input direction.
260///
261/// # Panics
262/// Panics if `unit` is too small to normalize. Use
263/// [`try_basis_from_unit`] when the input may be degenerate.
264pub fn basis_from_unit<T: FloatScalar>(unit: &Vector3<T>) -> [Vector3<T>; 3] {
265    try_basis_from_unit(unit, T::epsilon()).expect("basis direction must be non-zero")
266}
267
268#[cfg(test)]
269mod tests {
270    use super::Intersect;
271    use crate::primitives::{Box3, Ray, Sphere3};
272    use crate::vector::Vector3;
273    use crate::EPS_F32;
274
275    #[test]
276    fn test_box3_sphere_intersect_z_axis() {
277        let b = Box3::new(
278            &Vector3::new(0.0f32, 0.0, 0.0),
279            &Vector3::new(1.0, 1.0, 1.0),
280        );
281
282        let outside_z = Sphere3::new(Vector3::new(0.5, 0.5, 2.0), 0.25);
283        assert!(!b.intersect(&outside_z));
284
285        let touching_z = Sphere3::new(Vector3::new(0.5, 0.5, 2.0), 1.0);
286        assert!(b.intersect(&touching_z));
287    }
288
289    #[test]
290    fn test_box3_sphere_intersect_on_edge() {
291        let b = Box3::new(
292            &Vector3::new(0.0f32, 0.0, 0.0),
293            &Vector3::new(1.0, 1.0, 1.0),
294        );
295
296        let touching = Sphere3::new(Vector3::new(2.0, 0.5, 0.5), 1.0);
297        assert!(b.intersect(&touching));
298
299        let just_inside = Sphere3::new(Vector3::new(2.0, 0.5, 0.5), 1.0001);
300        assert!(b.intersect(&just_inside));
301    }
302
303    #[test]
304    fn test_box3_ray_intersect_touching_boundary() {
305        let b = Box3::new(
306            &Vector3::new(0.0f32, 0.0, 0.0),
307            &Vector3::new(1.0, 1.0, 1.0),
308        );
309        let ray = Ray::new(
310            &Vector3::new(1.0f32, 0.5, 0.5),
311            &Vector3::new(1.0, 0.0, 0.0),
312            EPS_F32,
313        )
314        .expect("ray should be valid");
315        assert!(b.intersect(&ray));
316    }
317
318    #[test]
319    fn test_try_basis_from_unit_zero_none() {
320        let zero = Vector3::new(0.0f32, 0.0, 0.0);
321        assert!(super::try_basis_from_unit(&zero, EPS_F32).is_none());
322    }
323}
rs_math3d/queries.rs

rs_math3d/
queries.rs
