Add example for dist and bearing

MaJerle · Jul 26, 2020 · f24c71f · f24c71f
1 parent 1d1e72c
commit f24c71f
Show file tree

Hide file tree

Showing 4 changed files with 53 additions and 5 deletions.
diff --git a/dev/VisualStudio/main.c b/dev/VisualStudio/main.c
@@ -12,7 +12,21 @@ extern void run_tests();
 
 int
 main() {
+    lwgps_float_t distance, bearing;
     run_tests();
 
+    /* Calculate distance and bearing */
+    lwgps_distance_bearing(40.6, -73.7, 48.3, 11.7, &distance, &bearing);
+
+    printf("Distance: %lf meters\r\n", (double)distance);
+    printf("Bearing:  %lf degrees\r\n", (double)bearing);
+
+    lwgps_distance_bearing(48.3, 11.7, 40.6, -73.7, &distance, &bearing);
+    printf("Distance: %lf meters\r\n", (double)distance);
+    printf("Bearing:  %lf degrees\r\n", (double)bearing);
+
     return 0;
 }
+
+/* JFK: 40.642569, -73.783790 */
+/* Munich: 48.353962, 11.775114 */
diff --git a/docs/examples/index.rst b/docs/examples/index.rst
@@ -31,5 +31,15 @@ Data are later processed outside interrupt context.
     :linenos:
     :caption: Example of buffer
 
+Distance and bearing
+^^^^^^^^^^^^^^^^^^^^
+
+Library provides calculation of distance and bearing between `2` coordinates on earth.
+
+.. literalinclude:: ../../examples/example_dist_bear.c
+    :language: c
+    :linenos:
+    :caption: Distance and bearing calculation
+
 .. toctree::
 	:maxdepth: 2
diff --git a/examples/example_dist_bear.c b/examples/example_dist_bear.c
@@ -0,0 +1,24 @@
+#include "lwgps/lwgps.h"
+
+/* Distance and bearing results */
+lwgps_float_t dist, bear;
+
+/* New York coordinates */
+lwgps_float_t lat1 = 40.685721;
+lwgps_float_t lon1 = -73.820465;
+
+/* Munich coordinates */
+lwgps_float_t lat2 = 48.150906;
+lwgps_float_t lon2 = 11.554176;
+
+/* Go from New York to Munich */
+/* Calculate distance and bearing related to north */
+lwgps_distance_bearing(lat1, lon1, lat2, lon2, &dist, &bear);
+printf("Distance: %f meters\r\n", (float)dist);
+printf("Initial bearing: %f degrees\r\n", (float)bear);
+
+/* Go from Munich to New York */
+/* Calculate distance and bearing related to north */
+lwgps_distance_bearing(lat2, lon2, lat1, lon1, &dist, &bear);
+printf("Distance: %f meters\r\n", (float)dist);
+printf("Initial bearing: %f degrees\r\n", (float)bear);
diff --git a/lwgps/src/lwgps/lwgps.c b/lwgps/src/lwgps/lwgps.c
@@ -69,10 +69,10 @@ prv_parse_number(lwgps_t* gh, const char* t) {
     if (t == NULL) {
         t = gh->p.term_str;
     }
-    for (; t != NULL && *t == ' '; t++) {}      /* Strip leading spaces */
+    for (; t != NULL && *t == ' '; ++t) {}      /* Strip leading spaces */
 
-    minus = (*t == '-' ? (t++, 1) : 0);
-    for (; t != NULL && CIN(*t); t++) {
+    minus = (*t == '-' ? (++t, 1) : 0);
+    for (; t != NULL && CIN(*t); ++t) {
         res = 10 * res + CTN(*t);
     }
     return minus ? -res : res;
@@ -91,7 +91,7 @@ prv_parse_float_number(lwgps_t* gh, const char* t) {
     if (t == NULL) {
         t = gh->p.term_str;
     }
-    for (; t != NULL && *t == ' '; t++) {}      /* Strip leading spaces */
+    for (; t != NULL && *t == ' '; ++t) {}      /* Strip leading spaces */
 
 #if LWGPS_CFG_DOUBLE
     res = strtod(t, NULL);                      /* Parse string to double */
@@ -514,7 +514,7 @@ lwgps_distance_bearing(lwgps_float_t las, lwgps_float_t los, lwgps_float_t lae,
     if (d != NULL) {
         /*
          * a = sin(df / 2)^2 + cos(las) * cos(lae) * sin(dfi / 2)^2
-         * *d = RADIUS * 2 * atan(a / (1 - a)) * 1000 (for meters)
+         * *d = RADIUS * 2 * atan(sqrt(a) / sqrt(1 - a)) * 1000 (for meters)
          */
 #if LWGPS_CFG_DOUBLE
         a = FLT(sin(df * 0.5) * sin(df * 0.5) + sin(dfi * 0.5) * sin(dfi * 0.5) * cos(las) * cos(lae));