UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools


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 (external edit)