===== Coordinates to distance and bearing and vice versa =====
A C library that does various conversions between distances, bearings and coordinates.
coordinates.c:
/***************************************************************************
* Copyright (C) 2006 the `High Altitude Slug Project' *
* *
* Permission is hereby granted, free of charge, to any person obtaining *
* a copy of this software and associated documentation files (the *
* "Software"), to deal in the Software without restriction, including *
* without limitation the rights to use, copy, modify, merge, publish, *
* distribute, sublicense, and/or sell copies of the Software, and to *
* permit persons to whom the Software is furnished to do so, subject to *
* the following conditions: *
* *
* The above copyright notice and this permission notice shall be *
* included in all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, *
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF *
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*
* IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR *
* OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, *
* ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR *
* OTHER DEALINGS IN THE SOFTWARE. *
***************************************************************************/
/*
* Coordinate calculations
* $Id: coordinates.c 28 2006-07-14 00:30:03Z ben $
*/
#include
#include
#include
#include "coordinates.h"
/* interesting stuff here:
* http://williams.best.vwh.net/avform.htm */
/*
* Constants
*/
/* a, b = major & minor semiaxes of the ellipsoid */
#define VIN_A (long long) 6378137
#define VIN_B (long long) 6356752.3142
/* f = flattening (a - b) / a */
#define VIN_F (1 / 298.257223563)
/* earth's mean radius in km */
#define EARTH_RADIUS 6371
/*
* Function-like macros
*/
#define deg_to_rad(a) (a * M_PI / 180)
#define rad_to_deg(a) (a * 180 / M_PI)
/**
* Finds the distance between two sets of coordinates
*
* @param coords1 first coordinates
* @param coords2 second coordinates
* @return distance in metres
*/
double coords_to_distance(struct coordinates coords1, struct coordinates coords2)
{
/*
*Vincenty Formula
* http://www.movable-type.co.uk/scripts/LatLongVincenty.html
* far too complicated, I love it ;)
* seriously though - maybe we should use the simpler haversine formula
* for speed
*/
if (coords1.lat == coords2.lat && coords1.lon == coords2.lon)
return 0;
coords1.lat = deg_to_rad(coords1.lat);
coords1.lon = deg_to_rad(coords1.lon);
coords2.lat = deg_to_rad(coords2.lat);
coords2.lon = deg_to_rad(coords2.lon);
double L = coords2.lon - coords1.lon;
double U1 = atan((1 - VIN_F) * tan(coords1.lat));
double U2 = atan((1 - VIN_F) * tan(coords2.lat));
double sinU1 = sin(U1);
double cosU1 = cos(U1);
double sinU2 = sin(U2);
double cosU2 = cos(U2);
double lambda = L;
double lambdaP = 2 * M_PI;
double iterLimit = 20;
double sinLambda, cosLambda;
double sinSigma, cosSigma;
double sigma, alpha;
double cosSqAlpha, cos2SigmaM, C;
while (abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0)
{
sinLambda = sin(lambda);
cosLambda = cos(lambda);
sinSigma = sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) +
(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2-sinU1 * cosU2 * cosLambda));
cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
sigma = atan2(sinSigma, cosSigma);
alpha = asin(cosU1 * cosU2 * sinLambda / sinSigma);
cosSqAlpha = cos(alpha) * cos(alpha);
cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
C = VIN_F / 16 * cosSqAlpha * (4 + VIN_F * (4 - 3 * cosSqAlpha));
lambdaP = lambda;
lambda = L + (1 - C) * VIN_F * sin(alpha) * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
}
if (iterLimit == 0)
return 0; /* formula failed to converge */
double uSq = cosSqAlpha * (VIN_A * VIN_A - VIN_B * VIN_B) / (VIN_B * VIN_B);
double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 *uSq)));
double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM*(- 3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
double s = VIN_B * A * (sigma - deltaSigma);
return s;
}
/**
* Finds the bearing between two sets of coordinates
*
* @param coords1 first coordinates
* @param coords2 second coordinates
* @return bearing in degrees
*/
double coords_to_bearing(struct coordinates coords1, struct coordinates coords2)
{
coords1.lat = deg_to_rad(coords1.lat);
coords1.lon = deg_to_rad(coords1.lon);
coords2.lat = deg_to_rad(coords2.lat);
coords2.lon = deg_to_rad(coords2.lon);
double dlat = coords2.lat - coords1.lat; /* delta */
double dlon = coords2.lon - coords1.lon; /* delta */
double bearing = atan2((sin(dlon) * cos(coords2.lat)) , (cos(coords1.lat) * sin(coords2.lat) - sin(coords1.lat) * cos(coords2.lat) * cos(dlon)));
bearing = rad_to_deg(bearing);
if (bearing < 0)
bearing += 360;
return bearing;
}
/**
* Finds the coordinates of a second point, given the first point
* and the distance and bearing to the second point.
*
* @param coords first point coordinates
* @param distance distance to second point in metres
* @param bearing bearing to second point in degrees
* @return coordinates of second point
*/
struct coordinates distance_to_coords (struct coordinates coords1, double distance, double bearing)
{
double d = distance / (EARTH_RADIUS * 1000); /* d = angular distance covered on earth's surface in radians */
struct coordinates coords2;
double rad_bearing = deg_to_rad(bearing);
coords1.lat = deg_to_rad(coords1.lat);
coords1.lon = deg_to_rad(coords1.lon);
/* http://williams.best.vwh.net/avform.htm#LL */
coords2.lat = asin( sin(coords1.lat) * cos(d) + cos(coords1.lat) * sin(d) * cos(rad_bearing) );
coords2.lon = coords1.lon + atan2( sin(rad_bearing) * sin(d) * cos(coords1.lat), cos(d) - sin(coords1.lat) * sin(coords2.lat));
coords2.lat = rad_to_deg(coords2.lat);
coords2.lon = rad_to_deg(coords2.lon);
return coords2;
}
coordinates.h:
/***************************************************************************
* Copyright (C) 2006 the "High Altitude Slug Project" *
* *
* Permission is hereby granted, free of charge, to any person obtaining *
* a copy of this software and associated documentation files (the *
* "Software"), to deal in the Software without restriction, including *
* without limitation the rights to use, copy, modify, merge, publish, *
* distribute, sublicense, and/or sell copies of the Software, and to *
* permit persons to whom the Software is furnished to do so, subject to *
* the following conditions: *
* *
* The above copyright notice and this permission notice shall be *
* included in all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, *
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF *
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*
* IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR *
* OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, *
* ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR *
* OTHER DEALINGS IN THE SOFTWARE. *
***************************************************************************/
/*
* Coordinate calculations
* $Id: coordinates.h 27 2006-07-13 15:31:59Z ben $
*/
struct coordinates
{
double lat;
double lon;
};
double coords_to_distance(struct coordinates coords1, struct coordinates coords2);
double coords_to_bearing(struct coordinates coords1, struct coordinates coords2);
struct coordinates distance_to_coords (struct coordinates coords, double distance, double bearing);