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.

radius.cc 5.3KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252
  1. /*
  2. Radius Filter
  3. Copyright (C) 2002 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 <stdlib.h>
  20. #include <stdio.h>
  21. #if FILTERS_ENABLED
  22. #ifndef M_PI
  23. # define M_PI 3.14159265358979323846
  24. #endif
  25. static double pos_dist;
  26. static char* distopt = NULL;
  27. static char* latopt = NULL;
  28. static char* lonopt = NULL;
  29. static char* exclopt = NULL;
  30. static char* nosort = NULL;
  31. static char* maxctarg = NULL;
  32. static char* routename = NULL;
  33. static int maxct;
  34. static Waypoint* home_pos;
  35. typedef struct {
  36. double distance;
  37. } extra_data;
  38. static
  39. arglist_t radius_args[] = {
  40. {
  41. "lat", &latopt, "Latitude for center point (D.DDDDD)",
  42. NULL, ARGTYPE_FLOAT | ARGTYPE_REQUIRED, ARG_NOMINMAX
  43. },
  44. {
  45. "lon", &lonopt, "Longitude for center point (D.DDDDD)",
  46. NULL, ARGTYPE_FLOAT | ARGTYPE_REQUIRED, ARG_NOMINMAX
  47. },
  48. {
  49. "distance", &distopt, "Maximum distance from center",
  50. NULL, ARGTYPE_FLOAT | ARGTYPE_REQUIRED, ARG_NOMINMAX
  51. },
  52. {
  53. "exclude", &exclopt, "Exclude points close to center",
  54. NULL, ARGTYPE_BOOL, ARG_NOMINMAX
  55. },
  56. {
  57. "nosort", &nosort, "Inhibit sort by distance to center",
  58. NULL, ARGTYPE_BOOL, ARG_NOMINMAX
  59. },
  60. {
  61. "maxcount", &maxctarg,"Output no more than this number of points",
  62. NULL, ARGTYPE_INT, "1", NULL
  63. },
  64. {
  65. "asroute", &routename,"Put resulting waypoints in route of this name",
  66. NULL, ARGTYPE_STRING, NULL, NULL
  67. },
  68. ARG_TERMINATOR
  69. };
  70. static double
  71. gc_distance(double lat1, double lon1, double lat2, double lon2)
  72. {
  73. return gcdist(
  74. RAD(lat1),
  75. RAD(lon1),
  76. RAD(lat2),
  77. RAD(lon2)
  78. );
  79. }
  80. static int
  81. dist_comp(const void* a, const void* b)
  82. {
  83. const Waypoint* x1 = *(Waypoint**)a;
  84. const Waypoint* x2 = *(Waypoint**)b;
  85. extra_data* x1e = (extra_data*) x1->extra_data;
  86. extra_data* x2e = (extra_data*) x2->extra_data;
  87. if (x1e->distance > x2e->distance) {
  88. return 1;
  89. }
  90. if (x1e->distance < x2e->distance) {
  91. return -1;
  92. }
  93. return 0;
  94. }
  95. void
  96. radius_process(void)
  97. {
  98. #if !NEWQ
  99. queue* elem, * tmp;
  100. #endif
  101. double dist;
  102. Waypoint** comp;
  103. int i, wc;
  104. queue temp_head;
  105. route_head* rte_head = NULL;
  106. #if NEWQ
  107. foreach(Waypoint* waypointp, waypt_list) {
  108. #else
  109. QUEUE_FOR_EACH(&waypt_head, elem, tmp) {
  110. Waypoint* waypointp = (Waypoint*)elem;
  111. #endif
  112. dist = gc_distance(waypointp->latitude,
  113. waypointp->longitude,
  114. home_pos->latitude,
  115. home_pos->longitude);
  116. /* convert radians to float point statute miles */
  117. dist = radtomiles(dist);
  118. if ((dist >= pos_dist) == (exclopt == NULL)) {
  119. waypt_del(waypointp);
  120. delete waypointp;
  121. continue;
  122. }
  123. extra_data* ed = (extra_data*) xcalloc(1, sizeof(*ed));
  124. ed->distance = dist;
  125. waypointp->extra_data = ed;
  126. }
  127. wc = waypt_count();
  128. QUEUE_INIT(&temp_head);
  129. comp = (Waypoint**) xcalloc(wc, sizeof(*comp));
  130. i = 0;
  131. /*
  132. * Create an array of remaining waypoints, popping them off the
  133. * master queue as we go. This gives us something reasonable
  134. * for qsort.
  135. */
  136. #if NEWQ
  137. foreach(Waypoint* wp, waypt_list) {
  138. #else
  139. QUEUE_FOR_EACH(&waypt_head, elem, tmp) {
  140. Waypoint* wp = (Waypoint*) elem;
  141. #endif
  142. comp[i] = wp;
  143. waypt_del(wp);
  144. i++;
  145. }
  146. if (!nosort) {
  147. qsort(comp, wc, sizeof(Waypoint*), dist_comp);
  148. }
  149. if (routename) {
  150. rte_head = route_head_alloc();
  151. rte_head->rte_name = routename;
  152. route_add_head(rte_head);
  153. }
  154. /*
  155. * The comp array is now sorted by distance. As we run through it,
  156. * we push them back onto the master wp list, letting us pass them
  157. * on through in the modified order.
  158. */
  159. for (i = 0; i < wc; i++) {
  160. Waypoint* wp = comp[i];
  161. xfree(wp->extra_data);
  162. wp->extra_data = NULL;
  163. if (maxctarg && i >= maxct) {
  164. continue;
  165. }
  166. if (routename) {
  167. route_add_wpt(rte_head, wp);
  168. } else {
  169. waypt_add(wp);
  170. }
  171. }
  172. xfree(comp);
  173. }
  174. void
  175. radius_init(const char* args)
  176. {
  177. char* fm;
  178. pos_dist = 0;
  179. if (distopt) {
  180. pos_dist = strtod(distopt, &fm);
  181. if ((*fm == 'k') || (*fm == 'K')) {
  182. /* distance is kilometers, convert to feet */
  183. pos_dist *= .6214;
  184. }
  185. }
  186. if (maxctarg) {
  187. maxct = atoi(maxctarg);
  188. } else {
  189. maxct = 0;
  190. }
  191. home_pos = (Waypoint*) xcalloc(sizeof(*home_pos), 1);
  192. if (latopt) {
  193. home_pos->latitude = atof(latopt);
  194. }
  195. if (lonopt) {
  196. home_pos->longitude = atof(lonopt);
  197. }
  198. }
  199. void
  200. radius_deinit(void)
  201. {
  202. if (home_pos) {
  203. xfree(home_pos);
  204. }
  205. }
  206. filter_vecs_t radius_vecs = {
  207. radius_init,
  208. radius_process,
  209. radius_deinit,
  210. NULL,
  211. radius_args
  212. };
  213. #endif // FILTERS_ENABLED