GY-63_MS5611/sketches/fastHaverSine/fastHaverSine.ino
2015-12-20 16:21:14 +01:00

186 lines
4.3 KiB
C++

//
// 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;
}