Destination point given distance and bearing from start point (#5)

* Destination point given distance and bearing from start point

* can be const

include/geo/latlng.h

@@ -32,6 +32,8 @@ latlng midpoint(latlng const&, latlng const&);
 latlng closest_on_segment(latlng const& x, latlng const& segment_from,
  latlng const& segment_to);
 
+latlng destination_point(latlng const& source, double const distance, double const bearing);
+
 uint32_t tile_hash_32(latlng const&);
 
 } // namespace geo

src/latlng.cc

@@ -112,4 +112,33 @@ latlng closest_on_segment(latlng const& x, latlng const& segment_from,
  }
 }
 
+// Destination point given distance and bearing from start point
+// http://www.movable-type.co.uk/scripts/latlong.html
+latlng destination_point(geo::latlng const& source,
+ double const distance,
+ double const bearing) {
+ // convert to rad
+ auto const lat_source_rad = to_rad(source.lat_);
+ auto const bearing_rad = to_rad(bearing);
+
+ // reused
+ auto const sin_lat_source = std::sin(lat_source_rad);
+ auto const cos_lat_source = std::cos(lat_source_rad);
+ auto const angular_distance = distance / kEarthRadiusMeters;
+ auto const sin_angular_distance = std::sin(angular_distance);
+ auto const cos_angular_distance = std::cos(angular_distance);
+
+ // calculate lat/lon of destination
+ auto const lat_dest =
+ std::asin(sin_lat_source * cos_angular_distance +
+ cos_lat_source * sin_angular_distance * std::cos(bearing_rad));
+ auto const lon_dest =
+ to_rad(source.lng_) +
+ std::atan2(std::sin(bearing_rad) * sin_angular_distance * cos_lat_source,
+ cos_angular_distance - sin_lat_source * std::sin(lat_dest));
+
+ // convert back to deg
+ return geo::latlng{to_deg(lat_dest), to_deg(lon_dest)};
+}
+
 } // namespace geo

test/latlng_test.cc

@@ -0,0 +1,29 @@
+#include "doctest/doctest.h"
+
+#include "geo/latlng.h"
+
+TEST_CASE("destination_point") {
+ auto constexpr source1 = geo::latlng{40.0, -20.0};
+ auto constexpr distance1 = 111800.0;
+ auto constexpr bearing1 = 0.0;
+ auto constexpr destination1_exp = geo::latlng{41.00555556, -20.0};
+ auto const destination1_act = geo::destination_point(source1, distance1, bearing1);
+ CHECK(destination1_act.lat_ == doctest::Approx(destination1_exp.lat_));
+ CHECK(destination1_act.lng_ == doctest::Approx(destination1_exp.lng_));
+
+ auto constexpr source2 = geo::latlng{-23.0, 42.0};
+ auto constexpr distance2 = 2342000.0;
+ auto constexpr bearing2 = 90.0;
+ auto constexpr destination2_exp = geo::latlng{-21.38472222, 64.70277777};
+ auto const destination2_act = geo::destination_point(source2, distance2, bearing2);
+ CHECK(destination2_act.lat_ == doctest::Approx(destination2_exp.lat_));
+ CHECK(destination2_act.lng_ == doctest::Approx(destination2_exp.lng_));
+
+ auto constexpr source3 = geo::latlng{89.0, 3.0};
+ auto constexpr distance3 = 11111000.0;
+ auto constexpr bearing3 = 77.0;
+ auto constexpr destination3_exp = geo::latlng{-9.69722222, 106.16833333};
+ auto const destination3_act = geo::destination_point(source3, distance3, bearing3);
+ CHECK(destination3_act.lat_ == doctest::Approx(destination3_exp.lat_));
+ CHECK(destination3_act.lng_ == doctest::Approx(destination3_exp.lng_));
+}