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.

arcdist.cc 8.2KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306
  1. /*
  2. Distance from point to arc 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. #include "defs.h"
  17. #include "filterdefs.h"
  18. #include "grtcirc.h"
  19. #include <math.h>
  20. #include <stdio.h>
  21. #include <stdlib.h> // strtod
  22. #if FILTERS_ENABLED
  23. #define MYNAME "Arc filter"
  24. static double pos_dist;
  25. static char* distopt = NULL;
  26. static char* arcfileopt = NULL;
  27. static char* rteopt = NULL;
  28. static char* trkopt = NULL;
  29. static char* exclopt = NULL;
  30. static char* ptsopt = NULL;
  31. static char* projectopt = NULL;
  32. typedef struct {
  33. double distance;
  34. double prjlatitude, prjlongitude;
  35. double frac;
  36. Waypoint* arcpt1, * arcpt2;
  37. } extra_data;
  38. static
  39. arglist_t arcdist_args[] = {
  40. {
  41. "file", &arcfileopt, "File containing vertices of arc",
  42. NULL, ARGTYPE_FILE, ARG_NOMINMAX
  43. },
  44. {
  45. "rte", &rteopt, "Route(s) are vertices of arc",
  46. NULL, ARGTYPE_BOOL, ARG_NOMINMAX
  47. },
  48. {
  49. "trk", &trkopt, "Track(s) are vertices of arc",
  50. NULL, ARGTYPE_BOOL, ARG_NOMINMAX
  51. },
  52. {
  53. "distance", &distopt, "Maximum distance from arc",
  54. NULL, ARGTYPE_FLOAT | ARGTYPE_REQUIRED, ARG_NOMINMAX
  55. },
  56. {
  57. "exclude", &exclopt, "Exclude points close to the arc", NULL,
  58. ARGTYPE_BOOL, ARG_NOMINMAX
  59. },
  60. {
  61. "points", &ptsopt, "Use distance from vertices not lines",
  62. NULL, ARGTYPE_BOOL, ARG_NOMINMAX
  63. },
  64. {
  65. "project", &projectopt, "Move waypoints to its projection on lines or vertices",
  66. NULL, ARGTYPE_BOOL, ARG_NOMINMAX
  67. },
  68. ARG_TERMINATOR
  69. };
  70. #define BADVAL 999999
  71. static void
  72. arcdist_arc_disp_wpt_cb(const Waypoint* arcpt2)
  73. {
  74. queue* elem, * tmp;
  75. Waypoint* waypointp;
  76. extra_data* ed;
  77. double dist;
  78. double prjlat, prjlon, frac;
  79. static Waypoint* arcpt1 = NULL;
  80. if (arcpt2 && arcpt2->latitude != BADVAL && arcpt2->longitude != BADVAL &&
  81. (ptsopt || (arcpt1 &&
  82. (arcpt1->latitude != BADVAL && arcpt1->longitude != BADVAL)))) {
  83. #if NEWQ
  84. foreach(Waypoint* waypointp, waypt_list) {
  85. #else
  86. QUEUE_FOR_EACH(&waypt_head, elem, tmp) {
  87. waypointp = (Waypoint*) elem;
  88. #endif
  89. if (waypointp->extra_data) {
  90. ed = (extra_data*) waypointp->extra_data;
  91. } else {
  92. ed = (extra_data*) xcalloc(1, sizeof(*ed));
  93. ed->distance = BADVAL;
  94. }
  95. if (ed->distance == BADVAL || projectopt || ed->distance >= pos_dist) {
  96. if (ptsopt) {
  97. dist = gcdist(RAD(arcpt2->latitude),
  98. RAD(arcpt2->longitude),
  99. RAD(waypointp->latitude),
  100. RAD(waypointp->longitude));
  101. prjlat = arcpt2->latitude;
  102. prjlon = arcpt2->longitude;
  103. frac = 1.0;
  104. } else {
  105. dist = linedistprj(arcpt1->latitude,
  106. arcpt1->longitude,
  107. arcpt2->latitude,
  108. arcpt2->longitude,
  109. waypointp->latitude,
  110. waypointp->longitude,
  111. &prjlat, &prjlon, &frac);
  112. }
  113. /* convert radians to float point statute miles */
  114. dist = radtomiles(dist);
  115. if (ed->distance > dist) {
  116. ed->distance = dist;
  117. if (projectopt) {
  118. ed->prjlatitude = prjlat;
  119. ed->prjlongitude = prjlon;
  120. ed->frac = frac;
  121. ed->arcpt1 = arcpt1;
  122. ed->arcpt2 = (Waypoint*) arcpt2;
  123. }
  124. }
  125. waypointp->extra_data = ed;
  126. }
  127. }
  128. }
  129. arcpt1 = (Waypoint*) arcpt2;
  130. }
  131. static void
  132. arcdist_arc_disp_hdr_cb(const route_head* rte)
  133. {
  134. /* Set arcpt1 to NULL */
  135. arcdist_arc_disp_wpt_cb(NULL);
  136. }
  137. void
  138. arcdist_process(void)
  139. {
  140. queue* elem, * tmp;
  141. unsigned removed;
  142. if (arcfileopt) {
  143. int fileline = 0;
  144. char* line;
  145. gbfile* file_in;
  146. Waypoint* arcpt2, * arcpt1;
  147. file_in = gbfopen(arcfileopt, "r", MYNAME);
  148. arcpt1 = new Waypoint;
  149. arcpt2 = new Waypoint;
  150. arcdist_arc_disp_hdr_cb(NULL);
  151. arcpt2->latitude = arcpt2->longitude = BADVAL;
  152. while ((line = gbfgetstr(file_in))) {
  153. char* pound = NULL;
  154. int argsfound = 0;
  155. fileline++;
  156. pound = strchr(line, '#');
  157. if (pound) {
  158. if (0 == strncmp(pound, "#break", 6)) {
  159. arcdist_arc_disp_hdr_cb(NULL);
  160. }
  161. *pound = '\0';
  162. }
  163. arcpt2->latitude = arcpt2->longitude = BADVAL;
  164. argsfound = sscanf(line, "%lf %lf", &arcpt2->latitude, &arcpt2->longitude);
  165. if (argsfound != 2 && strspn(line, " \t\n") < strlen(line)) {
  166. warning(MYNAME ": Warning: Arc file contains unusable vertex on line %d.\n", fileline);
  167. } else {
  168. Waypoint* arcpttmp = arcpt1;
  169. arcdist_arc_disp_wpt_cb(arcpt2);
  170. arcpt1 = arcpt2;
  171. arcpt2 = arcpttmp;
  172. }
  173. }
  174. delete arcpt1;
  175. delete arcpt2;
  176. gbfclose(file_in);
  177. } else if (rteopt) {
  178. route_disp_all(arcdist_arc_disp_hdr_cb, NULL, arcdist_arc_disp_wpt_cb);
  179. } else if (trkopt) {
  180. track_disp_all(arcdist_arc_disp_hdr_cb, NULL, arcdist_arc_disp_wpt_cb);
  181. }
  182. removed = 0;
  183. #if NEWQ
  184. foreach(Waypoint* wp, waypt_list) {
  185. #else
  186. QUEUE_FOR_EACH(&waypt_head, elem, tmp) {
  187. Waypoint* wp = (Waypoint*) elem;
  188. #endif
  189. extra_data* ed;
  190. ed = (extra_data*) wp->extra_data;
  191. wp->extra_data = NULL;
  192. if (ed) {
  193. if ((ed->distance >= pos_dist) == (exclopt == NULL)) {
  194. waypt_del(wp);
  195. delete wp;
  196. removed++;
  197. } else if (projectopt) {
  198. wp->longitude = ed->prjlongitude;
  199. wp->latitude = ed->prjlatitude;
  200. wp->route_priority = 1;
  201. if (!arcfileopt &&
  202. (ed->arcpt2->altitude != unknown_alt) &&
  203. (ptsopt || (ed->arcpt1->altitude != unknown_alt))) {
  204. /* Interpolate alititude */
  205. if (ptsopt) {
  206. wp->altitude = ed->arcpt2->altitude;
  207. } else {
  208. wp->altitude = ed->arcpt1->altitude +
  209. ed->frac * (ed->arcpt2->altitude - ed->arcpt1->altitude);
  210. }
  211. }
  212. if (trkopt &&
  213. (ed->arcpt2->GetCreationTime().isValid()) &&
  214. (ptsopt || (ed->arcpt1->GetCreationTime().isValid()))) {
  215. /* Interpolate time */
  216. if (ptsopt) {
  217. wp->SetCreationTime(ed->arcpt2->GetCreationTime());
  218. } else {
  219. // Apply the multiplier to the difference between the times
  220. // of the two points. Add that to the first for the
  221. // interpolated time.
  222. int scaled_time = ed->frac *
  223. ed->arcpt1->GetCreationTime().msecsTo(ed->arcpt2->GetCreationTime());
  224. QDateTime new_time(ed->arcpt1->GetCreationTime().addMSecs(scaled_time));
  225. wp->SetCreationTime(new_time);
  226. }
  227. }
  228. if (global_opts.debug_level >= 1) {
  229. warning("Including waypoint %s at dist:%f lat:%f lon:%f\n",
  230. qPrintable(wp->shortname), ed->distance, wp->latitude, wp->longitude);
  231. }
  232. }
  233. xfree(ed);
  234. }
  235. }
  236. if (global_opts.verbose_status > 0) {
  237. printf(MYNAME "-arc: %d waypoint(s) removed.\n", removed);
  238. }
  239. }
  240. void
  241. arcdist_init(const char* args)
  242. {
  243. char* fm;
  244. if ((!arcfileopt && !rteopt && !trkopt) ||
  245. (arcfileopt && (rteopt || trkopt)) ||
  246. (rteopt && trkopt)) {
  247. fatal(MYNAME ": Incompatible or incomplete option values!\n");
  248. }
  249. pos_dist = 0;
  250. if (distopt) {
  251. pos_dist = strtod(distopt, &fm);
  252. if ((*fm == 'k') || (*fm == 'K')) {
  253. /* distance is kilometers, convert to mile */
  254. pos_dist *= .6214;
  255. }
  256. }
  257. }
  258. void
  259. arcdist_deinit(void)
  260. {
  261. /* do nothing */
  262. }
  263. filter_vecs_t arcdist_vecs = {
  264. arcdist_init,
  265. arcdist_process,
  266. arcdist_deinit,
  267. NULL,
  268. arcdist_args
  269. };
  270. #endif // FILTERS_ENABLED