From 9ea3468091a69335d6c1b2f079a2075d623e2f32 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Tue, 7 Jul 2020 21:48:15 +0200 Subject: [PATCH] Negative distances go in the opposite direction --- geo/geo.cpp | 2 +- test/test_geo.cpp | 40 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 41 insertions(+), 1 deletion(-) diff --git a/geo/geo.cpp b/geo/geo.cpp index 876745ef9c..2ea12abf95 100644 --- a/geo/geo.cpp +++ b/geo/geo.cpp @@ -304,7 +304,7 @@ void waypoint_from_heading_and_distance(double lat_start, double lon_start, floa double *lat_target, double *lon_target) { bearing = wrap_2pi(bearing); - double radius_ratio = (double)fabs((double)dist) / CONSTANTS_RADIUS_OF_EARTH; + double radius_ratio = dist / CONSTANTS_RADIUS_OF_EARTH; double lat_start_rad = math::radians(lat_start); double lon_start_rad = math::radians(lon_start); diff --git a/test/test_geo.cpp b/test/test_geo.cpp index 0f30a017d3..c59c5a51d1 100644 --- a/test/test_geo.cpp +++ b/test/test_geo.cpp @@ -95,3 +95,43 @@ TEST_F(GeoTest, projectReproject) EXPECT_FLOAT_EQ(lat, lat_new); EXPECT_FLOAT_EQ(lon, lon_new); } + +TEST_F(GeoTest, waypoint_from_heading_and_zero_distance) +{ + // GIVEN: a starting waypoint, a heading and a distance of 0 + double lat_start = -33; + double lon_start = 18; + float bearing = 0; + float dist = 0; + + double lat_target = 0; + double lon_target = 0; + + // WHEN: we get the next waypoint + waypoint_from_heading_and_distance(lat_start, lon_start, bearing, dist, &lat_target, &lon_target); + + // THEN: it should be the same + EXPECT_DOUBLE_EQ(lat_start, lat_target); + EXPECT_DOUBLE_EQ(lon_start, lon_target); +} + + +TEST_F(GeoTest, waypoint_from_heading_and_negative_distance) +{ + // GIVEN: a starting waypoint, a heading and a distance of 0 + double lat_start = -33; + double lon_start = 18; + float bearing = 0; + float lat_offset = -0.01f; + float dist = CONSTANTS_RADIUS_OF_EARTH * std::sin(M_PI/180) * lat_offset; + + double lat_target = 0; + double lon_target = 0; + + // WHEN: we get the next waypoint + waypoint_from_heading_and_distance(lat_start, lon_start, bearing, dist, &lat_target, &lon_target); + + // THEN: it should be the same + EXPECT_FLOAT_EQ(lat_start + lat_offset, lat_target); + EXPECT_DOUBLE_EQ(lon_start, lon_target); +}