From c6af54caef69d02043594e1fadd6d2cdfcfb6ba2 Mon Sep 17 00:00:00 2001 From: rob tillaart Date: Sun, 20 Dec 2015 16:21:14 +0100 Subject: [PATCH] 0.1.01 fastHaverSine distance function --- sketches/fastHaverSine/fastHaverSine.ino | 185 +++++++++++++++++++++++ 1 file changed, 185 insertions(+) create mode 100644 sketches/fastHaverSine/fastHaverSine.ino diff --git a/sketches/fastHaverSine/fastHaverSine.ino b/sketches/fastHaverSine/fastHaverSine.ino new file mode 100644 index 00000000..27239680 --- /dev/null +++ b/sketches/fastHaverSine/fastHaverSine.ino @@ -0,0 +1,185 @@ +// +// FILE: fastHaverSine.ino +// AUTHOR: Rob Tillaart +// VERSION: 0.1.01 +// PURPOSE: fastHaverSine is a faster function than haversine for short distances (<1km). +// DATE: 2015-07-18 +// URL: http://forum.arduino.cc/index.php?topic=336729 +// +// Released to the public domain +// + + +#define NUMTESTS 8 + +float ts[NUMTESTS][4] = { + {50, 5, 51, 5}, + {50, 5, 50, 6}, + {50, 5, 50.1, 5}, + {50, 5, 50, 5.1}, + {50, 5, 50.1, 5.1}, + {50, 5, 50.01, 5.01}, + {50, 5, 50.001, 5.001}, + {50, 5, 50.0001, 5.0001}, +}; + + +uint32_t start; +uint32_t stop; + +void setup() +{ + Serial.begin(115200); + Serial.println("Start fastHaverSine()"); + + start = micros(); + float d = HaverSine(50, 5, 50.01, 5.01); + stop = micros(); + Serial.println("HAVERSINE"); + Serial.print("TIME: "); + Serial.println(stop - start); + Serial.print("DIST: "); + Serial.println(d); + + start = micros(); + d = fastHaverSine(50, 5, 50.01, 5.01); + stop = micros(); + Serial.println("\nFASTHAVERSINE"); + Serial.print("TIME: "); + Serial.println(stop - start); + Serial.print("DIST: "); + Serial.println(d); + + start = micros(); + d = fastHaverSine2(50, 5, 50.01, 5.01); + stop = micros(); + Serial.println("\nFASTHAVERSINE2"); + Serial.print("TIME: "); + Serial.println(stop - start); + Serial.print("DIST: "); + Serial.println(d); + + for (int i = 0; i < NUMTESTS; i++) + { + Serial.print("\ntest: "); + Serial.print(i); + Serial.print("\t\t"); + for (int j = 0; j < 4; j++) + { + Serial.print("\t"); + Serial.print(ts[i][j], 1); + } + Serial.println(); + d = HaverSine(ts[i][0], ts[i][1], ts[i][2], ts[i][3]); + Serial.println(d); + d = fastHaverSine(ts[i][0], ts[i][1], ts[i][2], ts[i][3]); + Serial.println(d); + } + + Serial.println("\nrelative error map test\nLAT/LONG:\t45-50\nSTEPSIZE:\t0.2 degrees"); + float y = 45.0; + while (y <= 50.0) + { + float x = 45.0; + while (x <= 50.0) + { + float d1 = HaverSine(45.0, 45.0, x, y); + float d2 = fastHaverSine(45.0, 45.0, x, y); + float relError = 100.0 * abs(d2 - d1) / d1; + Serial.print(relError, 3); + Serial.print("\t"); + x += 0.2; + } + Serial.println(); + y += 0.2; + } + Serial.println(); + + + Serial.println("\nrelative error map test\nLAT/LONG:\t45-46\nSTEPSIZE:\t0.05 degrees"); + y = 45.0; + while (y <= 46.0) + { + float x = 45.0; + while (x <= 46.0) + { + float d1 = HaverSine(45.0, 45.0, x, y); + float d2 = fastHaverSine(45.0, 45.0, x, y); + float relError = 100.0 * abs(d2 - d1) / d1; + Serial.print(relError, 3); + Serial.print("\t"); + x += 0.05; + } + Serial.println(); + y += 0.05; + } + Serial.println(); + + + Serial.println("\nrelative error map test\nLAT/LONG:\t45-45.1\nSTEPSIZE:\t0.005 degrees"); + y = 45.0; + while (y <= 45.1) + { + float x = 45.0; + while (x <= 45.1) + { + float d1 = HaverSine(45.0, 45.0, x, y); + float d2 = fastHaverSine(45.0, 45.0, x, y); + float relError = 100.0 * abs(d2 - d1) / d1; + Serial.print(relError, 3); + Serial.print("\t"); + x += 0.005; + } + Serial.println(); + y += 0.005; + } + Serial.println(); + + + Serial.println("done..."); +} + +void loop() +{ +} + +double fastHaverSine(double lat1, double long1, double lat2, double long2) +{ + // assume the sphere has circumference 1 on the equator + // to keep the number in same order of magnitude + // helps to keep precision + double dx = lat2 - lat1; + double dy = (long2 - long1) * cos(lat2 * (PI / 180.0)); + double dist = sqrt(dx * dx + dy * dy); + return dist * 111194.93; // correct for earth sizes ;) +} + +double fastHaverSine2(double lat1, double long1, double lat2, double long2) +{ + // assume the sphere has circumference 1 on the equator + // to keep the number in same order of magnitude + // helps to keep precision + double dx = lat2 - lat1; + double dy = (long2 - long1) * cos(lat2 * (PI / 180.0)); + return hypot(dx, dy) * 111194.93; // correct for earth sizes ;) +} + +double HaverSine(double lat1, double lon1, double lat2, double lon2) +{ + double ToRad = PI / 180.0; + double R = 6371000; // radius earth in meter + + double dLat = (lat2 - lat1) * ToRad; + double dLon = (lon2 - lon1) * ToRad; + + double a = sin(dLat / 2) * sin(dLat / 2) + + cos(lat1 * ToRad) * cos(lat2 * ToRad) * + sin(dLon / 2) * sin(dLon / 2); + + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + double d = R * c; + return d; +} + +