veloren_common/util/
lines.rs

1use vek::{LineSegment2, LineSegment3, Vec2, Vec3};
2
3// Get closest point between 2 3D line segments https://math.stackexchange.com/a/4289668
4pub fn closest_points_3d(n: LineSegment3<f32>, m: LineSegment3<f32>) -> (Vec3<f32>, Vec3<f32>) {
5    let p1 = n.start;
6    let p2 = n.end;
7    let p3 = m.start;
8    let p4 = m.end;
9
10    let d1 = p2 - p1;
11    let d2 = p4 - p3;
12    let d21 = p3 - p1;
13
14    let v22 = d2.dot(d2);
15    let v11 = d1.dot(d1);
16    let v21 = d2.dot(d1);
17    let v21_1 = d21.dot(d1);
18    let v21_2 = d21.dot(d2);
19
20    let denom = v21 * v21 - v22 * v11;
21
22    let (s, t) = if denom == 0.0 {
23        let s = 0.0;
24        let t = (v11 * s - v21_1) / v21;
25        (s, t)
26    } else {
27        let s = (v21_2 * v21 - v22 * v21_1) / denom;
28        let t = (-v21_1 * v21 + v11 * v21_2) / denom;
29        (s, t)
30    };
31
32    let (s, t) = (s.clamp(0.0, 1.0), t.clamp(0.0, 1.0));
33
34    let p_a = p1 + s * d1;
35    let p_b = p3 + t * d2;
36
37    (p_a, p_b)
38}
39
40/// Line result type
41pub enum LineIntersection<T>
42where
43    T: num_traits::Float + Copy,
44{
45    /// The intersection is a point.
46    Point(Vec2<T>),
47    /// The lines are coincident and do not intersect.
48    Coincident,
49    /// The lines are parallel and do not intersect.
50    Parallel,
51}
52
53/// Calculate the intersection of two 2D lines.
54/// The lines are defined by two line segments, and the intersection may lie
55/// outside either segment. I.e., for intersection purposes the lines are
56/// considered infinite. This function does not guarantee that the intersection
57/// point lies within the bounds of the line segments or that the intersection
58/// point lies within some coordinate space (like world size).
59///
60/// # Arguments
61/// * `n` - The first line segment.
62/// * `m` - The second line segment.
63///
64/// # Returns
65/// The intersection type.
66/// * LineIntersection::Point if there is an intersection.
67/// * LineIntersection::Coincident if the lines are coincident and there is no
68///   intersection. This means the lines lie on top of each other. They are
69///   parallel but have no separation. It could be said that they intersect at
70///   infinitely many points.
71/// * LineIntersection::Parallel if the lines are parallel and there is no
72///   intersection.
73pub fn line_intersection_2d<T>(n: LineSegment2<T>, m: LineSegment2<T>) -> LineIntersection<T>
74where
75    T: num_traits::Float + Copy,
76{
77    let a = n.end.x - n.start.x;
78    let b = -(m.end.x - m.start.x);
79    let c = m.start.x - n.start.x;
80    let d = n.end.y - n.start.y;
81    let e = -(m.end.y - m.start.y);
82    let f = m.start.y - n.start.y;
83
84    let num1 = c * e - b * f;
85    let num2 = a * f - c * d;
86    let denom = a * e - b * d;
87
88    if denom.abs() < T::epsilon() {
89        if num1.abs() < T::epsilon() && num2.abs() < T::epsilon() {
90            // Lines are coincident.
91            return LineIntersection::Coincident;
92        }
93        // Lines are parallel.
94        return LineIntersection::Parallel;
95    }
96
97    LineIntersection::Point(Vec2::new(
98        n.start.x + (num1 / denom) * a,
99        n.start.y + (num1 / denom) * d,
100    ))
101}
102
103#[cfg(test)]
104mod tests {
105    use super::*;
106    use vek::{LineSegment2, Vec2};
107
108    #[macro_export]
109    macro_rules! vec_to_line_segment2 {
110        ($vec:expr) => {
111            LineSegment2 {
112                start: Vec2::new($vec[0], $vec[1]),
113                end: Vec2::new($vec[2], $vec[3]),
114            }
115        };
116    }
117
118    #[test]
119    fn test_intersecting_line() {
120        let l1 = [0.0f32, 0.0, 2.0, 2.0];
121        let l2 = [0.0f32, 2.0, 2.0, 0.0];
122        let n = vec_to_line_segment2!(l1);
123        let m = vec_to_line_segment2!(l2);
124        match line_intersection_2d(n, m) {
125            LineIntersection::Point(p) => {
126                assert!((p.x - 1.0).abs() < 1e-6);
127                assert!((p.y - 1.0).abs() < 1e-6);
128            },
129            _ => panic!("Should intersect at (1, 1)"),
130        }
131    }
132
133    #[test]
134    fn test_parallel_lines() {
135        let l1 = [0.0f64, 0.0, 1.0, 1.0];
136        let l2 = [0.0f64, 1.0, 1.0, 2.0];
137        let n = vec_to_line_segment2!(l1);
138        let m = vec_to_line_segment2!(l2);
139        match line_intersection_2d(n, m) {
140            LineIntersection::Parallel => {},
141            _ => panic!("Should be parallel"),
142        }
143    }
144
145    #[test]
146    fn test_coincident_lines() {
147        let l1 = [0.0f32, 0.0, 1.0, 1.0];
148        let l2 = [0.5f32, 0.5, 1.5, 1.5];
149        let n = vec_to_line_segment2!(l1);
150        let m = vec_to_line_segment2!(l2);
151        match line_intersection_2d(n, m) {
152            LineIntersection::Coincident => {},
153            _ => panic!("Should be coincident"),
154        }
155    }
156
157    #[test]
158    fn test_almost_parallel_lines() {
159        let l1 = [0.0f64, 0.0, 1.0, 1.0];
160        let l2 = [0.0f64, 1.0, 1.0, 1.999999];
161        let n = vec_to_line_segment2!(l1);
162        let m = vec_to_line_segment2!(l2);
163        let expect = [1000000.0000823, 1000000.0000823];
164        match line_intersection_2d(n, m) {
165            LineIntersection::Point(p) => {
166                assert!((p.x - expect[0]).abs() < 1e-6);
167                assert!((p.y - expect[1]).abs() < 1e-6);
168            },
169            _ => panic!("Should intersect"),
170        }
171    }
172}
veloren_common/util/lines.rs

veloren_common/util/
lines.rs
