code:navigation
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 <stdio.h> #include <stdlib.h> #include <math.h> #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);
code/navigation.txt · Last modified: 2008/07/19 23:33 by 127.0.0.1