diff --git a/src/configuration.h b/src/configuration.h index f9be2fc46..0ed4dd893 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -88,6 +88,12 @@ along with this program. If not, see . #define MESHTASTIC_PREHOP_DROP 1 #endif +// Use polynomial approximations for trigonometric functions to save flash. +// Override with -D MESHTASTIC_TRIG_APPROX=0 for exact trig for special use cases e.g. close to Earth's poles. +#ifndef MESHTASTIC_TRIG_APPROX +#define MESHTASTIC_TRIG_APPROX 1 +#endif + // Debug/test only: let a wired client (serial/TCP) inject frames into the RX pipeline as if they had // arrived over LoRa - a SIMULATOR_APP ToRadio packet is delivered through the real receive path on real // hardware (see MeshService::injectAsReceived). This forges over-the-air traffic, so it MUST stay 0 in diff --git a/src/gps/GeoCoord.cpp b/src/gps/GeoCoord.cpp index 4afae9394..1fc60c304 100644 --- a/src/gps/GeoCoord.cpp +++ b/src/gps/GeoCoord.cpp @@ -1,4 +1,5 @@ #include "GeoCoord.h" +#include "configuration.h" #include // Narrow a UTM meter value to its unsigned field, clamping non-finite/negative/oversized inputs: an @@ -433,6 +434,43 @@ void GeoCoord::convertWGS84ToOSGB36(const double lat, const double lon, double & //(airyA*airyA/(airyA / sqrt(1 - airyEcc*sin(osgb.latitude)*sin(osgb.latitude)))); // Not used, no OSTN data } +#if MESHTASTIC_TRIG_APPROX +// cos(x) minimax approx for x in [-pi/2, pi/2] ("cos_52"): https://www.ganssle.com/approx.htm +static double cosLatitudeApprox(double latRad) +{ + constexpr double c1 = 0.9999932946, c2 = -0.4999124376, c3 = 0.0414877472, c4 = -0.0012712095; + double x2 = latRad * latRad; + return c1 + x2 * (c2 + x2 * (c3 + c4 * x2)); +} + +/// Approximate distance in meters via equirectangular projection (not exact spherical trig). +/// <1% error to ~500km, degrading near the poles at long range (see test_geocoord_distance). +float GeoCoord::latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b) +{ + // Don't do math if the points are the same + if (lat_a == lat_b && lng_a == lng_b) + return 0.0; + + double a1 = lat_a / DEG_CONVERT; + double a2 = lng_a / DEG_CONVERT; + double b1 = lat_b / DEG_CONVERT; + double b2 = lng_b / DEG_CONVERT; + + double meanLat = (a1 + b1) / 2; + double dLng = b2 - a2; + // Wrap to [-PI, PI]: unlike cos()/sin(), a raw longitude difference doesn't handle points that + // straddle the antimeridian (e.g. 179.9 and -179.9 are ~0.2 degrees apart, not ~360). + if (dLng > PI) + dLng -= 2 * PI; + else if (dLng < -PI) + dLng += 2 * PI; + double x = dLng * cosLatitudeApprox(meanLat); + double y = b1 - a1; + double tt = sqrt(x * x + y * y); + + return (float)(6366000 * tt); +} +#else /// Ported from my old java code, returns distance in meters along the globe /// surface (by Haversine formula) float GeoCoord::latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b) @@ -456,6 +494,7 @@ float GeoCoord::latLongToMeter(double lat_a, double lng_a, double lat_b, double return (float)(6366000 * tt); } +#endif /** * Computes the bearing in degrees between two points on Earth. Ported from my diff --git a/test/test_geocoord_distance/test_main.cpp b/test/test_geocoord_distance/test_main.cpp new file mode 100644 index 000000000..de3430f1c --- /dev/null +++ b/test/test_geocoord_distance/test_main.cpp @@ -0,0 +1,165 @@ +#include "configuration.h" +#include "gps/GeoCoord.h" +#include +#include +#include + +void setUp(void) {} +void tearDown(void) {} + +// Pins latLongToMeter()'s equirectangular-approximation accuracy against the original spherical +// law of cosines, so a future change can't silently regress it. + +static constexpr double kPi = 3.14159265358979323846; + +static double referenceSphericalLawOfCosines(double lat_a, double lng_a, double lat_b, double lng_b) +{ + double a1 = lat_a * kPi / 180.0; + double a2 = lng_a * kPi / 180.0; + double b1 = lat_b * kPi / 180.0; + double b2 = lng_b * kPi / 180.0; + double t1 = std::cos(a1) * std::cos(a2) * std::cos(b1) * std::cos(b2); + double t2 = std::cos(a1) * std::sin(a2) * std::cos(b1) * std::sin(b2); + double t3 = std::sin(a1) * std::sin(b1); + double arg = t1 + t2 + t3; + if (arg > 1.0) + arg = 1.0; + if (arg < -1.0) + arg = -1.0; + return 6366000 * std::acos(arg); +} + +// Below ~1m, relative error is dominated by rounding noise rather than the formula itself, so +// assert an absolute bound instead (still catches a badly-broken implementation). +static constexpr double kNearZeroAbsoluteToleranceMeters = 0.5; + +// An order of magnitude above what the implementation currently produces per group - tight enough to +// catch a regression, loose enough not to track float rounding. Groups differ because +// equirectangular error grows with both separation and latitude. +static constexpr double kLocalTolerancePercent = 0.01; +static constexpr double kRegionalTolerancePercent = 0.1; +static constexpr double kHighLatitudeTolerancePercent = 0.2; +static constexpr double kAntimeridianTolerancePercent = 0.01; + +static void assertWithinPercent(double expected, double actual, double pct, const char *msg) +{ + if (expected < 1.0) { + if (std::fabs(actual - expected) > kNearZeroAbsoluteToleranceMeters) { + char buf[160]; + snprintf(buf, sizeof(buf), "%s: expected=%.3f actual=%.3f (near-zero, limit %.1fm absolute)", msg, expected, actual, + kNearZeroAbsoluteToleranceMeters); + TEST_FAIL_MESSAGE(buf); + } + return; + } + double err = std::fabs(actual - expected) / expected * 100.0; + if (err > pct) { + char buf[160]; + snprintf(buf, sizeof(buf), "%s: expected=%.1f actual=%.1f err=%.2f%% (limit %.2f%%)", msg, expected, actual, err, pct); + TEST_FAIL_MESSAGE(buf); + } +} + +static void test_identical_points_is_zero(void) +{ + TEST_ASSERT_EQUAL_FLOAT(0.0f, GeoCoord::latLongToMeter(51.5, -0.1, 51.5, -0.1)); +} + +static void test_local_distances(void) +{ + // Movement-threshold scale (meters to a few km) - the most common real usage. + struct { + double la, lo, lb, lob; + } cases[] = { + {51.5074, -0.1278, 51.5080, -0.1278}, // ~67m north + {51.5074, -0.1278, 51.5074, -0.1200}, // ~540m east at London's latitude + {0.0, 0.0, 0.001, 0.001}, // ~157m near the equator + {65.0, 25.0, 65.001, 25.002}, // high-ish latitude, small delta + {-33.87, 151.21, -33.865, 151.215}, // Sydney, southern hemisphere + }; + for (auto &c : cases) { + double expected = referenceSphericalLawOfCosines(c.la, c.lo, c.lb, c.lob); + double actual = GeoCoord::latLongToMeter(c.la, c.lo, c.lb, c.lob); + assertWithinPercent(expected, actual, kLocalTolerancePercent, "local distance"); + } +} + +static void test_regional_distances(void) +{ + // City-to-city scale (tens to ~500km) below 60 degrees; see test_high_latitude_distances. + struct { + double la, lo, lb, lob; + } cases[] = { + {51.5074, -0.1278, 48.8566, 2.3522}, // London to Paris, ~344km + {40.7128, -74.0060, 42.3601, -71.0589}, // NYC to Boston, ~306km + {35.6762, 139.6503, 34.6937, 135.5023}, // Tokyo to Osaka, ~400km + {-33.8688, 151.2093, -37.8136, 144.9631}, // Sydney to Melbourne, ~714km + }; + for (auto &c : cases) { + double expected = referenceSphericalLawOfCosines(c.la, c.lo, c.lb, c.lob); + double actual = GeoCoord::latLongToMeter(c.la, c.lo, c.lb, c.lob); + assertWithinPercent(expected, actual, kRegionalTolerancePercent, "regional distance"); + } +} + +static void test_high_latitude_distances(void) +{ + // Regional scale above 60 degrees, where equirectangular error grows fastest - a 500km pair at + // 80 degrees already exceeds 1%. + struct { + double la, lo, lb, lob; + } cases[] = { + {69.6492, 18.9553, 67.2804, 14.4049}, // Tromso to Bodo, ~322km + {64.8378, -147.7164, 61.2181, -149.9003}, // Fairbanks to Anchorage, ~417km + {78.2232, 15.6469, 78.9230, 11.9219}, // Longyearbyen to Ny-Alesund, ~113km + }; + for (auto &c : cases) { + double expected = referenceSphericalLawOfCosines(c.la, c.lo, c.lb, c.lob); + double actual = GeoCoord::latLongToMeter(c.la, c.lo, c.lb, c.lob); + assertWithinPercent(expected, actual, kHighLatitudeTolerancePercent, "high-latitude distance"); + } +} + +static void test_antimeridian_wraparound(void) +{ + // Two points ~22km apart straddling the 180th meridian - regression case for the antimeridian + // wraparound fix (a naive b2-a2 would compute this as ~40,000km). + double expected = referenceSphericalLawOfCosines(0.0, 179.9, 0.0, -179.9); + double actual = GeoCoord::latLongToMeter(0.0, 179.9, 0.0, -179.9); + assertWithinPercent(expected, actual, kAntimeridianTolerancePercent, "antimeridian distance"); + TEST_ASSERT_LESS_THAN_FLOAT(1000000.0f, actual); // sanity: nowhere near the naive-bug's ~40,000km +} + +static void test_symmetry(void) +{ + // distance(a,b) should equal distance(b,a) + double d1 = GeoCoord::latLongToMeter(51.5074, -0.1278, 48.8566, 2.3522); + double d2 = GeoCoord::latLongToMeter(48.8566, 2.3522, 51.5074, -0.1278); + TEST_ASSERT_FLOAT_WITHIN(0.01f, d1, d2); +} + +static void test_no_nan_at_extreme_latitudes(void) +{ + float d1 = GeoCoord::latLongToMeter(90.0, 0.0, -90.0, 0.0); + float d2 = GeoCoord::latLongToMeter(89.9, 10.0, 89.9, -170.0); + float d3 = GeoCoord::latLongToMeter(-89.9, 45.0, -89.9, -135.0); + TEST_ASSERT_FALSE(std::isnan(d1)); + TEST_ASSERT_FALSE(std::isnan(d2)); + TEST_ASSERT_FALSE(std::isnan(d3)); + TEST_ASSERT_TRUE(d1 > 0); +} + +void setup() +{ + UNITY_BEGIN(); + RUN_TEST(test_identical_points_is_zero); + RUN_TEST(test_local_distances); + RUN_TEST(test_regional_distances); + RUN_TEST(test_high_latitude_distances); + RUN_TEST(test_antimeridian_wraparound); + RUN_TEST(test_symmetry); + RUN_TEST(test_no_nan_at_extreme_latitudes); + exit(UNITY_END()); +} + +void loop() {}