1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
use crate::aabb::AABB;
use crate::envelope::Envelope;
use crate::object::PointDistance;
use crate::object::RTreeObject;
use crate::point::{Point, PointExt};
use num_traits::{One, Zero};
#[derive(Debug, Clone, Copy, Eq, PartialEq, Ord, PartialOrd)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Line<P>
where
P: Point,
{
pub from: P,
pub to: P,
}
impl<P> Line<P>
where
P: Point,
{
pub fn new(from: P, to: P) -> Self {
Line { from, to }
}
}
impl<P> RTreeObject for Line<P>
where
P: Point,
{
type Envelope = AABB<P>;
fn envelope(&self) -> Self::Envelope {
AABB::from_corners(self.from, self.to)
}
}
impl<P> Line<P>
where
P: Point,
{
pub fn length_2(&self) -> P::Scalar {
self.from.sub(&self.to).length_2()
}
fn project_point(&self, query_point: &P) -> P::Scalar {
let (ref p1, ref p2) = (self.from, self.to);
let dir = p2.sub(p1);
query_point.sub(p1).dot(&dir) / dir.length_2()
}
pub fn nearest_point(&self, query_point: &P) -> P {
let (p1, p2) = (self.from, self.to);
let dir = p2.sub(&p1);
let s = self.project_point(query_point);
if P::Scalar::zero() < s && s < One::one() {
p1.add(&dir.mul(s))
} else if s <= P::Scalar::zero() {
p1
} else {
p2
}
}
}
impl<P> PointDistance for Line<P>
where
P: Point,
{
fn distance_2(
&self,
point: &<Self::Envelope as Envelope>::Point,
) -> <<Self::Envelope as Envelope>::Point as Point>::Scalar {
self.nearest_point(point).sub(point).length_2()
}
}
#[cfg(test)]
mod test {
use super::Line;
use crate::object::PointDistance;
use approx::*;
#[test]
fn edge_distance() {
let edge = Line::new([0.5, 0.5], [0.5, 2.0]);
assert_abs_diff_eq!(edge.distance_2(&[0.5, 0.5]), 0.0);
assert_abs_diff_eq!(edge.distance_2(&[0.0, 0.5]), 0.5 * 0.5);
assert_abs_diff_eq!(edge.distance_2(&[0.5, 1.0]), 0.0);
assert_abs_diff_eq!(edge.distance_2(&[0.0, 0.0]), 0.5);
assert_abs_diff_eq!(edge.distance_2(&[0.0, 1.0]), 0.5 * 0.5);
assert_abs_diff_eq!(edge.distance_2(&[1.0, 1.0]), 0.5 * 0.5);
assert_abs_diff_eq!(edge.distance_2(&[1.0, 3.0]), 0.5 * 0.5 + 1.0);
}
#[test]
fn length_2() {
let line = Line::new([1, -1], [5, 5]);
assert_eq!(line.length_2(), 16 + 36);
}
}