### Highlights from Vectorized geodetic distance and azimuth on the WGS84 earth ellipsoid

5.0
5.0 | 3 ratings Rate this file 21 Downloads (last 30 days) File Size: 8.51 KB File ID: #8607 Version: 1.0

# Vectorized geodetic distance and azimuth on the WGS84 earth ellipsoid

### Michael Kleder (view profile)

30 Sep 2005 (Updated )

Geodetic distance, forward azimuth, and return azimuth between coordinates

File Information
Description

Vector and matrix inputs are permitted, and forward and backward azimuth calculations are optionally output.

Description: In 1975, Vincenty published a rapidly converging algorithm for computing the distance between points on an ellipsoidal earth. The algorithm is precise to within a few millimeters. Since then, his algorithm has since seen significant implementation in geodesy and engineering. After adjusting the algorithm to converge in all cases (the original suffers from convergence failure in a few outlying cases), resolving the azimuth quadrant ambiguity present in the original, and vectorizing, I have provided it here in MATLAB form. The function itself does not require the Mapping Toolbox, but I have provided comparisons to that Toolbox in the "help" comments. This function will provide rapid, extremely precise results. Please see code comments for references.

To the many users who downloaded an earlier implemetation of this algorithm, without vectorized code and without azimuth calculations: thank you for your comments, and for your patience.

Michael Kleder, Sep 2005

Acknowledgements
MATLAB release MATLAB 7 (R14)
16 Jun 2013 Charles Karney

### Charles Karney (view profile)

You might be interested in the geodist function in
http://www.mathworks.com/matlabcentral/fileexchange/39108. This offers the same functionality as vdist without the convergence problems of Vincenty's algorithm.

Comment only
22 Aug 2008 David Erdos

Excellent work! This is exactly what I needed!

Thanks!

28 Nov 2006 John Lansberry

I haven't checked you code, but I wonder how it might compare to the NOAA code that I converted to an m-file and vectorized.

function [s_out, faz_out, baz_out] = wgs84dist(varargin)
%------------------------------------------------------------------------------
% function [s, faz, baz] = wgs84dist(lat1, lon1, lat2, lon2 [, deg_in, deg_out])
%
% This code is part of the geodetic tool kit, which can be downloaded
% from the National Oceanic and Atmospheric Administration (NOAA)
% National Geodetic Survey (ngs) website at:
% http://www.ngs.noaa.gov/index.shtml.
%
% input description
%
% lat1 initial latitude (rad or deg)
% lon1 initial longitude (rad or deg)
% lat2 final latitude (rad or deg)
% lon2 final longitude (rad or deg)
% deg_in selects whether angular inputs are in degrees or
% radians (deg_in = 1 for degrees) (optional)
% deg_out selects whether angular outputs are in degrees or
% radians (deg_out = 1 for degrees) (optional)
%
% output description
%
% s surface distance along ellipsoid (m)
% faz forward azimuth (rad or deg) (optional)
% baz backward azimuth (rad or deg) (optional)
%------------------------------------------------------------------------------

%------------------------------------------------------------------------------
%
% Solution of the geodetic direct problem after T.Vincenty
% modified Rainsford's method with Helmert's elliptical terms
% effective in any azimuth and at any distance short of antipodal
% standpoint/forepoint must not be the geographic pole
%
% a is the semi-major axis of the reference ellipsoid
% f is the flattening of the reference ellipsoid
% latitudes and longitudes in radians positive north and east
% forward azimuths at both points returned in radians from north
%
% programmed for cdc-6600 by LCDR L.Pfeifer NGS Rockville MD 18FEB75
% modified for system 360 by John G Gergen NGS Rockville MD 7507
%------------------------------------------------------------------------------

%-------------------------------------------------------------------------------
% check input arguments
%-------------------------------------------------------------------------------
error(nargchk(4, 6, nargin));

%-------------------------------------------------------------------------------
% check for input errors (e.g., inputs have incorrect number of rows or columns)
%-------------------------------------------------------------------------------
rows = zeros(1,nargin);
cols = zeros(1,nargin);

for ii=1:nargin,
[rows(ii), cols(ii)] = size(varargin{ii});
end

maxrows = max(rows);

% make sure each input has either 1 row or n rows
inputerr = any(rows ~= 1 & rows ~= maxrows);

if inputerr,
error('inputs must have either 1 row or n rows')
end

% all inputs must have 1 column
inputerr = any(cols ~= 1);

if inputerr
error('all inputs must have one column');
end

%-------------------------------------------------------------------------------
% expand inputs as necessary
%-------------------------------------------------------------------------------
expand = any(rows == 1) & (maxrows > 1);

if expand,
for ii=1:nargin,
if rows(ii) == 1,
varargin{ii} = repmat(varargin{ii}, maxrows, 1);
end
end
end

