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
30use crate::primitives::*;
31use crate::scalar::*;
32use crate::vector::*;
33use num_traits::{Zero, One};
34
35////////////////////////////////////////////////////////////////////////////////
36/// Query Traits
37////////////////////////////////////////////////////////////////////////////////
38pub trait Distance<T, Other> {
39    /// Computes the distance to another value, or `None` if undefined.
40    fn distance(&self, other: &Other) -> Option<T>;
41}
42
43/// Trait for boolean intersection tests.
44pub trait Intersect<T> {
45    /// Returns `true` when two objects intersect.
46    fn intersect(&self, other: &T) -> bool;
47}
48
49/// Trait for producing intersection results.
50pub trait Intersection<T, Other> {
51    /// Returns the intersection result, or `None` if there is no intersection.
52    fn intersection(&self, other: &Other) -> Option<T>;
53}
54
55////////////////////////////////////////////////////////////////////////////////
56/// Distance Queries
57////////////////////////////////////////////////////////////////////////////////
58impl<T: FloatScalar> Distance<T, Vector3<T>> for Plane<T> {
59    fn distance(&self, other: &Vector3<T>) -> Option<T> {
60        let n = self.normal();
61        let nom = Vector3::dot(other, &n) + self.constant();
62        let denom = Vector3::dot(&n, &n);
63        if denom <= T::epsilon() {
64            return None;
65        }
66        Some(T::tabs(nom) / denom)
67    }
68}
69
70impl<T: FloatScalar> Distance<T, Vector3<T>> for Segment<T, Vector3<T>> {
71    fn distance(&self, other: &Vector3<T>) -> Option<T> {
72        Segment::distance(self, other, T::epsilon())
73    }
74}
75
76////////////////////////////////////////////////////////////////////////////////
77/// Intersect Queries
78////////////////////////////////////////////////////////////////////////////////
79impl<T: FloatScalar> Intersect<Ray<T, Vector3<T>>> for Box3<T> {
80    fn intersect(&self, other: &Ray<T, Vector3<T>>) -> bool {
81        // From the paper: An Efficient and Robust Ray–Box Intersection Algorithm by A. Williams et. al.
82        // "... Note also that since IEEE arithmetic guarantees that a positive number divided by zero
83        // is +\infinity and a negative number divided by zero is -\infinity, the code works for vertical
84        // and horizontal line ..."
85
86        let mut tmin = -T::infinity();
87        let mut tmax = T::infinity();
88
89        let mut t0 = (self.min.x - other.start.x) / other.direction.x;
90        let mut t1 = (self.max.x - other.start.x) / other.direction.x;
91
92        tmin = T::max(tmin, T::min(t0, t1));
93        tmax = T::min(tmax, T::max(t0, t1));
94
95        t0 = (self.min.y - other.start.y) / other.direction.y;
96        t1 = (self.max.y - other.start.y) / other.direction.y;
97
98        tmin = T::max(tmin, T::min(t0, t1));
99        tmax = T::min(tmax, T::max(t0, t1));
100
101        t0 = (self.min.z - other.start.z) / other.direction.z;
102        t1 = (self.max.z - other.start.z) / other.direction.z;
103
104        tmin = T::max(tmin, T::min(t0, t1));
105        tmax = T::min(tmax, T::max(t0, t1));
106
107        tmax > T::max(tmin, <T as Zero>::zero())
108    }
109}
110
111impl<T: FloatScalar> Intersect<Sphere3<T>> for Box3<T> {
112    fn intersect(&self, other: &Sphere3<T>) -> bool {
113        let mut dist = <T as Zero>::zero();
114
115        let c = other.center;
116        let r = other.radius;
117
118        if c.x < self.min.x {
119            dist += T::squared(c.x - self.min.x)
120        } else if c.x > self.max.x {
121            dist += T::squared(c.x - self.max.x)
122        }
123
124        if c.y < self.min.y {
125            dist += T::squared(c.y - self.min.y)
126        } else if c.y > self.max.y {
127            dist += T::squared(c.y - self.max.y)
128        }
129
130        if c.z < self.min.z {
131            dist += T::squared(c.z - self.min.z)
132        } else if c.z > self.max.z {
133            dist += T::squared(c.z - self.max.z)
134        }
135
136        r * r > dist
137    }
138}
139
140fn is_in_0_to_1_range<T: FloatScalar>(x: T) -> bool {
141    x >= <T as Zero>::zero() && x <= <T as One>::one()
142}
143
144impl<T: FloatScalar> Intersect<Tri3<T>> for Sphere3<T> {
145    fn intersect(&self, tri: &Tri3<T>) -> bool {
146        let uvw = tri.barycentric_coordinates(&self.center);
147        let verts = tri.vertices();
148        let v0 = verts[0];
149        let v1 = verts[1];
150        let v2 = verts[2];
151        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) {
152            let p = Plane::from_tri(&v0, &v1, &v2);
153            if p.distance(&self.center)
154                .map_or(false, |dist| dist <= self.radius)
155            {
156                true
157            } else {
158                false
159            }
160        } else {
161            let d0 = Segment::new(&v0, &v1).distance(&self.center, T::epsilon());
162            let d1 = Segment::new(&v1, &v2).distance(&self.center, T::epsilon());
163            let d2 = Segment::new(&v2, &v0).distance(&self.center, T::epsilon());
164
165            match (d0, d1, d2) {
166                (Some(d0), Some(d1), Some(d2)) => {
167                    let m = T::min(d0, T::min(d1, d2));
168                    if m <= self.radius {
169                        true
170                    } else {
171                        false
172                    }
173                }
174                _ => false,
175            }
176        }
177    }
178}
179
180///
181/// Ray-Triangle Intersection Test Routines
182/// Different optimizations of my and Ben Trumbore's
183/// code from journals of graphics tools (JGT)
184/// <http://www.acm.org/jgt/>
185/// by Tomas Moller, May 2000
186///
187impl<T: FloatScalar> Intersection<(T, Vector3<T>), Tri3<T>> for Ray<T, Vector3<T>> {
188    fn intersection(&self, tri: &Tri3<T>) -> Option<(T, Vector3<T>)> {
189        let verts = tri.vertices();
190        let v0 = verts[0];
191        let v1 = verts[1];
192        let v2 = verts[2];
193        // find vectors for two edges sharing vert0
194        let edge1 = v1 - v0;
195        let edge2 = v2 - v0;
196
197        // begin calculating determinant - also used to calculate U parameter
198        let pvec = Vector3::cross(&self.direction, &edge2);
199
200        // if determinant is near zero, ray lies in plane of triangle
201        let det = Vector3::dot(&edge1, &pvec);
202
203        if det > -T::epsilon() && det < T::epsilon() {
204            return None; // Parallel
205        }
206
207        // calculate distance from vert0 to ray origin
208        let tvec = self.start - v0;
209
210        let qvec = Vector3::cross(&tvec, &edge1);
211
212        let u = Vector3::dot(&tvec, &pvec) / det;
213
214        if u < -T::epsilon() || u > <T as One>::one() + T::epsilon() {
215            return None; // NoIntersection
216        }
217
218        // calculate V parameter and test bounds
219        let v = Vector3::dot(&self.direction, &qvec) / det;
220        if v < -T::epsilon() || u + v > <T as One>::one() + T::two() * T::epsilon() {
221            return None; // NoIntersection
222        }
223
224        let t = Vector3::dot(&edge2, &qvec) / det;
225        let out = self.start + (self.direction * t);
226        Some((t, out)) //Intersect
227    }
228}
229
230///
231/// Building an Orthonormal Basis from a Unit Vector
232/// John. F. Hughes & Thomas Moller
233/// Journal of Graphics Tools, 4:4, 33-35 (DOI: 10.1080/10867651.1999.10487513)
234///
235/// The original implementation has issues around edge cases (any of x,y,z = 0)
236/// I attempt to fix using the following without proof
237/// TODO: do the proof one day!
238///
239pub fn basis_from_unit<T: FloatScalar>(unit: &Vector3<T>) -> [Vector3<T>; 3] {
240    let u = Vector3::normalize(unit);
241    let x = u.x;
242    let y = u.y;
243    let z = u.z;
244
245    let v = if x.tabs() <= y.tabs() && x.tabs() <= z.tabs() {
246        Vector3::new(<T as Zero>::zero(), -z, y)
247    } else if y.tabs() <= x.tabs() && y.tabs() <= z.tabs() {
248        Vector3::new(-z, <T as Zero>::zero(), x)
249    } else {
250        // z.abs() < x.abs() && z.abs() <= y.abs()
251        Vector3::new(-y, x, <T as Zero>::zero())
252    };
253    let v = Vector3::normalize(&v);
254    let w = Vector3::normalize(&Vector3::cross(&u, &v));
255    [u, w, v]
256}
257
258#[cfg(test)]
259mod tests {
260    use super::Intersect;
261    use crate::primitives::{Box3, Sphere3};
262    use crate::vector::Vector3;
263
264    #[test]
265    fn test_box3_sphere_intersect_z_axis() {
266        let b = Box3::new(
267            &Vector3::new(0.0f32, 0.0, 0.0),
268            &Vector3::new(1.0, 1.0, 1.0),
269        );
270
271        let outside_z = Sphere3::new(Vector3::new(0.5, 0.5, 2.0), 0.25);
272        assert!(!b.intersect(&outside_z));
273
274        let touching_z = Sphere3::new(Vector3::new(0.5, 0.5, 2.0), 1.1);
275        assert!(b.intersect(&touching_z));
276    }
277
278    #[test]
279    fn test_box3_sphere_intersect_on_edge() {
280        let b = Box3::new(
281            &Vector3::new(0.0f32, 0.0, 0.0),
282            &Vector3::new(1.0, 1.0, 1.0),
283        );
284
285        let touching = Sphere3::new(Vector3::new(2.0, 0.5, 0.5), 1.0);
286        assert!(!b.intersect(&touching));
287
288        let just_inside = Sphere3::new(Vector3::new(2.0, 0.5, 0.5), 1.0001);
289        assert!(b.intersect(&just_inside));
290    }
291}
rs_math3d/queries.rs

rs_math3d/
queries.rs
