geop_core_math/primitives/
ray.rs

1//! [`Ray`]: a half-line from a point in a direction, and how near it passes
2//! to points, segments, triangles, planes and lines.
3
4use serde::{Deserialize, Serialize};
5
6use super::CoordinateSystem;
7use crate::{
8    geop_error::{GeopError, GeopResult},
9    scalars::Scalar,
10    vector::{Vector2, Vector3},
11};
12
13/// The points `origin + t dir` for `t >= 0`. `dir` has unit length, so `t`
14/// is a distance.
15#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
16#[serde(try_from = "RayData<S>", bound = "S: Scalar")]
17pub struct Ray<S: Scalar> {
18    origin: Vector3<S>,
19    dir: Vector3<S>,
20}
21
22/// A ray as it is written: any direction, normalized on reading.
23#[derive(Deserialize)]
24#[serde(bound(deserialize = "S: Scalar"))]
25struct RayData<S: Scalar> {
26    origin: Vector3<S>,
27    dir: Vector3<S>,
28}
29
30impl<S: Scalar> TryFrom<RayData<S>> for Ray<S> {
31    type Error = GeopError;
32
33    fn try_from(data: RayData<S>) -> GeopResult<Self> {
34        Ray::try_new(data.origin, data.dir)
35    }
36}
37
38/// `t`, or zero if it is definitely negative: a distance along a ray, which
39/// never lies behind its origin.
40fn ahead<S: Scalar>(t: S) -> S {
41    if t.definitely_less(S::ZERO) {
42        S::ZERO
43    } else {
44        t
45    }
46}
47
48impl<S: Scalar> Ray<S> {
49    /// The ray from `origin` along `dir`, of any length. Fails if `dir`
50    /// could be zero.
51    pub fn try_new(origin: Vector3<S>, dir: Vector3<S>) -> GeopResult<Self> {
52        let dir = dir
53            .normalize()
54            .map_err(|e| e.with_context(format!("Ray::try_new(origin={origin:?}, dir={dir:?})")))?;
55        Ok(Self { origin, dir })
56    }
57
58    pub fn origin(&self) -> &Vector3<S> {
59        &self.origin
60    }
61
62    /// Unit length.
63    pub fn dir(&self) -> &Vector3<S> {
64        &self.dir
65    }
66
67    /// The point `t` along the ray.
68    pub fn at(&self, t: S) -> Vector3<S> {
69        self.origin.add(&self.dir.prod_scalar(t))
70    }
71
72    /// How far along the ray it comes closest to `p`, never behind its
73    /// origin.
74    pub fn closest_to_point(&self, p: &Vector3<S>) -> S {
75        ahead(p.sub(&self.origin).prod_dot(&self.dir))
76    }
77
78    /// How far the ray passes from `p`, and how far along it that is.
79    pub fn distance_to_point(&self, p: &Vector3<S>) -> (S, S) {
80        let t = self.closest_to_point(p);
81        (self.at(t).sub(p).norm(), t)
82    }
83
84    /// How far the ray passes from the segment `a..b`, and how far along
85    /// the ray that is.
86    pub fn distance_to_segment(&self, a: &Vector3<S>, b: &Vector3<S>) -> (S, S) {
87        let d = b.sub(a);
88        let r = self.origin.sub(a);
89        let e = d.norm_sq();
90        if e.could_be_equal(S::ZERO) {
91            return self.distance_to_point(a);
92        }
93        let b_ = self.dir.prod_dot(&d);
94        let c = self.dir.prod_dot(&r);
95        let f = d.prod_dot(&r);
96        // `dir` has unit length, so `dir . dir = 1`: the ray and the
97        // segment's line are parallel exactly where this vanishes.
98        let denom = e.sub(b_.mul(b_));
99        let t = if denom.could_be_equal(S::ZERO) {
100            S::ZERO
101        } else {
102            ahead(b_.mul(f).sub(c.mul(e)).div(denom).unwrap_or(S::ZERO))
103        };
104        let u = b_.mul(t).add(f).div(e).unwrap_or(S::ZERO);
105        let (u, t) = if u.definitely_less(S::ZERO) {
106            (S::ZERO, self.closest_to_point(a))
107        } else if u.definitely_greater(S::ONE) {
108            (S::ONE, self.closest_to_point(b))
109        } else {
110            (u, t)
111        };
112        let on_segment = a.add(&d.prod_scalar(u));
113        (self.at(t).sub(&on_segment).norm(), t)
114    }
115
116    /// How far along the ray it enters the triangle `a, b, c`, if it could
117    /// (Möller–Trumbore).
118    pub fn intersect_triangle(&self, a: &Vector3<S>, b: &Vector3<S>, c: &Vector3<S>) -> Option<S> {
119        let e1 = b.sub(a);
120        let e2 = c.sub(a);
121        let h = self.dir.prod_cross(&e2);
122        let det = e1.prod_dot(&h);
123        if det.could_be_equal(S::ZERO) {
124            return None;
125        }
126        let s = self.origin.sub(a);
127        let q = s.prod_cross(&e1);
128        let u = s.prod_dot(&h).div(det).ok()?;
129        let v = self.dir.prod_dot(&q).div(det).ok()?;
130        let t = e2.prod_dot(&q).div(det).ok()?;
131        let outside = u.definitely_less(S::ZERO)
132            || v.definitely_less(S::ZERO)
133            || u.add(v).definitely_greater(S::ONE)
134            || t.definitely_less(S::ZERO);
135        (!outside).then_some(t)
136    }
137
138    /// Where the ray meets the plane through `origin` normal to `normal`,
139    /// and how far along it: `None` if it could run along the plane, or
140    /// points away from it.
141    pub fn intersect_plane(
142        &self,
143        origin: &Vector3<S>,
144        normal: &Vector3<S>,
145    ) -> Option<(S, Vector3<S>)> {
146        let denom = self.dir.prod_dot(normal);
147        if denom.could_be_equal(S::ZERO) {
148            return None;
149        }
150        let t = origin.sub(&self.origin).prod_dot(normal).div(denom).ok()?;
151        (!t.definitely_less(S::ZERO)).then(|| (t, self.at(t)))
152    }
153
154    /// Where the ray meets `frame`'s `u`/`v` plane, in `u`/`v` coordinates,
155    /// and how far along the ray: `None` if it could run along the plane,
156    /// or points away from it.
157    pub fn intersect_uv_plane(&self, frame: &CoordinateSystem<S>) -> Option<(S, Vector2<S>)> {
158        let (t, p) = self.intersect_plane(frame.origin(), frame.w())?;
159        Some((t, frame.to_uvw(&p).head()))
160    }
161
162    /// The parameter `s` of the point `at + s direction` of a line nearest
163    /// the ray's line. `None` if the two could be parallel, where no point
164    /// of the line is nearer than another.
165    pub fn line_parameter(&self, at: &Vector3<S>, direction: &Vector3<S>) -> Option<S> {
166        let w0 = at.sub(&self.origin);
167        let b = direction.prod_dot(&self.dir);
168        let denom = direction.norm_sq().sub(b.mul(b));
169        if denom.could_be_equal(S::ZERO) {
170            return None;
171        }
172        b.mul(self.dir.prod_dot(&w0))
173            .sub(direction.prod_dot(&w0))
174            .div(denom)
175            .ok()
176    }
177}
178
179#[cfg(test)]
180mod tests {
181    use super::*;
182    use crate::for_all_scalars;
183
184    fn v<S: Scalar>(x: f64, y: f64, z: f64) -> Vector3<S> {
185        Vector3::from_array([x, y, z].map(S::from_f64))
186    }
187
188    /// Looking straight down `-z` from `(x, y, 10)`.
189    fn down<S: Scalar>(x: f64, y: f64) -> Ray<S> {
190        Ray::try_new(v(x, y, 10.0), v(0.0, 0.0, -2.0)).unwrap()
191    }
192
193    /// Whether `a` encloses `b`.
194    fn close<S: Scalar>(a: S, b: f64) -> bool {
195        a.could_be_equal(S::from_f64(b))
196    }
197
198    fn check_points_and_segments<S: Scalar>() {
199        let (dist, t) = down::<S>(0.0, 0.3).distance_to_point(&v(0.0, 0.0, 1.0));
200        assert!(close(dist, 0.3) && close(t, 9.0));
201        // Past the end of the segment, its end is nearest.
202        let (dist, _) =
203            down::<S>(2.0, 0.0).distance_to_segment(&v(0.0, 0.0, 0.0), &v(1.0, 0.0, 0.0));
204        assert!(close(dist, 1.0));
205        let (dist, t) =
206            down::<S>(0.5, 0.2).distance_to_segment(&v(0.0, 0.0, 0.0), &v(1.0, 0.0, 0.0));
207        assert!(close(dist, 0.2) && close(t, 10.0));
208    }
209
210    #[test]
211    fn points_and_segments() {
212        for_all_scalars!(check_points_and_segments);
213    }
214
215    fn check_triangles_and_planes<S: Scalar>() {
216        let (a, b, c) = (v::<S>(0.0, 0.0, 1.0), v(1.0, 0.0, 1.0), v(0.0, 1.0, 1.0));
217        assert!(close(
218            down::<S>(0.2, 0.2).intersect_triangle(&a, &b, &c).unwrap(),
219            9.0
220        ));
221        assert!(down::<S>(0.8, 0.8).intersect_triangle(&a, &b, &c).is_none());
222        let (t, p) = down::<S>(1.0, 2.0)
223            .intersect_plane(&v(0.0, 0.0, -1.0), &v(0.0, 0.0, 1.0))
224            .unwrap();
225        assert!(close(t, 11.0) && close(p[1], 2.0));
226        // Behind the ray, and along it.
227        assert!(
228            down::<S>(0.0, 0.0)
229                .intersect_plane(&v(0.0, 0.0, 20.0), &v(0.0, 0.0, 1.0))
230                .is_none()
231        );
232        assert!(
233            down::<S>(0.0, 0.0)
234                .intersect_plane(&v(0.0, 0.0, 0.0), &v(1.0, 0.0, 0.0))
235                .is_none()
236        );
237    }
238
239    #[test]
240    fn triangles_and_planes() {
241        for_all_scalars!(check_triangles_and_planes);
242    }
243
244    /// Seen from the side, the point of a line along `z` nearest the ray
245    /// follows the ray's height — and looking straight along the line, no
246    /// point of it is nearest.
247    fn check_line_parameters<S: Scalar>() {
248        let side = Ray::try_new(v::<S>(5.0, 0.0, 0.7), v(-1.0, 0.0, 0.0)).unwrap();
249        let s = side
250            .line_parameter(&v(0.0, 0.0, 0.0), &v(0.0, 0.0, 2.0))
251            .unwrap();
252        assert!(close(s, 0.35));
253        assert!(
254            down::<S>(0.0, 0.0)
255                .line_parameter(&v(0.0, 0.0, 0.0), &v(0.0, 0.0, 1.0))
256                .is_none()
257        );
258    }
259
260    #[test]
261    fn line_parameters() {
262        for_all_scalars!(check_line_parameters);
263    }
264}
geop_core_math/primitives/ray.rs

geop_core_math/primitives/
ray.rs
