VDIST - Using the WGS-84 Earth ellipsoid, compute the distance between two points within a few millimeters of accuracy, compute forward azimuth, and compute backward azimuth, all using a vectorized version of Vincenty's algorithm. s = vdist(lat1,lon1,lat2,lon2) [s,a12] = vdist(lat1,lon1,lat2,lon2) [s,a12,a21] = vdist(lat1,lon1,lat2,lon2) s = distance in meters (inputs may be scalars, vectors, or matrices) a12 = azimuth in degrees from first point to second point (forward) a21 = azimuth in degrees from second point to first point (backward) (Azimuths are in degrees clockwise from north.) lat1 = GEODETIC latitude of first point (degrees) lon1 = longitude of first point (degrees) lat2, lon2 = second point (degrees) Original algorithm source: T. Vincenty, "Direct and Inverse Solutions of Geodesics on the Ellipsoid with Application of Nested Equations", Survey Review, vol. 23, no. 176, April 1975, pp 88-93. Available at: http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf Notes: (1) lat1,lon1,lat2,lon2 can be any (identical) size/shape. Outputs will have the same size and shape. (2) Error correcting code, convergence failure traps, antipodal corrections, polar error corrections, WGS84 ellipsoid parameters, testing, and comments: Michael Kleder, 2004. (3) Azimuth implementation (including quadrant abiguity resolution) and code vectorization, Michael Kleder, Sep 2005. (4) Vectorization is convergence sensitive; that is, quantities which have already converged to within tolerance are not recomputed during subsequent iterations (while other quantities are still converging). (5) Vincenty describes his distance algorithm as precise to within 0.01 millimeters, subject to the ellipsoidal model. (6) For distance calculations, essentially antipodal points are treated as exactly antipodal, potentially reducing accuracy slightly. (7) Distance failures for points exactly at the poles are eliminated by moving the points by 0.6 millimeters. (8) The Vincenty distance algorithm was transcribed verbatim by Peter Cederholm, August 12, 2003. It was modified and translated to English by Michael Kleder. Mr. Cederholm's website is http://www.plan.aau.dk/~pce/ (9) Distances agree with the Mapping Toolbox, version 2.2 (R14SP3) with a max relative difference of about 5e-9, except when the two points are nearly antipodal, and except when one point is near the equator and the two longitudes are nearly 180 degrees apart. This function (vdist) is more accurate in such cases. For example, note this difference (as of this writing): >>vdist(0.2,305,15,125) 18322827.0131551 >>distance(0.2,305,15,125,[6378137 0.08181919]) 0 (10) Azimuths FROM the north pole (either forward starting at the north pole or backward when ending at the north pole) are set to 180 degrees by convention. Azimuths FROM the south pole are set to 0 degrees by convention. (11) Azimuths agree with the Mapping Toolbox, version 2.2 (R14SP3) to within about a hundred-thousandth of a degree, except when traversing to or from a pole, where the convention for this function is described in (10), and except in the cases noted above in (9). (12) No warranties; use at your own risk.
0001 function varargout = vdist(lat1,lon1,lat2,lon2) 0002 % VDIST - Using the WGS-84 Earth ellipsoid, compute the distance between 0003 % two points within a few millimeters of accuracy, compute forward 0004 % azimuth, and compute backward azimuth, all using a vectorized 0005 % version of Vincenty's algorithm. 0006 % 0007 % s = vdist(lat1,lon1,lat2,lon2) 0008 % [s,a12] = vdist(lat1,lon1,lat2,lon2) 0009 % [s,a12,a21] = vdist(lat1,lon1,lat2,lon2) 0010 % 0011 % s = distance in meters (inputs may be scalars, vectors, or matrices) 0012 % a12 = azimuth in degrees from first point to second point (forward) 0013 % a21 = azimuth in degrees from second point to first point (backward) 0014 % (Azimuths are in degrees clockwise from north.) 0015 % lat1 = GEODETIC latitude of first point (degrees) 0016 % lon1 = longitude of first point (degrees) 0017 % lat2, lon2 = second point (degrees) 0018 % 0019 % Original algorithm source: 0020 % T. Vincenty, "Direct and Inverse Solutions of Geodesics on the Ellipsoid 0021 % with Application of Nested Equations", Survey Review, vol. 23, no. 176, 0022 % April 1975, pp 88-93. 0023 % Available at: http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf 0024 % 0025 % Notes: (1) lat1,lon1,lat2,lon2 can be any (identical) size/shape. Outputs 0026 % will have the same size and shape. 0027 % (2) Error correcting code, convergence failure traps, antipodal 0028 % corrections, polar error corrections, WGS84 ellipsoid 0029 % parameters, testing, and comments: Michael Kleder, 2004. 0030 % (3) Azimuth implementation (including quadrant abiguity 0031 % resolution) and code vectorization, Michael Kleder, Sep 2005. 0032 % (4) Vectorization is convergence sensitive; that is, quantities 0033 % which have already converged to within tolerance are not 0034 % recomputed during subsequent iterations (while other 0035 % quantities are still converging). 0036 % (5) Vincenty describes his distance algorithm as precise to within 0037 % 0.01 millimeters, subject to the ellipsoidal model. 0038 % (6) For distance calculations, essentially antipodal points are 0039 % treated as exactly antipodal, potentially reducing accuracy 0040 % slightly. 0041 % (7) Distance failures for points exactly at the poles are 0042 % eliminated by moving the points by 0.6 millimeters. 0043 % (8) The Vincenty distance algorithm was transcribed verbatim by 0044 % Peter Cederholm, August 12, 2003. It was modified and 0045 % translated to English by Michael Kleder. 0046 % Mr. Cederholm's website is http://www.plan.aau.dk/~pce/ 0047 % (9) Distances agree with the Mapping Toolbox, version 2.2 (R14SP3) 0048 % with a max relative difference of about 5e-9, except when the 0049 % two points are nearly antipodal, and except when one point is 0050 % near the equator and the two longitudes are nearly 180 degrees 0051 % apart. This function (vdist) is more accurate in such cases. 0052 % For example, note this difference (as of this writing): 0053 % >>vdist(0.2,305,15,125) 0054 % 18322827.0131551 0055 % >>distance(0.2,305,15,125,[6378137 0.08181919]) 0056 % 0 0057 % (10) Azimuths FROM the north pole (either forward starting at the 0058 % north pole or backward when ending at the north pole) are set 0059 % to 180 degrees by convention. Azimuths FROM the south pole are 0060 % set to 0 degrees by convention. 0061 % (11) Azimuths agree with the Mapping Toolbox, version 2.2 (R14SP3) 0062 % to within about a hundred-thousandth of a degree, except when 0063 % traversing to or from a pole, where the convention for this 0064 % function is described in (10), and except in the cases noted 0065 % above in (9). 0066 % (12) No warranties; use at your own risk. 0067 0068 % reshape inputs 0069 keepsize = size(lat1); 0070 lat1=lat1(:); 0071 lon1=lon1(:); 0072 lat2=lat2(:); 0073 lon2=lon2(:); 0074 % Input check: 0075 if any(abs(lat1)>90 | abs(lat2)>90) 0076 error('Input latitudes must be between -90 and 90 degrees, inclusive.') 0077 end 0078 % Supply WGS84 earth ellipsoid axis lengths in meters: 0079 a = 6378137; % definitionally 0080 b = 6356752.31424518; % computed from WGS84 earth flattening coefficient 0081 % preserve true input latitudes: 0082 lat1tr = lat1; 0083 lat2tr = lat2; 0084 % convert inputs in degrees to radians: 0085 lat1 = lat1 * 0.0174532925199433; 0086 lon1 = lon1 * 0.0174532925199433; 0087 lat2 = lat2 * 0.0174532925199433; 0088 lon2 = lon2 * 0.0174532925199433; 0089 % correct for errors at exact poles by adjusting 0.6 millimeters: 0090 kidx = abs(pi/2-abs(lat1)) < 1e-10; 0091 if any(kidx) 0092 lat1(kidx) = sign(lat1(kidx))*(pi/2-(1e-10)); 0093 end 0094 kidx = abs(pi/2-abs(lat2)) < 1e-10; 0095 if any(kidx) 0096 lat2(kidx) = sign(lat2(kidx))*(pi/2-(1e-10)); 0097 end 0098 f = (a-b)/a; 0099 U1 = atan((1-f)*tan(lat1)); 0100 U2 = atan((1-f)*tan(lat2)); 0101 lon1 = mod(lon1,2*pi); 0102 lon2 = mod(lon2,2*pi); 0103 L = abs(lon2-lon1); 0104 kidx = L > pi; 0105 if any(kidx) 0106 L(kidx) = 2*pi - L(kidx); 0107 end 0108 lambda = L; 0109 lambdaold = 0*lat1; 0110 itercount = 0; 0111 notdone = logical(1+0*lat1); 0112 alpha = 0*lat1; 0113 sigma = 0*lat1; 0114 cos2sigmam = 0*lat1; 0115 C = 0*lat1; 0116 warninggiven = false; 0117 while any(notdone) % force at least one execution 0118 %disp(['lambda(21752) = ' num2str(lambda(21752),20)]); 0119 itercount = itercount+1; 0120 if itercount > 50 0121 if ~warninggiven 0122 warning(['Essentially antipodal points encountered. ' ... 0123 'Precision may be reduced slightly.']); 0124 end 0125 lambda(notdone) = pi; 0126 break 0127 end 0128 lambdaold(notdone) = lambda(notdone); 0129 sinsigma(notdone) = sqrt((cos(U2(notdone)).*sin(lambda(notdone)))... 0130 .^2+(cos(U1(notdone)).*sin(U2(notdone))-sin(U1(notdone)).*... 0131 cos(U2(notdone)).*cos(lambda(notdone))).^2); 0132 cossigma(notdone) = sin(U1(notdone)).*sin(U2(notdone))+... 0133 cos(U1(notdone)).*cos(U2(notdone)).*cos(lambda(notdone)); 0134 % eliminate rare imaginary portions at limit of numerical precision: 0135 sinsigma(notdone)=real(sinsigma(notdone)); 0136 cossigma(notdone)=real(cossigma(notdone)); 0137 sigma(notdone) = atan2(sinsigma(notdone),cossigma(notdone)); 0138 alpha(notdone) = asin(cos(U1(notdone)).*cos(U2(notdone)).*... 0139 sin(lambda(notdone))./sin(sigma(notdone))); 0140 cos2sigmam(notdone) = cos(sigma(notdone))-2*sin(U1(notdone)).*... 0141 sin(U2(notdone))./cos(alpha(notdone)).^2; 0142 C(notdone) = f/16*cos(alpha(notdone)).^2.*(4+f*(4-3*... 0143 cos(alpha(notdone)).^2)); 0144 lambda(notdone) = L(notdone)+(1-C(notdone)).*f.*sin(alpha(notdone))... 0145 .*(sigma(notdone)+C(notdone).*sin(sigma(notdone)).*... 0146 (cos2sigmam(notdone)+C(notdone).*cos(sigma(notdone)).*... 0147 (-1+2.*cos2sigmam(notdone).^2))); 0148 %disp(['then, lambda(21752) = ' num2str(lambda(21752),20)]); 0149 % correct for convergence failure in the case of essentially antipodal 0150 % points 0151 if any(lambda(notdone) > pi) 0152 warning(['Essentially antipodal points encountered. ' ... 0153 'Precision may be reduced slightly.']); 0154 warninggiven = true; 0155 lambdaold(lambda>pi) = pi; 0156 lambda(lambda>pi) = pi; 0157 end 0158 notdone = abs(lambda-lambdaold) > 1e-12; 0159 end 0160 u2 = cos(alpha).^2.*(a^2-b^2)/b^2; 0161 A = 1+u2./16384.*(4096+u2.*(-768+u2.*(320-175.*u2))); 0162 B = u2./1024.*(256+u2.*(-128+u2.*(74-47.*u2))); 0163 deltasigma = B.*sin(sigma).*(cos2sigmam+B./4.*(cos(sigma).*(-1+2.*... 0164 cos2sigmam.^2)-B./6.*cos2sigmam.*(-3+4.*sin(sigma).^2).*(-3+4*... 0165 cos2sigmam.^2))); 0166 varargout{1} = reshape(b.*A.*(sigma-deltasigma),keepsize); 0167 if nargout > 1 0168 % From point #1 to point #2 0169 % correct sign of lambda for azimuth calcs: 0170 lambda = abs(lambda); 0171 kidx=sign(sin(lon2-lon1)) .* sign(sin(lambda)) < 0; 0172 lambda(kidx) = -lambda(kidx); 0173 numer = cos(U2).*sin(lambda); 0174 denom = cos(U1).*sin(U2)-sin(U1).*cos(U2).*cos(lambda); 0175 a12 = atan2(numer,denom); 0176 kidx = a12<0; 0177 a12(kidx)=a12(kidx)+2*pi; 0178 % from poles: 0179 a12(lat1tr <= -90) = 0; 0180 a12(lat1tr >= 90 ) = pi; 0181 varargout{2} = reshape(rad2deg(a12),keepsize); % to degrees 0182 end 0183 if nargout > 2 0184 a21=NaN*lat1; 0185 % From point #2 to point #1 0186 % correct sign of lambda for azimuth calcs: 0187 lambda = abs(lambda); 0188 kidx=sign(sin(lon1-lon2)) .* sign(sin(lambda)) < 0; 0189 lambda(kidx)=-lambda(kidx); 0190 numer = cos(U1).*sin(lambda); 0191 denom = sin(U1).*cos(U2)-cos(U1).*sin(U2).*cos(lambda); 0192 a21 = atan2(numer,denom); 0193 kidx=a21<0; 0194 a21(kidx)= a21(kidx)+2*pi; 0195 % backwards from poles: 0196 a21(lat2tr >= 90) = pi; 0197 a21(lat2tr <= -90) = 0; 0198 varargout{3} = reshape(rad2deg(a21),keepsize); % to degrees 0199 end 0200 0201 end % function