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

rs_math3d/
queries.rs
