You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

smplrout.cc 12KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425
  1. /*
  2. Route / track simplification filter
  3. Copyright (C) 2002-2014 Robert Lipe, robertlipe+source@gpsbabel.org
  4. This program is free software; you can redistribute it and/or modify
  5. it under the terms of the GNU General Public License as published by
  6. the Free Software Foundation; either version 2 of the License, or
  7. (at your option) any later version.
  8. This program is distributed in the hope that it will be useful,
  9. but WITHOUT ANY WARRANTY; without even the implied warranty of
  10. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  11. GNU General Public License for more details.
  12. You should have received a copy of the GNU General Public License
  13. along with this program; if not, write to the Free Software
  14. Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
  15. */
  16. /* The following comments are from an email I wrote to Paul Fox in November
  17. * 2005 in an attempt to explain how the cross track error minimization method
  18. * works (RLP 2005):
  19. *
  20. * It's pretty simple, really: for each triplet of vertices A-B-C, we compute
  21. * how much cross-track error we'd introduce by going straight from A to C
  22. * (the maximum cross-track error for that segment is the height of the
  23. * triangle ABC, measured between vertex B and edge AC.) If we need to remove
  24. * 40 points, we just sort the points by that metric and remove the 40
  25. * smallest ones.
  26. *
  27. * It's actually a little more complicated than that, because removing a
  28. * point changes the result for its two nearest neighbors. When we remove
  29. * one, we recompute the neighbors and then sort them back into the list
  30. * at their new locations.
  31. *
  32. * As you can see, this hasn't been shown to be an optimal algorithm. After
  33. * all, removing one high-xte point might create two very low-xte neighbors
  34. * that more than make up for the high xte of the original point. I believe
  35. * the optimal algorithm would be NP-complete, but I haven't proven it. This
  36. * is really more of a heuristic than anything, but it seems to work well for
  37. * the routes I've fed it.
  38. *
  39. * Not in that email was an explanation of how the pathlength-based calculation
  40. * works: instead of computing the height of the triangle, we just compute
  41. * the difference in pathlength from taking the direct route. This case,
  42. * too, is only a heuristic, as it's possible that a different combination or
  43. * order of point removals could lead to a smaller number of points with less
  44. * reduction in path length. In the case of pathlength, error is cumulative.
  45. */
  46. /*
  47. History:
  48. 2008/08/20: added "relative" option, (Carsten Allefeld, carsten.allefeld@googlemail.com)
  49. */
  50. #include "defs.h"
  51. #include "filterdefs.h"
  52. #include "grtcirc.h"
  53. #include <stdlib.h>
  54. #define MYNAME "simplify"
  55. #define sqr(a) ((a)*(a))
  56. static int count = 0;
  57. static double totalerror = 0;
  58. static double error = 0;
  59. static char* countopt;
  60. static char* erroropt;
  61. static char* xteopt;
  62. static char* lenopt;
  63. static char* relopt;
  64. void (*waypt_del_fnp)(route_head* rte, Waypoint* wpt);
  65. static
  66. arglist_t routesimple_args[] = {
  67. {
  68. "count", &countopt, "Maximum number of points in route",
  69. NULL, ARGTYPE_INT | ARGTYPE_BEGIN_REQ | ARGTYPE_BEGIN_EXCL, "1", NULL
  70. },
  71. {
  72. "error", &erroropt, "Maximum error", NULL,
  73. ARGTYPE_STRING | ARGTYPE_END_REQ | ARGTYPE_END_EXCL, "0", NULL
  74. },
  75. {
  76. "crosstrack", &xteopt, "Use cross-track error (default)", NULL,
  77. ARGTYPE_BOOL | ARGTYPE_BEGIN_EXCL, ARG_NOMINMAX
  78. },
  79. {
  80. "length", &lenopt, "Use arclength error", NULL,
  81. ARGTYPE_BOOL, ARG_NOMINMAX
  82. },
  83. {
  84. "relative", &relopt, "Use relative error", NULL,
  85. ARGTYPE_BOOL | ARGTYPE_END_EXCL, ARG_NOMINMAX
  86. },
  87. ARG_TERMINATOR
  88. };
  89. struct xte_intermed;
  90. struct xte {
  91. double distance;
  92. int ordinal;
  93. struct xte_intermed* intermed;
  94. };
  95. struct xte_intermed {
  96. struct xte* xte_rec;
  97. struct xte_intermed* next;
  98. struct xte_intermed* prev;
  99. const Waypoint* wpt;
  100. };
  101. void
  102. free_xte(struct xte* xte_rec)
  103. {
  104. xfree(xte_rec->intermed);
  105. }
  106. #define HUGEVAL 2000000000
  107. static struct xte_intermed* tmpprev = NULL;
  108. static int xte_count = 0;
  109. static const route_head* cur_rte = NULL;
  110. static struct xte* xte_recs = NULL;
  111. void
  112. routesimple_waypt_pr(const Waypoint* wpt)
  113. {
  114. if (!cur_rte) {
  115. return;
  116. }
  117. xte_recs[xte_count].ordinal=xte_count;
  118. xte_recs[xte_count].intermed = (struct xte_intermed*) xmalloc(sizeof(struct xte_intermed));
  119. xte_recs[xte_count].intermed->wpt = wpt;
  120. xte_recs[xte_count].intermed->xte_rec = xte_recs+xte_count;
  121. xte_recs[xte_count].intermed->next = NULL;
  122. xte_recs[xte_count].intermed->prev = tmpprev;
  123. if (tmpprev) {
  124. tmpprev->next = xte_recs[xte_count].intermed;
  125. }
  126. tmpprev = xte_recs[xte_count].intermed;
  127. xte_count++;
  128. }
  129. void
  130. compute_xte(struct xte* xte_rec)
  131. {
  132. const Waypoint* wpt3 = xte_rec->intermed->wpt;
  133. const Waypoint* wpt1 = NULL;
  134. const Waypoint* wpt2 = NULL;
  135. double frac, reslat, reslon;
  136. /* if no previous, this is an endpoint and must be preserved. */
  137. if (!xte_rec->intermed->prev) {
  138. xte_rec->distance = HUGEVAL;
  139. return;
  140. }
  141. wpt1 = xte_rec->intermed->prev->wpt;
  142. /* if no next, this is an endpoint and must be preserved. */
  143. if (!xte_rec->intermed->next) {
  144. xte_rec->distance = HUGEVAL;
  145. return;
  146. }
  147. wpt2 = xte_rec->intermed->next->wpt;
  148. if (xteopt) {
  149. xte_rec->distance = radtomiles(linedist(
  150. wpt1->latitude, wpt1->longitude,
  151. wpt2->latitude, wpt2->longitude,
  152. wpt3->latitude, wpt3->longitude));
  153. } else if (lenopt) {
  154. xte_rec->distance = radtomiles(
  155. gcdist(wpt1->latitude, wpt1->longitude,
  156. wpt3->latitude, wpt3->longitude) +
  157. gcdist(wpt3->latitude, wpt3->longitude,
  158. wpt2->latitude, wpt2->longitude) -
  159. gcdist(wpt1->latitude, wpt1->longitude,
  160. wpt2->latitude, wpt2->longitude));
  161. } else if (relopt) {
  162. if (wpt3->hdop == 0) {
  163. fatal(MYNAME ": relative needs hdop information.\n");
  164. }
  165. // if timestamps exist, distance to interpolated point
  166. if (wpt1->GetCreationTime() != wpt2->GetCreationTime()) {
  167. frac = (double)(wpt3->GetCreationTime().toTime_t() - wpt1->GetCreationTime().toTime_t()) /
  168. (wpt2->GetCreationTime().toTime_t() - wpt1->GetCreationTime().toTime_t());
  169. linepart(wpt1->latitude, wpt1->longitude,
  170. wpt2->latitude, wpt2->longitude,
  171. frac, &reslat, &reslon);
  172. xte_rec->distance = radtometers(gcdist(
  173. wpt3->latitude, wpt3->longitude,
  174. reslat, reslon));
  175. } else { // else distance to connecting line
  176. xte_rec->distance = radtometers(linedist(
  177. wpt1->latitude, wpt1->longitude,
  178. wpt2->latitude, wpt2->longitude,
  179. wpt3->latitude, wpt3->longitude));
  180. }
  181. // error relative to horizontal precision
  182. xte_rec->distance /= (6 * wpt3->hdop);
  183. // (hdop->meters following to J. Person at <http://www.developerfusion.co.uk/show/4652/3/>)
  184. }
  185. }
  186. int
  187. compare_xte(const void* a, const void* b)
  188. {
  189. double distdiff = ((struct xte*)a)->distance -
  190. ((struct xte*)b)->distance;
  191. int priodiff = ((struct xte*)a)->intermed->wpt->route_priority -
  192. ((struct xte*)b)->intermed->wpt->route_priority;
  193. if (HUGEVAL == ((struct xte*)a)->distance) {
  194. return -1;
  195. }
  196. if (HUGEVAL == ((struct xte*)b)->distance) {
  197. return 1;
  198. }
  199. if (priodiff < 0) {
  200. return 1;
  201. }
  202. if (priodiff > 0) {
  203. return -1;
  204. }
  205. if (distdiff < 0) {
  206. return 1;
  207. }
  208. if (distdiff > 0) {
  209. return -1;
  210. }
  211. return 0;
  212. }
  213. void
  214. routesimple_head(const route_head* rte)
  215. {
  216. cur_rte = NULL;
  217. /* build array of XTE/wpt xref records */
  218. xte_count = 0;
  219. tmpprev = NULL;
  220. totalerror = 0;
  221. /* short-circuit if we already have fewer than the max points */
  222. if (countopt && count >= rte->rte_waypt_ct) {
  223. return;
  224. }
  225. /* short-circuit if the route is impossible to simplify, too. */
  226. if (2 >= rte->rte_waypt_ct) {
  227. return;
  228. }
  229. xte_recs = (struct xte*) xcalloc(rte->rte_waypt_ct, sizeof(struct xte));
  230. cur_rte = rte;
  231. }
  232. void
  233. shuffle_xte(struct xte* xte_rec)
  234. {
  235. struct xte tmp_xte;
  236. while (xte_rec > xte_recs && compare_xte(xte_rec, xte_rec-1) < 0) {
  237. tmp_xte.distance = xte_rec->distance;
  238. tmp_xte.ordinal = xte_rec->ordinal;
  239. tmp_xte.intermed = xte_rec->intermed;
  240. xte_rec->distance = xte_rec[-1].distance;
  241. xte_rec->ordinal = xte_rec[-1].ordinal;
  242. xte_rec->intermed = xte_rec[-1].intermed;
  243. xte_rec->intermed->xte_rec = xte_rec;
  244. xte_rec--;
  245. xte_rec->distance = tmp_xte.distance;
  246. xte_rec->ordinal = tmp_xte.ordinal;
  247. xte_rec->intermed = tmp_xte.intermed;
  248. xte_rec->intermed->xte_rec = xte_rec;
  249. }
  250. while (xte_rec - xte_recs < xte_count-2 &&
  251. compare_xte(xte_rec, xte_rec+1) > 0) {
  252. tmp_xte.distance = xte_rec->distance;
  253. tmp_xte.ordinal = xte_rec->ordinal;
  254. tmp_xte.intermed = xte_rec->intermed;
  255. xte_rec->distance = xte_rec[1].distance;
  256. xte_rec->ordinal = xte_rec[1].ordinal;
  257. xte_rec->intermed = xte_rec[1].intermed;
  258. xte_rec->intermed->xte_rec = xte_rec;
  259. xte_rec++;
  260. xte_rec->distance = tmp_xte.distance;
  261. xte_rec->ordinal = tmp_xte.ordinal;
  262. xte_rec->intermed = tmp_xte.intermed;
  263. xte_rec->intermed->xte_rec = xte_rec;
  264. }
  265. }
  266. void
  267. routesimple_tail(const route_head* rte)
  268. {
  269. int i;
  270. if (!cur_rte) {
  271. return;
  272. }
  273. /* compute all distances */
  274. for (i = 0; i < xte_count ; i++) {
  275. compute_xte(xte_recs+i);
  276. }
  277. /* sort XTE array, lowest XTE last */
  278. qsort(xte_recs, xte_count, sizeof(struct xte), compare_xte);
  279. for (i = 0; i < xte_count; i++) {
  280. xte_recs[i].intermed->xte_rec = xte_recs+i;
  281. }
  282. // Ensure totalerror starts with the distance between first and second points
  283. // and not the zero-init. From a June 25, 2014 thread titled "Simplify
  284. // Filter: GPSBabel removes one trackpoint..." I never could repro it it
  285. // with the sample data, so there is no automated test case, but Steve's
  286. // fix is "obviously" right here.
  287. if (xte_count >= 1) {
  288. totalerror = xte_recs[xte_count-1].distance;
  289. }
  290. /* while we still have too many records... */
  291. while ((xte_count) && ((countopt && count < xte_count) || (erroropt && totalerror < error))) {
  292. i = xte_count - 1;
  293. /* remove the record with the lowest XTE */
  294. if (erroropt) {
  295. if (xteopt || relopt) {
  296. if (i > 1) {
  297. totalerror = xte_recs[i-1].distance;
  298. } else {
  299. totalerror = xte_recs[i].distance;
  300. }
  301. }
  302. if (lenopt) {
  303. totalerror += xte_recs[i].distance;
  304. }
  305. }
  306. (*waypt_del_fnp)((route_head*)(void*)rte,
  307. (Waypoint*)(void*)(xte_recs[i].intermed->wpt));
  308. delete (Waypoint*)(void*)(xte_recs[i].intermed->wpt);
  309. if (xte_recs[i].intermed->prev) {
  310. xte_recs[i].intermed->prev->next = xte_recs[i].intermed->next;
  311. compute_xte(xte_recs[i].intermed->prev->xte_rec);
  312. shuffle_xte(xte_recs[i].intermed->prev->xte_rec);
  313. }
  314. if (xte_recs[i].intermed->next) {
  315. xte_recs[i].intermed->next->prev = xte_recs[i].intermed->prev;
  316. compute_xte(xte_recs[i].intermed->next->xte_rec);
  317. shuffle_xte(xte_recs[i].intermed->next->xte_rec);
  318. }
  319. xte_count--;
  320. free_xte(xte_recs+xte_count);
  321. /* end of loop */
  322. }
  323. if (xte_count) {
  324. do {
  325. xte_count--;
  326. free_xte(xte_recs+xte_count);
  327. } while (xte_count);
  328. }
  329. xfree(xte_recs);
  330. }
  331. void
  332. routesimple_process(void)
  333. {
  334. waypt_del_fnp = route_del_wpt;
  335. route_disp_all(routesimple_head, routesimple_tail, routesimple_waypt_pr);
  336. waypt_del_fnp = track_del_wpt;
  337. track_disp_all(routesimple_head, routesimple_tail, routesimple_waypt_pr);
  338. }
  339. void
  340. routesimple_init(const char* args)
  341. {
  342. count = 0;
  343. if (!!countopt == !!erroropt) {
  344. fatal(MYNAME ": You must specify either count or error, but not both.\n");
  345. }
  346. if ((!!xteopt + !!lenopt + !!relopt) > 1) {
  347. fatal(MYNAME ": You may specify only one of crosstrack, length, or relative.\n");
  348. }
  349. if (!xteopt && !lenopt && !relopt) {
  350. xteopt = (char*) "";
  351. }
  352. if (countopt) {
  353. count = atol(countopt);
  354. }
  355. if (erroropt) {
  356. int res = parse_distance(erroropt, &error, 1.0, MYNAME);
  357. if (res == 0) {
  358. error = 0;
  359. } else if (res == 2) { /* parameter with unit */
  360. error = METERS_TO_MILES(error);
  361. }
  362. }
  363. }
  364. void
  365. routesimple_deinit(void)
  366. {
  367. /* do nothing */
  368. }
  369. filter_vecs_t routesimple_vecs = {
  370. routesimple_init,
  371. routesimple_process,
  372. routesimple_deinit,
  373. NULL,
  374. routesimple_args
  375. };