Skip to content

Commit dc8c97e

Browse files
dnesbitt61dnesbitt61
committed
Merge pull request #9 from mapzen/dwn
Add polyline and distance from point to polyline.
2 parents c9d3c5d + ce37006 commit dc8c97e

File tree

5 files changed

+392
-6
lines changed

5 files changed

+392
-6
lines changed

‎geosupport/geosupport.h

+181
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,7 @@ inline float FastInvSqrt(float x) {
8484
#include "ellipse.h"
8585
#include "clipper2.h"
8686
#include "tiles.h"
87+
#include "polyline2.h"
8788

8889
// Define the operators in Point2 that allow a vector to be added
8990
// to and subtracted from a point. Descriptions are in the Point2
@@ -105,4 +106,184 @@ inline Vector2 operator *(float s, const Vector2 &v){
105106
return Vector2(v.x() * s, v.y() * s);
106107
}
107108

109+
/**
110+
* TODO - move to .cc file!
111+
* Finds the closest point to the supplied polyline as well as the distance
112+
* squared to that point.
113+
* @param pts Polyline points
114+
* @param closest (OUT) Closest point along the polyline
115+
* @param idx (OUT) Index of the segment of the polyline which contains
116+
* the closest point.
117+
* @return Returns the distance squared of the closest point.
118+
*/
119+
inline float Point2::ClosestPoint(const std::vector<Point2>& pts,
120+
Point2& closest, int& idx) const {
121+
// TODO - make sure at least 2 points are in list
122+
123+
// Iterate through the pts
124+
unsigned int count = pts.size();
125+
bool beyond_end = true; // Need to test past the end point?
126+
Vector2 v1; // Segment vector (v1)
127+
Vector2 v2; // Vector from origin to target (v2)
128+
Point2 projpt; // Projected point along v1
129+
float dot; // Dot product of v1 and v2
130+
float comp; // Component of v2 along v1
131+
float dist; // Squared distance from target to closest point on line
132+
float mindist = 2147483647.0f;
133+
const Point2* p0 = &pts[0];
134+
const Point2* p1 = &pts[1];
135+
const Point2* end = &pts[count - 1];
136+
for (int index = 0; p1 < end; index++, p0++, p1++) {
137+
// Construct vector v1 - represents the segment. Skip 0 length segments
138+
v1.Set(*p0, *p1);
139+
if (v1.x() == 0.0f && v1.y() == 0.0f)
140+
continue;
141+
142+
// Vector v2 from the segment origin to the target point
143+
v2.Set(*p0, *this);
144+
145+
// Find the dot product of v1 and v2. If less than 0 the segment
146+
// origin is the closest point. Find the distance and continue
147+
// to the next segment.
148+
dot = v1.Dot(v2);
149+
if (dot <= 0.0f) {
150+
beyond_end = false;
151+
dist = DistanceSquared(*p0);
152+
if (dist < mindist) {
153+
mindist = dist;
154+
closest = *p0;
155+
idx = index;
156+
}
157+
continue;
158+
}
159+
160+
// Closest point is either beyond the end of the segment or at a point
161+
// along the segment. Find the component of v2 along v1
162+
comp = dot / v1.Dot(v1);
163+
164+
// If component >= 1.0 the segment end is the closest point. A future
165+
// polyline segment will be closer. If last segment we need to check
166+
// distance to the endpoint. Set flag so this happens.
167+
if (comp >= 1.0f)
168+
beyond_end = true;
169+
else {
170+
// Closest point is along the segment. The closest point is found
171+
// by adding the projection of v2 onto v1 to the origin point.
172+
// The squared distance from this point to the target is then found.
173+
beyond_end = false;
174+
projpt = *p0 + v1 * comp;
175+
dist = DistanceSquared(projpt);
176+
if (dist < mindist) {
177+
mindist = dist;
178+
closest = projpt;
179+
idx = index;
180+
}
181+
}
182+
}
183+
184+
// Test the end point if flag is set - it may be the closest point
185+
if (beyond_end) {
186+
dist = DistanceSquared(pts[count-1]);
187+
if (dist < mindist) {
188+
mindist = dist;
189+
closest = *end;
190+
idx = (int)(count-2);
191+
}
192+
}
193+
return mindist;
194+
}
195+
196+
/**
197+
* TODO - move to .cc file!
198+
* Finds the closest point to the supplied polyline as well as the distance
199+
* squared to that point. Uses DistanceApproximator to set the point as the
200+
* test point - then uses DistanceSquared method to get more accurate
201+
* distances than just Euclidean distance in lat,lng space
202+
* @param pts Polyline points
203+
* @param closest (OUT) Closest point along the polyline
204+
* @param idx (OUT) Index of the segment of the polyline which contains
205+
* the closest point.
206+
* @return Returns the distance squared of the closest point.
207+
*/
208+
inline float PointLL::ClosestPoint(const std::vector<PointLL>& pts,
209+
PointLL& closest, int& idx) const {
210+
// TODO - make sure at least 2 points are in list
211+
212+
DistanceApproximator approx;
213+
approx.SetTestPoint(*this);
214+
215+
// Iterate through the pts
216+
unsigned int count = pts.size();
217+
bool beyond_end = true; // Need to test past the end point?
218+
Vector2 v1; // Segment vector (v1)
219+
Vector2 v2; // Vector from origin to target (v2)
220+
PointLL projpt; // Projected point along v1
221+
float dot; // Dot product of v1 and v2
222+
float comp; // Component of v2 along v1
223+
float dist; // Squared distance from target to closest point on line
224+
float mindist = 2147483647.0f;
225+
const PointLL* p0 = &pts[0];
226+
const PointLL* p1 = &pts[1];
227+
const PointLL* end = &pts[count - 1];
228+
for (int index = 0; p1 < end; index++, p0++, p1++) {
229+
// Construct vector v1 - represents the segment. Skip 0 length segments
230+
v1.Set(*p0, *p1);
231+
if (v1.x() == 0.0f && v1.y() == 0.0f)
232+
continue;
233+
234+
// Vector v2 from the segment origin to the target point
235+
v2.Set(*p0, *this);
236+
237+
// Find the dot product of v1 and v2. If less than 0 the segment
238+
// origin is the closest point. Find the distance and continue
239+
// to the next segment.
240+
dot = v1.Dot(v2);
241+
if (dot <= 0.0f) {
242+
beyond_end = false;
243+
dist = approx.DistanceSquared(*p0);
244+
if (dist < mindist) {
245+
mindist = dist;
246+
closest = *p0;
247+
idx = index;
248+
}
249+
continue;
250+
}
251+
252+
// Closest point is either beyond the end of the segment or at a point
253+
// along the segment. Find the component of v2 along v1
254+
comp = dot / v1.Dot(v1);
255+
256+
// If component >= 1.0 the segment end is the closest point. A future
257+
// polyline segment will be closer. If last segment we need to check
258+
// distance to the endpoint. Set flag so this happens.
259+
if (comp >= 1.0f)
260+
beyond_end = true;
261+
else {
262+
// Closest point is along the segment. The closest point is found
263+
// by adding the projection of v2 onto v1 to the origin point.
264+
// The squared distance from this point to the target is then found.
265+
// TODO - why not: *p0 + v1 * comp;
266+
beyond_end = false;
267+
projpt.Set(p0->lat() + v1.y() * comp, p0->lng() + v1.x() * comp);
268+
dist = approx.DistanceSquared(projpt);
269+
if (dist < mindist) {
270+
mindist = dist;
271+
closest = projpt;
272+
idx = index;
273+
}
274+
}
275+
}
276+
277+
// Test the end point if flag is set - it may be the closest point
278+
if (beyond_end) {
279+
dist = approx.DistanceSquared(pts[count-1]);
280+
if (dist < mindist) {
281+
mindist = dist;
282+
closest = *end;
283+
idx = (int)(count-2);
284+
}
285+
}
286+
return mindist;
287+
}
288+
108289
#endif

‎geosupport/point2.h

+23-1
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22
#define __point2_h__
33

44
#include <math.h>
5+
#include <vector>
56

67
// Forward references
78
class Vector2;
@@ -107,13 +108,22 @@ class Point2 {
107108
return (x_ != p.x() || y_ != p.y());
108109
}
109110

111+
/**
112+
* Get the distance squared from this point to point p.
113+
* @param p Other point.
114+
* @return Returns the distance squared between this point and p.
115+
*/
116+
virtual float DistanceSquared(const Point2& p) const {
117+
return sqr(x_ - p.x()) + sqr(y_ - p.y());
118+
}
119+
110120
/**
111121
* Get the distance from this point to point p.
112122
* @param p Other point.
113123
* @return Returns the distance between this point and p.
114124
*/
115125
virtual float Distance(const Point2& p) const {
116-
return sqrtf(sqr(x_ - p.x()) + sqr(y_ - p.y()) );
126+
return sqrtf(sqr(x_ - p.x()) + sqr(y_ - p.y()));
117127
}
118128

119129
/**
@@ -162,6 +172,18 @@ class Point2 {
162172
*/
163173
Vector2 operator - (const Point2& p) const;
164174

175+
/**
176+
* Finds the closest point to the supplied polyline as well as the distance
177+
* squared to that point.
178+
* @param pts List of points on the polyline.
179+
* @param closest (OUT) Closest point along the polyline
180+
* @param idx (OUT) Index of the segment of the polyline which contains
181+
* the closest point.
182+
* @return Returns the distance squared of the closest point.
183+
*/
184+
float ClosestPoint(const std::vector<Point2>& pts, Point2& closest,
185+
int& idx) const;
186+
165187
protected:
166188
float x_;
167189
float y_;

‎geosupport/pointll.h

+14
Original file line numberDiff line numberDiff line change
@@ -120,6 +120,20 @@ class PointLL : public Point2 {
120120
float bearing = radians_to_degrees(atan2f(y, x));
121121
return (bearing < 0.0f) ? bearing + 360.0f : bearing;
122122
}
123+
124+
// Method defined in geosupport.h
125+
126+
/**
127+
* Finds the closest point to the supplied polyline as well as the distance
128+
* squared to that point.
129+
* @param pts List of points on the polyline.
130+
* @param closest (OUT) Closest point along the polyline
131+
* @param idx (OUT) Index of the segment of the polyline which contains
132+
* the closest point.
133+
* @return Returns the distance squared of the closest point.
134+
*/
135+
float ClosestPoint(const std::vector<PointLL>& pts, PointLL& closest,
136+
int& idx) const;
123137
};
124138

125139
#endif

0 commit comments

Comments
 (0)