File content as of revision 1:9530746906b6:

#define M_PI 3.141592

double deg2rad(double deg) {
  return deg * (M_PI/180.0);
}


double getDistance(double lat1,double lon1,double lat2,double lon2) {
  double R = 6371; // Radius of the earth in km
  double dLat = deg2rad(lat2-lat1);  // deg2rad below
  double dLon = deg2rad(lon2-lon1); 
  double a = 
    sin(dLat/2) * sin(dLat/2) +
    cos(deg2rad(lat1)) * cos(deg2rad(lat2)) * 
    sin(dLon/2) * sin(dLon/2)
    ; 
  double c = 2 * atan2(sqrt(a), sqrt(1-a)); 
  double d = R * c; // Distance in km
  d = d * 1000;     // Distance in meter
  return d;
}

double MIN_MAX(double input, double min, double max){
    double output;
    if( input < min) output = min;
    else if(input > max) output = max;
    else output = input;
    
    return output;       
}


float bearing(float lat1,float lon1,float lat2,float lon2){

    float teta1 = (lat1)*M_PI/180.0;
    float teta2 = (lat2)*M_PI/180.0;;
//    float delta1 = (lat2-lat1)*M_PI/180.0;;
    float delta2 = (lon2-lon1)*M_PI/180.0;;

    //==================Heading Formula Calculation================//

    float y = sin(delta2) * cos(teta2);
    float x = cos(teta1)*sin(teta2) - sin(teta1)*cos(teta2)*cos(delta2);
    float brng = atan2(y,x);
    brng = (brng)*180.0/M_PI;// radians to degrees
    brng = ( ((int)brng + 360) % 360 ); 
//    if(brng > 180) brng -= 360;
    if(brng > 360) brng -= 360;
    else if(brng < 0) brng += 360;
    return brng;
}

float ABSOL(float input){
    float output;
    if( input > 0){
        output = input;
    }else{
        output = -input;
    }
    return output;
}