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.

219 lines
5.7KB

  1. /*
  2. Add route points before and after a bend.
  3. Copyright (C) 2011 Fernando Arbeiza, fernando.arbeiza@gmail.com
  4. Copyright (C) 2011 Robert Lipe, robertlipe+source@gpsbabel.org
  5. This program is free software; you can redistribute it and/or modify
  6. it under the terms of the GNU General Public License as published by
  7. the Free Software Foundation; either version 2 of the License, or
  8. (at your option) any later version.
  9. This program is distributed in the hope that it will be useful,
  10. but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. GNU General Public License for more details.
  13. You should have received a copy of the GNU General Public License
  14. along with this program; if not, write to the Free Software
  15. Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
  16. */
  17. #include "defs.h"
  18. #include "filterdefs.h"
  19. #include "grtcirc.h"
  20. #include <stdlib.h>
  21. #include <cmath>
  22. #define MYNAME "bend"
  23. #if FILTERS_ENABLED
  24. static char* distopt = NULL;
  25. static char* minangleopt = NULL;
  26. static double maxDist;
  27. static double minAngle;
  28. static queue* routes_orig = NULL;
  29. static int routes_orig_num = 0;
  30. static
  31. arglist_t bend_args[] = {
  32. {
  33. "distance", &distopt, "Distance to the bend in meters where the new points will be added",
  34. "25", ARGTYPE_FLOAT, ARG_NOMINMAX
  35. },
  36. {
  37. "minangle", &minangleopt, "Minimum bend angle in degrees", "5",
  38. ARGTYPE_FLOAT, ARG_NOMINMAX
  39. },
  40. ARG_TERMINATOR
  41. };
  42. static void
  43. bend_init(const char* args)
  44. {
  45. maxDist = 0.0;
  46. if (distopt) {
  47. maxDist = strtod(distopt, NULL);
  48. }
  49. minAngle = 0.0;
  50. if (minangleopt) {
  51. minAngle = strtod(minangleopt, NULL);
  52. }
  53. route_backup(&routes_orig_num, &routes_orig);
  54. route_flush_all_routes();
  55. }
  56. static Waypoint*
  57. create_wpt_dest(const Waypoint* wpt_orig, double lat_orig,
  58. double long_orig, double lat_orig_adj, double long_orig_adj)
  59. {
  60. double distance = gcdist(lat_orig, long_orig,
  61. lat_orig_adj, long_orig_adj);
  62. double frac;
  63. double lat_dest;
  64. double long_dest;
  65. Waypoint* wpt_dest = NULL;
  66. distance = radtometers(distance);
  67. if (distance <= maxDist) {
  68. return NULL;
  69. }
  70. frac = maxDist / distance;
  71. linepart(lat_orig, long_orig, lat_orig_adj, long_orig_adj, frac,
  72. &lat_dest, &long_dest);
  73. wpt_dest = new Waypoint(*wpt_orig);
  74. wpt_dest->latitude = DEG(lat_dest);
  75. wpt_dest->longitude = DEG(long_dest);
  76. return wpt_dest;
  77. }
  78. static int
  79. is_small_angle(double lat_orig, double long_orig, double lat_orig_prev,
  80. double long_orig_prev, double lat_orig_next,
  81. double long_orig_next)
  82. {
  83. double heading_prev = heading_true_degrees(lat_orig, long_orig,
  84. lat_orig_prev, long_orig_prev);
  85. double heading_next = heading_true_degrees(lat_orig, long_orig,
  86. lat_orig_next, long_orig_next);
  87. double heading_diff = heading_next - heading_prev;
  88. return ((std::abs(heading_diff - 0.0) < minAngle)
  89. || (std::abs(heading_diff - 180.0) < minAngle)
  90. || (std::abs(heading_diff - 360.0) < minAngle));
  91. }
  92. static void
  93. process_route(const route_head* route_orig, route_head* route_dest)
  94. {
  95. Waypoint* wpt_orig_prev = NULL;
  96. Waypoint* wpt_orig = NULL;
  97. queue* elem, *tmp;
  98. QUEUE_FOR_EACH(&route_orig->waypoint_list, elem, tmp) {
  99. Waypoint* wpt_orig_next = (Waypoint*)elem;
  100. if (wpt_orig_prev == NULL) {
  101. if (wpt_orig != NULL) {
  102. Waypoint* waypoint_dest = new Waypoint(*wpt_orig);
  103. route_add_wpt(route_dest, waypoint_dest);
  104. }
  105. } else {
  106. double lat_orig = RAD(wpt_orig->latitude);
  107. double long_orig = RAD(wpt_orig->longitude);
  108. double lat_orig_prev = RAD(wpt_orig_prev->latitude);
  109. double long_orig_prev = RAD(wpt_orig_prev->longitude);
  110. double lat_orig_next = RAD(wpt_orig_next->latitude);
  111. double long_orig_next = RAD(wpt_orig_next->longitude);
  112. Waypoint* wpt_dest_next = NULL;
  113. if (is_small_angle(lat_orig, long_orig, lat_orig_prev,
  114. long_orig_prev, lat_orig_next, long_orig_next)) {
  115. Waypoint* waypoint_dest = new Waypoint(*wpt_orig);
  116. route_add_wpt(route_dest, waypoint_dest);
  117. } else {
  118. Waypoint* wpt_dest_prev = create_wpt_dest(wpt_orig,
  119. lat_orig, long_orig, lat_orig_prev, long_orig_prev);
  120. if (wpt_dest_prev != NULL) {
  121. route_add_wpt(route_dest, wpt_dest_prev);
  122. }
  123. wpt_dest_next = create_wpt_dest(wpt_orig,
  124. lat_orig, long_orig, lat_orig_next, long_orig_next);
  125. if (wpt_dest_next != NULL) {
  126. route_add_wpt(route_dest, wpt_dest_next);
  127. wpt_orig = wpt_dest_next;
  128. }
  129. }
  130. }
  131. wpt_orig_prev = wpt_orig;
  132. wpt_orig = wpt_orig_next;
  133. }
  134. if (wpt_orig != NULL) {
  135. Waypoint* waypoint_dest = new Waypoint(*wpt_orig);
  136. route_add_wpt(route_dest, waypoint_dest);
  137. }
  138. }
  139. static void
  140. process_route_orig(const route_head* route_orig)
  141. {
  142. route_head* route_dest = route_head_alloc();
  143. route_dest->rte_name = route_orig->rte_name;
  144. route_dest->rte_desc = route_orig->rte_desc;
  145. route_dest->fs = fs_chain_copy(route_orig->fs);
  146. route_dest->rte_num = route_orig->rte_num;
  147. route_add_head(route_dest);
  148. process_route(route_orig, route_dest);
  149. }
  150. static void
  151. bend_process(void)
  152. {
  153. queue* elem, *tmp;
  154. QUEUE_FOR_EACH(routes_orig, elem, tmp) {
  155. route_head* route_orig = (route_head*)elem;
  156. process_route_orig(route_orig);
  157. }
  158. }
  159. static void
  160. bend_deinit(void)
  161. {
  162. route_flush(routes_orig);
  163. xfree(routes_orig);
  164. }
  165. static void
  166. bend_exit(void)
  167. {
  168. }
  169. filter_vecs_t bend_vecs = {
  170. bend_init,
  171. bend_process,
  172. bend_deinit,
  173. bend_exit,
  174. bend_args
  175. };
  176. #endif // FILTERS_ENABLED