I'm trying to calculate the true course from one point to anoter on the surface of the earth in as few CPU cycles as possible. The result should be a double 0 <= tc < 360, however in a few special cases i get the result 360 (should be reported as 0). I realize that this is due to machine precision when working with fmod and floating point number, but what will be the most efficient workaround of the problem?
#include <stdio.h>
#include <math.h>
#define EPS 1e-15 // EPS a small number ~ machine precision
#define R2D 57.295779513082320876798154814105 //multiply radian with R2D to get degrees
#define D2R 0.01745329251994329576923690768489 //multiply degrees with D2R to get radians
#define TWO_PI 6.283185307179586476925286766559 //2*Pi
/*----------------------------------------------------------------------------
* Course between points
* We obtain the initial course, tc1, (at point 1) from point 1 to point 2
* by the following. The formula fails if the initial point is a pole. We can
* special case this with as IF statement
----------------------------------------------------------------------------
Implementation
Argument 1: INPUT - Pointer to double containing Latitude of point 1 in degrees
Argument 2: INPUT - Pointer to double containing Longitude of point 1 in degrees
Argument 3: INPUT - Pointer to double containing Latitude of point 2 in degrees
Argument 4: INPUT - Pointer to double containing Longitude of point 2 in degrees
RETURN: Double containing initial course in degrees from point1 to point 2
--------------------------------------------------------------------------*/
double _stdcall CourseInitial (double *lat1, double *lon1, double *lat2, double *lon2)
{
double radLat1 = D2R * *lat1;
double radLon1 = D2R * *lon1;
double radLat2 = D2R * *lat2;
double radLon2 = D2R * *lon2;
double tc = 0;
if (cos(radLat1) < EPS) { // EPS a small number ~ machine precision
if (radLat1 > 0) {
tc = 180; // starting from N pole
} else {
tc = 0; // starting from S pole
}
} else {
// Calculate true course [180, 540]
tc = R2D * atan2(sin(radLon2-radLon1),
cos(radLat1) * tan(radLat2) - sin(radLat1) * cos(radLon2-radLon1)
) + 360;
}
//Return 0 <= true course < 360
return fmod(tc, 360);
}
int main(void)
{
double lat1 = 89;
double lon1 = 17;
double lat2 = 68;
double lon2 = -163;
double tc = 0;
tc = CourseInitial(&lat1, &lon1, &lat2, &lon2);
printf("The course from point 1 to 2 is: %.5f", tc);
return 0;
}
Output:
The course from point 1 to 2 is: 360.00000