Fast map matching  0.1.0
geom_algorithm.cpp
1 #include "algorithm/geom_algorithm.hpp"
2 #include "util/debug.hpp"
3 
4 #include <cmath>
5 #include <cstdlib>
6 
7 std::vector<double> FMM::ALGORITHM::cal_eu_dist(
8  const FMM::CORE::LineString &trajectory) {
9  int N = trajectory.get_num_points();
10  std::vector<double> lengths(N - 1);
11  double x0 = trajectory.get_x(0);
12  double y0 = trajectory.get_y(0);
13  for (int i = 1; i < N; ++i) {
14  double x1 = trajectory.get_x(i);
15  double y1 = trajectory.get_y(i);
16  double dx = x1 - x0;
17  double dy = y1 - y0;
18  lengths[i - 1] = sqrt(dx * dx + dy * dy);
19  x0 = x1;
20  y0 = y1;
21  }
22  return lengths;
23 }
24 
25 void FMM::ALGORITHM::append_segs_to_line(
26  FMM::CORE::LineString *line, const FMM::CORE::LineString &segs,
27  int offset) {
28  int Npoints = segs.get_num_points();
29  for (int i = 0; i < Npoints; ++i) {
30  if (i >= offset) {
31  line->add_point(segs.get_x(i), segs.get_y(i));
32  }
33  }
34 }
35 
36 FMM::CORE::LineString FMM::ALGORITHM::reverse_geometry(
37  const FMM::CORE::LineString &rhs) {
38  FMM::CORE::LineString line;
39  int Npoints = rhs.get_num_points();
40  for (int i = Npoints - 1; i >= 0; --i) {
41  line.add_point(rhs.get_x(i), rhs.get_y(i));
42  }
43  return line;
44 }
45 
46 std::vector<FMM::CORE::LineString> FMM::ALGORITHM::split_line(
47  const FMM::CORE::LineString &line, double delta) {
48  // SPDLOG_INFO("FMM::CORE::LineString {}", line.exportToWkt());
49  std::vector<FMM::CORE::LineString> segments;
50  if (line.get_length() <= delta) {
51  segments.push_back(line);
52  return segments;
53  }
54  int Npoints = line.get_num_points();
55  if (Npoints < 2) {
56  return segments;
57  }
58  double length_visited = 0;
59  FMM::CORE::LineString interpolated_line;
60  int i = 1; // Point in line
61  double cx = line.get_x(0);
62  double cy = line.get_y(0);
63  interpolated_line.add_point(cx, cy);
64  while (i < Npoints) {
65  double nx = line.get_x(i);
66  double ny = line.get_y(i);
67  double temp = std::sqrt((nx - cx) * (nx - cx) + (ny - cy) * (ny - cy));
68  if (length_visited <= delta
69  && length_visited + temp > delta) {
70  // Split the edge
71  double ratio = (delta - length_visited) / temp;
72  // SPDLOG_INFO("Ratio is {}",ratio);
73  cx = ratio * (nx - cx) + cx;
74  cy = ratio * (ny - cy) + cy;
75  interpolated_line.add_point(cx, cy);
76  segments.push_back(interpolated_line);
77  interpolated_line.clear();
78  interpolated_line.add_point(cx, cy);
79  length_visited = 0;
80  } else {
81  cx = nx;
82  cy = ny;
83  interpolated_line.add_point(cx, cy);
84  length_visited += temp;
85  ++i;
86  }
87  }
88  segments.push_back(interpolated_line);
89  // for (int i = 0;i<segments.size();++i){
90  // SPDLOG_INFO(" Segments {} {}", i, segments[i].exportToWkt());
91  // }
92  return segments;
93 }
94 
95 FMM::CORE::LineString FMM::ALGORITHM::interpolate_line_distances(
96  const FMM::CORE::LineString &line, const std::vector<double> &distances) {
97  FMM::CORE::LineString interpolated_line;
98  int Npoints = line.get_num_points();
99  int distance_count = distances.size();
100  double length_visited = 0;
101  int i = 0; // Point in line
102  int j = 0; // Element visited in distances vector
103  while (i < Npoints - 1 && j < distance_count) {
104  double x1 = line.get_x(i);
105  double y1 = line.get_y(i);
106  double x2 = line.get_x(i + 1);
107  double y2 = line.get_y(i + 1);
108  double temp = std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
109  while (j < distance_count && length_visited <= distances[j]
110  && length_visited + temp > distances[j]) {
111  double ratio = (distances[j] - length_visited) / temp;
112  // SPDLOG_INFO("Ratio is {}",ratio);
113  interpolated_line.add_point(ratio * (x2 - x1) + x1,
114  ratio * (y2 - y1) + y1);
115  ++j;
116  }
117  length_visited += temp;
118  ++i;
119  }
120  if (length_visited < distances.back()) {
121  interpolated_line.add_point(line.get_x(Npoints - 1),
122  line.get_y(Npoints - 1));
123  }
124  return interpolated_line;
125 }
126 
127 FMM::CORE::LineString FMM::ALGORITHM::interpolate_line_distance(
128  const FMM::CORE::LineString &line, double distance) {
129  double length = line.get_length();
130  int k = (int) ceil(length / distance);
131  std::vector<double> distances;
132  for (int i = 0; i < k + 1; ++i) {
133  distances.push_back(distance * i);
134  }
135  return interpolate_line_distances(line, distances);
136 }
137 
138 FMM::CORE::LineString FMM::ALGORITHM::interpolate_line_kpoints(
139  const FMM::CORE::LineString &line, int k) {
140  double length = line.get_length();
141  std::vector<double> distances;
142  for (int i = 0; i < k; ++i) {
143  distances.push_back((double) rand() / RAND_MAX * length);
144  }
145  std::sort(distances.begin(), distances.end());
146  // SPDLOG_DEBUG("Length is {}",length);
147  // SPDLOG_DEBUG("Sorted length is {}",UTIL::vec2string<double>(distances));
148  return interpolate_line_distances(line, distances);
149 }
150 
151 void FMM::ALGORITHM::boundingbox_geometry(
152  const FMM::CORE::LineString &linestring, double *x1, double *y1,
153  double *x2, double *y2) {
154  int Npoints = linestring.get_num_points();
155  *x1 = DBL_MAX;
156  *y1 = DBL_MAX;
157  *x2 = DBL_MIN;
158  *y2 = DBL_MIN;
159  double x, y;
160  for (int i = 0; i < Npoints; ++i) {
161  x = linestring.get_x(i);
162  y = linestring.get_y(i);
163  if (x < *x1) *x1 = x;
164  if (y < *y1) *y1 = y;
165  if (x > *x2) *x2 = x;
166  if (y > *y2) *y2 = y;
167  }
168 }
169 
170 std::vector<double> FMM::ALGORITHM::calc_length_to_end_vec(
171  const FMM::CORE::LineString &geom) {
172  int N = geom.get_num_points();
173  if (N < 2) return std::vector<double>();
174  std::vector<double> result(N - 1);
175  double x0 = geom.get_x(0);
176  double y0 = geom.get_y(0);
177  for (int i = 1; i < N; ++i) {
178  double x1 = geom.get_x(i);
179  double y1 = geom.get_y(i);
180  double dx = x1 - x0;
181  double dy = y1 - y0;
182  result[i - 1] = sqrt(dx * dx + dy * dy);
183  x0 = x1;
184  y0 = y1;
185  }
186  double temp = 0;
187  for (int i = N - 2; i >= 0; --i) {
188  result[i] = temp + result[i];
189  temp = result[i];
190  }
191  return result;
192 }
193 
194 void FMM::ALGORITHM::closest_point_on_segment(
195  double x, double y, double x1, double y1, double x2, double y2,
196  double *dist, double *offset) {
197  double L2 = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
198  if (L2 == 0.0) {
199  *dist = std::sqrt((x - x1) * (x - x1) + (y - y1) * (y - y1));
200  *offset = 0.0;
201  return;
202  }
203  double x1_x = x - x1;
204  double y1_y = y - y1;
205  double x1_x2 = x2 - x1;
206  double y1_y2 = y2 - y1;
207  double ratio = (x1_x * x1_x2 + y1_y * y1_y2) / L2;
208  ratio = (ratio > 1) ? 1 : ratio;
209  ratio = (ratio < 0) ? 0 : ratio;
210  double prj_x = x1 + ratio * (x1_x2);
211  double prj_y = y1 + ratio * (y1_y2);
212  *offset = std::sqrt((prj_x - x1) * (prj_x - x1) +
213  (prj_y - y1) * (prj_y - y1));
214  *dist = std::sqrt((prj_x - x) * (prj_x - x) + (prj_y - y) * (prj_y - y));
215 } // closest_point_on_segment
216 
217 void FMM::ALGORITHM::closest_point_on_segment(
218  double x, double y, double x1, double y1,
219  double x2, double y2, double *dist,
220  double *offset, double *closest_x,
221  double *closest_y) {
222  double L2 = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
223  if (L2 == 0.0) {
224  *dist = std::sqrt((x - x1) * (x - x1) + (y - y1) * (y - y1));
225  *offset = 0.0;
226  *closest_x = x1;
227  *closest_y = y1;
228  return;
229  }
230  double x1_x = x - x1;
231  double y1_y = y - y1;
232  double x1_x2 = x2 - x1;
233  double y1_y2 = y2 - y1;
234  double ratio = (x1_x * x1_x2 + y1_y * y1_y2) / L2;
235  ratio = (ratio > 1) ? 1 : ratio;
236  ratio = (ratio < 0) ? 0 : ratio;
237  double prj_x = x1 + ratio * (x1_x2);
238  double prj_y = y1 + ratio * (y1_y2);
239  *offset = std::sqrt((prj_x - x1) * (prj_x - x1) +
240  (prj_y - y1) * (prj_y - y1));
241  *dist = std::sqrt((prj_x - x) * (prj_x - x) + (prj_y - y) * (prj_y - y));
242  *closest_x = prj_x;
243  *closest_y = prj_y;
244 } // closest_point_on_segment
245 
246 void FMM::ALGORITHM::linear_referencing(
247  double px, double py, const FMM::CORE::LineString &linestring,
248  double *result_dist, double *result_offset) {
249  int Npoints = linestring.get_num_points();
250  double min_dist = DBL_MAX;
251  double final_offset = DBL_MAX;
252  double length_parsed = 0;
253  int i = 0;
254  // Iterating to check p(i) == p(i+2)
255  // int seg_idx=0;
256  while (i < Npoints - 1) {
257  double x1 = linestring.get_x(i);
258  double y1 = linestring.get_y(i);
259  double x2 = linestring.get_x(i + 1);
260  double y2 = linestring.get_y(i + 1);
261  double temp_min_dist;
262  double temp_min_offset;
263  closest_point_on_segment(px, py, x1, y1, x2, y2,
264  &temp_min_dist, &temp_min_offset);
265  if (temp_min_dist < min_dist) {
266  min_dist = temp_min_dist;
267  final_offset = length_parsed + temp_min_offset;
268  }
269  length_parsed += std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
270  ++i;
271  }
272  *result_dist = min_dist;
273  *result_offset = final_offset;
274 } // linear_referencing
275 
276 void FMM::ALGORITHM::linear_referencing(
277  double px, double py, const FMM::CORE::LineString &linestring,
278  double *result_dist, double *result_offset,
279  double *proj_x, double *proj_y) {
280  int Npoints = linestring.get_num_points();
281  double min_dist = DBL_MAX;
282  double temp_x = 0, temp_y = 0;
283  double final_offset = DBL_MAX;
284  double length_parsed = 0;
285  int i = 0;
286  // Iterating to check p(i) == p(i+2)
287  // int seg_idx=0;
288  while (i < Npoints - 1) {
289  double x1 = linestring.get_x(i);
290  double y1 = linestring.get_y(i);
291  double x2 = linestring.get_x(i + 1);
292  double y2 = linestring.get_y(i + 1);
293  double temp_min_dist;
294  double temp_min_offset;
295  closest_point_on_segment(px, py, x1, y1, x2, y2,
296  &temp_min_dist, &temp_min_offset,
297  &temp_x, &temp_y);
298  if (temp_min_dist < min_dist) {
299  min_dist = temp_min_dist;
300  final_offset = length_parsed + temp_min_offset;
301  *proj_x = temp_x;
302  *proj_y = temp_y;
303  }
304  length_parsed += std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
305  ++i;
306  }
307  *result_dist = min_dist;
308  *result_offset = final_offset;
309 } // linear_referencing
310 
311 void FMM::ALGORITHM::locate_point_by_offset(
312  const FMM::CORE::LineString &linestring, double offset,
313  double *x, double *y) {
314  int Npoints = linestring.get_num_points();
315  if (offset <= 0.0) {
316  *x = linestring.get_x(0);
317  *y = linestring.get_y(0);
318  return;
319  }
320  double L_processed = 0; // length parsed
321  int i = 0;
322  double px = 0;
323  double py = 0;
324  bool found = false;
325  // Find the idx of the point to be exported close to p
326  while (i < Npoints - 1) {
327  double x1 = linestring.get_x(i);
328  double y1 = linestring.get_y(i);
329  double x2 = linestring.get_x(i + 1);
330  double y2 = linestring.get_y(i + 1);
331  double deltaL = std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
332  double ratio = (offset - L_processed) / deltaL;
333  if (offset >= L_processed && offset <= L_processed + deltaL) {
334  px = x1 + ratio * (x2 - x1);
335  py = y1 + ratio * (y2 - y1);
336  found = true;
337  break;
338  }
339  ++i;
340  L_processed += deltaL;
341  }
342  if (found) {
343  *x = px;
344  *y = py;
345  } else {
346  *x = linestring.get_x(Npoints - 1);
347  *y = linestring.get_y(Npoints - 1);
348  }
349 } // calculate_offset_point
350 
351 FMM::CORE::LineString FMM::ALGORITHM::cutoffseg_unique(
352  const FMM::CORE::LineString &linestring, double offset1, double offset2) {
353  FMM::CORE::LineString cutoffline;
354  SPDLOG_TRACE("Offset1 {} Offset2 {}", offset1, offset2);
355  int Npoints = linestring.get_num_points();
356  if (Npoints == 2) {
357  // A single segment
358  double x1 = linestring.get_x(0);
359  double y1 = linestring.get_y(0);
360  double x2 = linestring.get_x(1);
361  double y2 = linestring.get_y(1);
362  double L = std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
363  double ratio1 = offset1 / L;
364  double new_x1 = x1 + ratio1 * (x2 - x1);
365  double new_y1 = y1 + ratio1 * (y2 - y1);
366  double ratio2 = offset2 / L;
367  double new_x2 = x1 + ratio2 * (x2 - x1);
368  double new_y2 = y1 + ratio2 * (y2 - y1);
369  cutoffline.add_point(new_x1, new_y1);
370  cutoffline.add_point(new_x2, new_y2);
371  } else {
372  // Multiple segments
373  double l1 = 0;
374  double l2 = 0;
375  int i = 0;
376  while (i < Npoints - 1) {
377  double x1 = linestring.get_x(i);
378  double y1 = linestring.get_y(i);
379  double x2 = linestring.get_x(i + 1);
380  double y2 = linestring.get_y(i + 1);
381  double deltaL = std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
382  l2 = l1 + deltaL;
383  // Insert p1
384  SPDLOG_TRACE(" L1 {} L2 {} ", l1, l2);
385  if (l1 >= offset1 && l1 <= offset2) {
386  cutoffline.add_point(x1, y1);
387  SPDLOG_TRACE(" add p1 {} {}", x1, y1);
388  }
389 
390  // Insert p between p1 and p2
391  if (offset1 > l1 && offset1 < l2) {
392  double ratio1 = (offset1 - l1) / deltaL;
393  double px = x1 + ratio1 * (x2 - x1);
394  double py = y1 + ratio1 * (y2 - y1);
395  cutoffline.add_point(px, py);
396  SPDLOG_TRACE(" add p {} {} between p1 p2", px, py);
397  }
398 
399  if (offset2 > l1 && offset2 < l2) {
400  double ratio2 = (offset2 - l1) / deltaL;
401  double px = x1 + ratio2 * (x2 - x1);
402  double py = y1 + ratio2 * (y2 - y1);
403  cutoffline.add_point(px, py);
404  SPDLOG_TRACE(" add p {} {} between p1 p2", px, py);
405  }
406 
407  // last point
408  if (i == Npoints - 2 && offset2 >= l2) {
409  cutoffline.add_point(x2, y2);
410  SPDLOG_TRACE(" add p2 {} {} for last point", x2, y2);
411  }
412 
413  l1 = l2;
414  ++i;
415  }
416  }
417  return cutoffline;
418 } //cutoffseg_twoparameters
419 
420 FMM::CORE::LineString FMM::ALGORITHM::cutoffseg(
421  const FMM::CORE::LineString &linestring, double offset, int mode) {
422  double L = linestring.get_length();
423  double offset1, offset2;
424  if (mode == 0) {
425  offset1 = offset;
426  offset2 = L;
427  } else {
428  offset1 = 0;
429  offset2 = offset;
430  }
431  return FMM::ALGORITHM::cutoffseg_unique(linestring, offset1, offset2);
432 }
FMM::ALGORITHM::reverse_geometry
FMM::CORE::LineString reverse_geometry(const FMM::CORE::LineString &rhs)
Create a new linestring as the reverse of an input linestring.
Definition: geom_algorithm.cpp:36
FMM::CORE::LineString::add_point
void add_point(double x, double y)
Add a point to the end of the current line.
Definition: geometry.hpp:79
FMM::ALGORITHM::split_line
std::vector< FMM::CORE::LineString > split_line(const FMM::CORE::LineString &line, double delta)
Interpolate a linestring at a fixed distance threshold and return the result in as a vector of linest...
Definition: geom_algorithm.cpp:46
FMM::ALGORITHM::interpolate_line_distance
FMM::CORE::LineString interpolate_line_distance(const FMM::CORE::LineString &line, double distance)
Interpolate a linestring according to a distance step value.
Definition: geom_algorithm.cpp:127
FMM::ALGORITHM::interpolate_line_kpoints
FMM::CORE::LineString interpolate_line_kpoints(const FMM::CORE::LineString &line, int k)
Interpolate k points in a linestring with equal distance.
Definition: geom_algorithm.cpp:138
FMM::ALGORITHM::interpolate_line_distances
FMM::CORE::LineString interpolate_line_distances(const FMM::CORE::LineString &line, const std::vector< double > &distances)
Interpolate a linestring according to a vector of distances to the start point of a line.
Definition: geom_algorithm.cpp:95
FMM::ALGORITHM::locate_point_by_offset
void locate_point_by_offset(const FMM::CORE::LineString &linestring, double offset, double *x, double *y)
Locate the point on a linestring according to the input of offset The two pointer's target value will...
Definition: geom_algorithm.cpp:311
FMM::CORE::LineString::clear
void clear()
Remove all points in the current line.
Definition: geometry.hpp:128
FMM::ALGORITHM::boundingbox_geometry
void boundingbox_geometry(const FMM::CORE::LineString &linestring, double *x1, double *y1, double *x2, double *y2)
Compute the boundary of an FMM::CORE::LineString and returns the result in the passed x1,...
Definition: geom_algorithm.cpp:151
FMM::CORE::LineString
Linestring geometry class.
Definition: geometry.hpp:34
FMM::ALGORITHM::cutoffseg_unique
FMM::CORE::LineString cutoffseg_unique(const FMM::CORE::LineString &linestring, double offset1, double offset2)
Cut a linestring at two offset values.
Definition: geom_algorithm.cpp:351
FMM::CORE::LineString::get_length
double get_length() const
Get the length of the line.
Definition: geometry.hpp:135
FMM::ALGORITHM::linear_referencing
void linear_referencing(double px, double py, const FMM::CORE::LineString &linestring, double *result_dist, double *result_offset)
A linear referencing function.
Definition: geom_algorithm.cpp:246
FMM::ALGORITHM::closest_point_on_segment
void closest_point_on_segment(double x, double y, double x1, double y1, double x2, double y2, double *dist, double *offset)
Calculate the closest point p' on a segment p1 (x1,y1) p2 (x2,y2) to a specific point p (x,...
Definition: geom_algorithm.cpp:194
FMM::ALGORITHM::append_segs_to_line
void append_segs_to_line(FMM::CORE::LineString *line, const FMM::CORE::LineString &segs, int offset=0)
Concatenate a linestring segs to a linestring line, used in the function network.complete_path_to_geo...
Definition: geom_algorithm.cpp:25
FMM::ALGORITHM::cutoffseg
FMM::CORE::LineString cutoffseg(const FMM::CORE::LineString &linestring, double offset, int mode)
Added by Diao 18.01.17 modified by Can 18.01.19 modified by Can 18.03.14.
Definition: geom_algorithm.cpp:420
FMM::ALGORITHM::cal_eu_dist
std::vector< double > cal_eu_dist(const FMM::CORE::LineString &trajectory)
Calculate segment length of a trajectory.
Definition: geom_algorithm.cpp:7
FMM::CORE::LineString::get_num_points
int get_num_points() const
Get the number of points in a line.
Definition: geometry.hpp:115
FMM::CORE::LineString::get_y
double get_y(int i) const
Get the y coordinate of i-th point in the line.
Definition: geometry.hpp:55
FMM::CORE::LineString::get_x
double get_x(int i) const
Get the x coordinate of i-th point in the line.
Definition: geometry.hpp:46
FMM::ALGORITHM::calc_length_to_end_vec
std::vector< double > calc_length_to_end_vec(const FMM::CORE::LineString &geom)
Calculate the distance from each point in a linestring to the end point of a linestring.
Definition: geom_algorithm.cpp:170
Generated by   doxygen 1.8.17