fyrox_math/
segment.rs

1// Copyright (c) 2019-present Dmitry Stepanov and Fyrox Engine contributors.
2//
3// Permission is hereby granted, free of charge, to any person obtaining a copy
4// of this software and associated documentation files (the "Software"), to deal
5// in the Software without restriction, including without limitation the rights
6// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7// copies of the Software, and to permit persons to whom the Software is
8// furnished to do so, subject to the following conditions:
9//
10// The above copyright notice and this permission notice shall be included in all
11// copies or substantial portions of the Software.
12//
13// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19// SOFTWARE.
20
21use nalgebra::{RealField, SVector, Scalar, Vector2};
22use num_traits::{One, Signed, Zero};
23use rectutils::{Number, Rect};
24
25/// Line segment in two dimensions
26pub type LineSegment2<T> = LineSegment<T, 2>;
27/// Line segment in three dimensions
28pub type LineSegment3<T> = LineSegment<T, 3>;
29
30/// Line segment in any number of dimensions
31#[derive(Clone, Debug)]
32pub struct LineSegment<T, const D: usize> {
33    /// One end of the line segment, the point returned when interpolating at t = 0.0
34    pub start: SVector<T, D>,
35    /// One end of the line segment, the point returned when interpolating at t = 1.0
36    pub end: SVector<T, D>,
37}
38
39impl<T, const D: usize> LineSegment<T, D>
40where
41    T: Zero + One + Scalar + RealField,
42{
43    /// Create a new line segment with the given points.
44    pub fn new(start: &SVector<T, D>, end: &SVector<T, D>) -> Self {
45        Self {
46            start: start.clone_owned(),
47            end: end.clone_owned(),
48        }
49    }
50    /// Creates a reversed line segment by swapping `start` and `end`.
51    pub fn swapped(&self) -> Self {
52        Self::new(&self.end, &self.start)
53    }
54    /// The two end-points of the line segment are equal.
55    pub fn is_degenerate(&self) -> bool {
56        self.start == self.end
57    }
58    /// Create a point somewhere between `start` and `end`.
59    /// When t = 0.0, `start` is returned.
60    /// When t = 1.0, `end` is returned.
61    /// The result is `(1.0 - t) * start + t * end`, which may produce points off the line segment,
62    /// if t < 0.0 or t > 1.0.
63    pub fn interpolate(&self, t: T) -> SVector<T, D> {
64        self.start.lerp(&self.end, t)
65    }
66    /// Create a point somewhere between `start` and `end`.
67    /// This is just like [LineSegment::interpolate] except that t is clamped to between 0.0 and 1.0,
68    /// so points off the line segment can never be returned.
69    pub fn interpolate_clamped(&self, t: T) -> SVector<T, D> {
70        self.interpolate(t.clamp(<T as Zero>::zero(), <T as One>::one()))
71    }
72    /// The vector from `start` to `end`
73    pub fn vector(&self) -> SVector<T, D> {
74        self.end.clone() - self.start.clone()
75    }
76    /// The distance between `start` and `end`
77    pub fn length(&self) -> T {
78        self.vector().norm()
79    }
80    /// The square of the distance between `start` and `end`
81    pub fn length_squared(&self) -> T {
82        self.vector().norm_squared()
83    }
84    /// The interpolation parameter of the point on this segment that is closest to the given point.
85    ///
86    /// [Stack Exchange question: Find a point on a line segment which is the closest to other point not on the line segment](https://math.stackexchange.com/questions/2193720/find-a-point-on-a-line-segment-which-is-the-closest-to-other-point-not-on-the-li)
87    pub fn nearest_t(&self, point: &SVector<T, D>) -> T {
88        let v = self.vector();
89        let u = self.start.clone() - point;
90        let n2 = v.norm_squared();
91        if n2.is_zero() {
92            return T::zero();
93        }
94        -v.dot(&u) / n2
95    }
96    /// The point on this segment that is closest to the given point.
97    pub fn nearest_point(&self, point: &SVector<T, D>) -> SVector<T, D> {
98        self.interpolate_clamped(self.nearest_t(point))
99    }
100    /// The squared distance between the given point and the nearest point on this line segment.
101    pub fn distance_squared(&self, point: &SVector<T, D>) -> T {
102        (point - self.nearest_point(point)).norm_squared()
103    }
104    /// The distance between the given point and the nearest point on this line segment.
105    pub fn distance(&self, point: &SVector<T, D>) -> T {
106        (point - self.nearest_point(point)).norm()
107    }
108}
109
110impl<T> LineSegment2<T>
111where
112    T: Zero + One + Scalar + RealField,
113{
114    /// AABB for a 2D line segment
115    pub fn bounds(&self) -> Rect<T>
116    where
117        T: Number,
118    {
119        Rect::from_points(self.start, self.end)
120    }
121    /// Test whether a point is collinear with this segment.
122    /// * 0.0 means collinear. Near to 0.0 means near to collinear.
123    /// * Negative means that the point is to the counter-clockwise of `end` as viewed from `start`.
124    /// * Positive means that the point is to the clockwise of `end` as viewed from `start`.
125    pub fn collinearity(&self, point: &Vector2<T>) -> T {
126        let v = self.vector();
127        let u = self.start.clone() - point;
128        v.x.clone() * u.y.clone() - u.x.clone() * v.y.clone()
129    }
130    /// True if this segment intersects the given segment based on collinearity.
131    pub fn intersects(&self, other: &LineSegment2<T>) -> bool {
132        fn pos<T>(t: &T) -> bool
133        where
134            T: Zero + Signed,
135        {
136            t.is_positive() && !t.is_zero()
137        }
138        fn neg<T>(t: &T) -> bool
139        where
140            T: Zero + Signed,
141        {
142            t.is_negative() && !t.is_zero()
143        }
144        let o1 = self.collinearity(&other.start);
145        let o2 = self.collinearity(&other.end);
146        let s1 = other.collinearity(&self.start);
147        let s2 = other.collinearity(&self.end);
148        // If both points of self are left of `other` or both points are right of `other`...
149        if neg(&s1) && neg(&s2) || pos(&s1) && pos(&s2) {
150            return false;
151        }
152        // If both points of `other` are left of self or both points are right of self...
153        if neg(&o1) && neg(&o2) || pos(&o1) && pos(&o2) {
154            return false;
155        }
156        true
157    }
158}
159
160#[cfg(test)]
161mod test {
162    use super::*;
163    use nalgebra::Vector2;
164    #[test]
165    fn nearest_at_start() {
166        let segment = LineSegment2::new(&Vector2::new(0.0, 0.0), &Vector2::new(1.0, 2.0));
167        assert_eq!(segment.nearest_t(&Vector2::new(-1.0, -1.0)).max(0.0), 0.0);
168        assert_eq!(
169            segment.nearest_point(&Vector2::new(-1.0, -1.0)),
170            Vector2::new(0.0, 0.0)
171        );
172        assert_eq!(segment.distance_squared(&Vector2::new(-1.0, -1.0)), 2.0);
173        assert_eq!(segment.distance(&Vector2::new(-1.0, 0.0)), 1.0);
174    }
175    #[test]
176    fn nearest_at_end() {
177        let segment = LineSegment2::new(&Vector2::new(0.0, 0.0), &Vector2::new(1.0, 2.0));
178        assert_eq!(segment.nearest_t(&Vector2::new(2.0, 2.0)).min(1.0), 1.0);
179        assert_eq!(
180            segment.nearest_point(&Vector2::new(2.0, 2.0)),
181            Vector2::new(1.0, 2.0)
182        );
183        assert_eq!(segment.distance_squared(&Vector2::new(3.0, 2.0)), 4.0);
184        assert_eq!(segment.distance(&Vector2::new(3.0, 2.0)), 2.0);
185    }
186    #[test]
187    fn nearest_in_middle() {
188        let segment = LineSegment2::new(&Vector2::new(0.0, 0.0), &Vector2::new(1.0, 2.0));
189        assert_eq!(segment.nearest_t(&Vector2::new(2.5, 0.0)), 0.5);
190        assert_eq!(
191            segment.nearest_point(&Vector2::new(2.5, 0.0)),
192            Vector2::new(0.5, 1.0)
193        );
194        assert_eq!(segment.distance_squared(&Vector2::new(2.5, 0.0)), 5.0);
195    }
196    #[test]
197    fn length() {
198        let segment = LineSegment2::new(&Vector2::new(0.0, 0.0), &Vector2::new(4.0, 3.0));
199        assert_eq!(segment.length_squared(), 25.0);
200        assert_eq!(segment.length(), 5.0);
201    }
202    #[test]
203    fn degenerate() {
204        let segment = LineSegment2::new(&Vector2::new(1.0, 2.0), &Vector2::new(1.0, 2.0));
205        assert!(segment.is_degenerate());
206        assert_eq!(segment.length_squared(), 0.0);
207        assert_eq!(segment.length(), 0.0);
208    }
209    #[test]
210    fn collinear() {
211        let segment = LineSegment2::new(&Vector2::new(0.0, 0.0), &Vector2::new(1.0, 2.0));
212        assert_eq!(segment.collinearity(&Vector2::new(2.0, 4.0)), 0.0);
213        assert_eq!(segment.collinearity(&Vector2::new(0.0, 0.0)), 0.0);
214        assert_eq!(segment.collinearity(&Vector2::new(1.0, 2.0)), 0.0);
215        assert!(
216            segment.collinearity(&Vector2::new(1.0, 5.0)) < 0.0,
217            "{} >= 0.0",
218            segment.collinearity(&Vector2::new(1.0, 5.0))
219        );
220        assert!(
221            segment.collinearity(&Vector2::new(1.0, 3.0)) < 0.0,
222            "{} >= 0.0",
223            segment.collinearity(&Vector2::new(1.0, 3.0))
224        );
225        assert!(
226            segment.collinearity(&Vector2::new(1.0, 1.0)) > 0.0,
227            "{} <= 0.0",
228            segment.collinearity(&Vector2::new(1.0, 1.0))
229        );
230        assert!(
231            segment.collinearity(&Vector2::new(-1.0, -5.0)) > 0.0,
232            "{} <= 0.0",
233            segment.collinearity(&Vector2::new(-1.0, -5.0))
234        );
235    }
236    #[test]
237    fn intersects() {
238        let a = LineSegment::new(&Vector2::new(1.0, 2.0), &Vector2::new(3.0, 1.0));
239        let b = LineSegment::new(&Vector2::new(2.0, 0.0), &Vector2::new(2.5, 3.0));
240        let c = LineSegment::new(&Vector2::new(1.0, 2.0), &Vector2::new(-3.0, 1.0));
241        assert!(a.intersects(&b));
242        assert!(a.intersects(&c));
243        assert!(b.intersects(&a));
244        assert!(c.intersects(&a));
245        assert!(a.swapped().intersects(&b));
246        assert!(a.swapped().intersects(&c));
247    }
248    #[test]
249    fn not_intersects() {
250        let a = LineSegment::new(&Vector2::new(1.0, 2.0), &Vector2::new(3.0, 1.0));
251        let b = LineSegment::new(&Vector2::new(0.0, 0.0), &Vector2::new(-1.0, 6.0));
252        let c = LineSegment::new(&Vector2::new(2.0, 0.0), &Vector2::new(2.0, -1.0));
253        assert!(!a.intersects(&b));
254        assert!(!b.intersects(&c));
255        assert!(!c.intersects(&a));
256        assert!(!b.intersects(&a));
257        assert!(!c.intersects(&b));
258        assert!(!a.intersects(&c));
259        assert!(!a.swapped().intersects(&b));
260        assert!(!b.swapped().intersects(&c));
261        assert!(!c.swapped().intersects(&a));
262    }
263}
fyrox_math/segment.rs

fyrox_math/
segment.rs
