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
use crate::*;
pub fn douglas_peucker_2d<P>(mut pc: PointCloud2D<P>, epsilon: f64) -> PointCloud2D<P>
where
P: Is2D + Clone,
{
if pc.len() < 1 {
return pc;
}
let mut dmax = 0.0;
let mut index = 0;
let end = pc.len() - 1;
for i in 1..end {
let d = distance_point_line(&pc.data[i], &pc.data[0], &pc.data[end]);
if d > dmax {
index = i;
dmax = d;
}
}
if dmax > epsilon {
let mut pc1 = pc.clone();
let mut pc2 = pc.clone();
pc1.data = pc.data.iter().cloned().take(index).collect();
pc1 = douglas_peucker_2d(pc1, epsilon);
pc2.data = pc.data.into_iter().skip(index).collect();
pc2 = douglas_peucker_2d(pc2, epsilon);
let n_take = pc1.data.len() - 1;
pc1.data = pc1.data.into_iter().take(n_take).collect();
pc = pc1.combine(&pc2);
} else {
let p1 = pc.data[0].clone();
let p2 = pc.data[end].clone();
pc.data.clear();
pc.data.push(p1);
pc.data.push(p2);
}
pc
}
fn distance_point_line<P, Q, R>(p: &P, l1: &Q, l2: &R) -> f64
where
P: Is2D,
Q: Is2D,
R: Is2D,
{
let a1 = l1.x();
let a2 = l1.y();
let b1 = l2.x();
let b2 = l2.y();
let c1 = p.x();
let c2 = p.y();
let x = (a1 * a1 * c1 - a1 * a2 * b2 + a1 * a2 * c2 - 2.0 * a1 * b1 * c1 + a1 * b2 * b2
- a1 * b2 * c2
+ a2 * a2 * b1
- a2 * b1 * b2
- a2 * b1 * c2
+ b1 * b1 * c1
+ b1 * b2 * c2)
/ (a1 * a1 - 2.0 * a1 * b1 + a2 * a2 - 2.0 * a2 * b2 + b1 * b1 + b2 * b2);
let y = ((a2 - b2) * x + a1 * b2 - a2 * b1) / (a1 - b1);
((x - p.x()).powi(2) + (y - p.y()).powi(2)).sqrt()
}