rustsim_geometry/
segment.rs

1//! Line segments in 2-D and 3-D with closest-point queries.
2
3use crate::vec2::{self, Vec2};
4use crate::vec3::{self, Vec3};
5
6/// 2-D segment from `a` to `b`.
7#[derive(Debug, Clone, Copy, PartialEq)]
8pub struct Segment2 {
9    /// First endpoint.
10    pub a: Vec2,
11    /// Second endpoint.
12    pub b: Vec2,
13}
14
15impl Segment2 {
16    /// Closest point on the segment to `p`.
17    pub fn closest_point(&self, p: Vec2) -> Vec2 {
18        closest_point_on_segment_2(p, self.a, self.b)
19    }
20
21    /// Squared distance from `p` to the closest point on the segment.
22    pub fn distance_squared(&self, p: Vec2) -> f64 {
23        let q = self.closest_point(p);
24        vec2::norm_squared(vec2::sub(p, q))
25    }
26
27    /// Segment length.
28    pub fn length(&self) -> f64 {
29        vec2::distance(self.a, self.b)
30    }
31}
32
33/// 3-D segment from `a` to `b`.
34#[derive(Debug, Clone, Copy, PartialEq)]
35pub struct Segment3 {
36    /// First endpoint.
37    pub a: Vec3,
38    /// Second endpoint.
39    pub b: Vec3,
40}
41
42impl Segment3 {
43    /// Closest point on the segment to `p`.
44    pub fn closest_point(&self, p: Vec3) -> Vec3 {
45        closest_point_on_segment_3(p, self.a, self.b)
46    }
47
48    /// Squared distance from `p` to the closest point on the segment.
49    pub fn distance_squared(&self, p: Vec3) -> f64 {
50        let q = self.closest_point(p);
51        vec3::norm_squared(vec3::sub(p, q))
52    }
53
54    /// Segment length.
55    pub fn length(&self) -> f64 {
56        vec3::distance(self.a, self.b)
57    }
58}
59
60/// Returns the closest point on segment `ab` to point `p` in 2-D.
61#[inline]
62pub fn closest_point_on_segment_2(p: Vec2, a: Vec2, b: Vec2) -> Vec2 {
63    let ab = vec2::sub(b, a);
64    let denom = vec2::dot(ab, ab);
65    if denom < 1e-18 {
66        return a;
67    }
68    let t = (vec2::dot(vec2::sub(p, a), ab) / denom).clamp(0.0, 1.0);
69    vec2::add(a, vec2::scale(ab, t))
70}
71
72/// Returns the closest point on segment `ab` to point `p` in 3-D.
73#[inline]
74pub fn closest_point_on_segment_3(p: Vec3, a: Vec3, b: Vec3) -> Vec3 {
75    let ab = vec3::sub(b, a);
76    let denom = vec3::dot(ab, ab);
77    if denom < 1e-18 {
78        return a;
79    }
80    let t = (vec3::dot(vec3::sub(p, a), ab) / denom).clamp(0.0, 1.0);
81    vec3::add(a, vec3::scale(ab, t))
82}
83
84#[cfg(test)]
85mod tests {
86    use super::*;
87
88    #[test]
89    fn closest_point_on_2d_segment() {
90        let s = Segment2 {
91            a: [0.0, 0.0],
92            b: [10.0, 0.0],
93        };
94        assert_eq!(s.closest_point([-5.0, 5.0]), [0.0, 0.0]);
95        assert_eq!(s.closest_point([15.0, 5.0]), [10.0, 0.0]);
96        assert_eq!(s.closest_point([5.0, 5.0]), [5.0, 0.0]);
97    }
98
99    #[test]
100    fn closest_point_on_3d_segment() {
101        let s = Segment3 {
102            a: [0.0, 0.0, 0.0],
103            b: [0.0, 0.0, 10.0],
104        };
105        assert_eq!(s.closest_point([5.0, 0.0, 5.0]), [0.0, 0.0, 5.0]);
106    }
107}
rustsim_geometry/segment.rs

rustsim_geometry/
segment.rs
