-
Notifications
You must be signed in to change notification settings - Fork 1
/
RamerDouglasPeucker.cpp
126 lines (97 loc) · 4.34 KB
/
RamerDouglasPeucker.cpp
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
#include <cassert>
#include <tuple>
#include <vector>
namespace rdp {
struct Point2D
{
double x;
double y;
};
Point2D operator-(Point2D a, Point2D b)
{
return {a.x - b.x, a.y - b.y};
}
double abs2(Point2D p)
{
return p.x * p.x + p.y * p.y;
}
// Find the point furthest away from reference (points[startIndex] == points[endIndex])
std::pair<double, std::size_t> findMostDistantPoint(const std::vector<Point2D> &points,
std::size_t startIndex, std::size_t endIndex)
{
assert(startIndex < endIndex && "Start index must be smaller than end index");
assert(endIndex < points.size() && "End index is larger than the number of points");
assert(points.size() >= 2 && "At least two points needed");
assert(abs2(points[startIndex] - points[endIndex]) == 0 && "Start and end point must be equal");
double maxDistanceSquared = 0.0;
std::size_t maxDistanceIndex = startIndex;
for (std::size_t i = startIndex + 1; i != endIndex; ++i)
{
double distanceSquared = abs2(points[i] - points[startIndex]);
if (distanceSquared > maxDistanceSquared)
{
maxDistanceIndex = i;
maxDistanceSquared = distanceSquared;
}
}
return std::make_pair(maxDistanceSquared, maxDistanceIndex);
}
// Find the point with the maximum distance from line between start and end.
// Rearranging this formula to avoid recomputing constants:
// https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
std::pair<double, std::size_t> findMostDistantPointFromLine(const std::vector<Point2D> &points,
std::size_t startIndex,
std::size_t endIndex)
{
assert(startIndex < endIndex && "Start index must be smaller than end index");
assert(endIndex < points.size() && "End index is larger than the number of points");
assert(points.size() >= 2 && "At least two points needed");
Point2D lineDiff = points[endIndex] - points[startIndex];
double lineLengthSquared = abs2(lineDiff);
if (lineLengthSquared == 0)
{
return findMostDistantPoint(points, startIndex, endIndex);
}
double offset = points[startIndex].y * lineDiff.x - points[startIndex].x * lineDiff.y;
double maxDistanceSquared = 0.0;
std::size_t maxDistanceIndex = startIndex;
for (std::size_t i = startIndex + 1; i != endIndex; ++i)
{
double unscaledDistance = offset - points[i].y * lineDiff.x + points[i].x * lineDiff.y;
double unscaledDistanceSquared = unscaledDistance * unscaledDistance;
if (unscaledDistanceSquared > maxDistanceSquared)
{
maxDistanceIndex = i;
maxDistanceSquared = unscaledDistanceSquared;
}
}
maxDistanceSquared /= lineLengthSquared;
// Constructor is faster than initialization
return std::make_pair(maxDistanceSquared, maxDistanceIndex);
}
void RamerDouglasPeucker(const std::vector<Point2D> &points, std::size_t startIndex,
std::size_t endIndex, double epsilonSquared,
std::vector<std::size_t> &indicesToKeep)
{
assert(startIndex < endIndex && "Start index must be smaller than end index");
assert(endIndex < points.size() && "End index is larger than the number of points");
// The inequalities 0 <= startIndex < endIndex < points.size() imply that points.size() >= 2
assert(points.size() >= 2 && "At least two points needed");
assert(epsilonSquared >= 0 && "epsilonSquared must be non-negative");
assert(indicesToKeep.size() >= 1 && "indicesToKeep should be non-empty");
assert(indicesToKeep[0] == 0 && "indicesToKeep should be initialized with a 0");
auto [maxDistanceSquared, maxDistanceIndex] =
findMostDistantPointFromLine(points, startIndex, endIndex);
if (maxDistanceSquared > epsilonSquared)
{
RamerDouglasPeucker(points, startIndex, maxDistanceIndex, epsilonSquared, indicesToKeep);
RamerDouglasPeucker(points, maxDistanceIndex, endIndex, epsilonSquared, indicesToKeep);
}
else
{
// startIndex is included from the previous run because we execute sequentially with the
// lower parts of the list first
indicesToKeep.push_back(endIndex);
}
}
} // end namespace rdp