File content as of revision 3:af291e8853b1:

#include "gps_utils.h"
#include "mbed.h"

double haversine(double lat1, double lon1, double lat2, double lon2) {
    double sinLat = sin((lat2-lat1)*0.5);
    double sinLon = sin((lon2-lon1)*0.5);
    double a = sinLat*sinLat + cos(lon1)*cos(lon2)*sinLon*sinLon;
    return EARTH_RADIUS_METERS * 2.0 * atan2(sqrt(a),sqrt(1.0-a));
}

double cosines(double lat1, double lon1, double lat2, double lon2) {
    return acos(sin(lat1)*sin(lat2) + cos(lat1)*cos(lat2)*cos(lon2-lon1))*EARTH_RADIUS_METERS;
}

double equirectangular(double lat1, double lon1, double lat2, double lon2) {
    extern Serial usb;
    
    double x = (lon2-lon1)*cos((lat1+lat2)*0.5);
    usb.printf("  equirectangular x: %0.5f\r\n", x);
    double y = (lat2-lat1);
    usb.printf("  equirectangular y: %0.5f\r\n", y);
    
    return sqrt(x*x + y*y) * EARTH_RADIUS_METERS;
}


double startBearing(double lat1, double lon1, double lat2, double lon2) {
    double y = sin(lon2-lon1)*cos(lat2);
    double x = cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(lon2-lon1);
    double b = atan2(y,x);
    return b<0.0 ? (b+TWO_PI) : b;
}

double endBearing(double lat1, double lon1, double lat2, double lon2) {
    double y = sin(lon1-lon2)*cos(lat1);
    double x = cos(lat2)*sin(lat1) - sin(lat2)*cos(lat1)*cos(lon1-lon2);
    double b = atan2(y,x);
    return b<0.0 ? (b+M_PI) : (b-M_PI);
}