%------------------------------------------------------------------------------
% extract inputs from varargin
%------------------------------------------------------------------------------
lat1 = varargin{1};
lon1 = varargin{2};
lat2 = varargin{3};
lon2 = varargin{4};

deg_in = zeros(maxrows, 1);
deg_out = zeros(maxrows, 1);

if nargin >= 5,
deg_in = varargin{5};
if nargin == 6,
deg_out = varargin{6};
end
end

%------------------------------------------------------------------------------
% WGS-84 defining parameters.
%------------------------------------------------------------------------------
a = 6378137.0;
f = 1.0 / 298.257223563;

%------------------------------------------------------------------------------
% Miscellaneous parameters.
%------------------------------------------------------------------------------

zero = 0.0; one = 1.0; two = 2.0;three = 3.0; four = 4.0; six = 6.0;
three_eighths = 3.0 / 8.0; sixteen = 16.0;

%------------------------------------------------------------------------------
% Input conversions.
%------------------------------------------------------------------------------
lat1(deg_in == 1) = lat1(deg_in == 1) * deg2rad;
lon1(deg_in == 1) = lon1(deg_in == 1) * deg2rad;
lat2(deg_in == 1) = lat2(deg_in == 1) * deg2rad;
lon2(deg_in == 1) = lon2(deg_in == 1) * deg2rad;

%------------------------------------------------------------------------------
% Find lat/lon pairs that are identical - remove them from the calculations.
% The surface distance, forward azimuth and backward azimuth for these points
% will be set to zero.
%------------------------------------------------------------------------------
equal = ( abs(lat1 - lat2) < eps & abs(lon1 - lon2) < eps);

s_out = zeros(size(lat1));
faz_out = zeros(size(lat1));
baz_out = zeros(size(lat1));

lat1 = lat1(~equal);
lon1 = lon1(~equal);
lat2 = lat2(~equal);
lon2 = lon2(~equal);

%------------------------------------------------------------------------------
% Main routine.
%------------------------------------------------------------------------------
eps0 = 0.5e-13;
r = one - f;
tu1 = r * sin(lat1) ./ cos(lat1);
tu2 = r * sin(lat2) ./ cos(lat2);
cu1 = one ./ sqrt(tu1 .* tu1 + one);
su1 = cu1 .* tu1;
cu2 = one ./ sqrt(tu2 .* tu2 + one);
s = cu1 .* cu2;
baz = s .* tu2;
faz = baz .* tu1;
x = lon2 - lon1;

repeat = 1;

while repeat == 1,

sx = sin(x);
cx = cos(x);
tu1 = cu2 .* sx;
tu2 = baz - su1 .* cu2 .* cx;
sy = sqrt(tu1 .* tu1 + tu2 .* tu2);
cy = s .* cx + faz;
y = atan2(sy, cy);
sa = s .* sx ./ sy;
c2a = -sa .* sa + one;
cz = faz + faz;

ii = c2a > 0;

cz(ii) = -cz(ii) ./ c2a(ii) + cy(ii);

e = cz .* cz * two - one;
c = ((-three * c2a + four) * f + four) .* c2a * f / sixteen;
d = x;
x = ((e .* cy .* c + cz) .* sy .* c + y) .* sa;
x = (one - c) .* x * f + lon2 - lon1;

if all(abs(d-x) <= eps0),
break
end

end

faz = atan2(tu1,tu2);
baz = atan2(cu1 .* sx, baz .* cx - su1 .* cu2) + pi;
x = sqrt((one / r / r - one) .* c2a + one) + one;
x = (x - two) ./ x;
c = one - x;
c = (x .* x / four + one) ./ c;
d = (three_eighths * x .* x - one) .* x;
x = e .* cy;
s = one - e - e;
s = ((((sy .* sy * four - three) .* s .* cz .* (d / six) - x) .* ...
(d / four) + cz) .* sy .* d + y) .* c * a * r;

%------------------------------------------------------------------------------
% Map to outputs.
%------------------------------------------------------------------------------
s_out(~equal) = s;
faz_out(~equal) = faz;
baz_out(~equal) = baz;

%------------------------------------------------------------------------------
% Output conversions.
%------------------------------------------------------------------------------
faz_out(deg_out == 1) = faz_out(deg_out == 1) * rad2deg;
baz_out(deg_out == 1) = baz_out(deg_out == 1) * rad2deg;

if nargout == 1,
s_out = [s_out faz_out baz_out];
end

Comment only
10 Oct 2005 Gabriel Ruiz

Very useful. I was checking the output of this function with a GPS MobileMapper Pro, if we take the accurancy of the GPS device, I think this routine has a good approximation.
So, congratulations!

05 Oct 2005 Gerald Dalley

Nice update from the non-vectorized version.