1 | function [s,a12,a21] = m_idist(lon1,lat1,lon2,lat2,spheroid)
|
---|
2 | % M_IDIST - On an ellipsoidal earth, compute the distance between
|
---|
3 | % two points within a few millimeters of accuracy, compute forward
|
---|
4 | % azimuth, and compute backward azimuth, all using a vectorized
|
---|
5 | % version of Vincenty's algorithm.
|
---|
6 | %
|
---|
7 | % [s,a12,a21] = m_idist(lon1,lat1,lon2,lat2,spheroid)
|
---|
8 | %
|
---|
9 | % lat1 = GEODETIC latitude of first point (degrees)
|
---|
10 | % lon1 = longitude of first point (degrees)
|
---|
11 | % lat2, lon2 = second point (degrees)
|
---|
12 | % spheroid = (Optional) spheroid, defaults to 'wgs84'
|
---|
13 | % s = distance in meters
|
---|
14 | % a12 = azimuth in degrees from first point to second point (forward)
|
---|
15 | % a21 = azimuth in degrees from second point to first point (backward)
|
---|
16 | % (Azimuths are in degrees clockwise from north.)
|
---|
17 | %
|
---|
18 | % Inputs can be all the same size, or a mix of scalars and matrices.
|
---|
19 | % If a mixture, inputs are expanded as required.
|
---|
20 | %
|
---|
21 | % Original algorithm source:
|
---|
22 | % T. Vincenty, "Direct and Inverse Solutions of Geodesics on the Ellipsoid
|
---|
23 | % with Application of Nested Equations", Survey Review, vol. 23, no. 176,
|
---|
24 | % April 1975, pp 88-93.
|
---|
25 | % Available at: http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
|
---|
26 | %
|
---|
27 | % Code slightly modified version of vdist.m (M. Kleder) available
|
---|
28 | % at user-contributed area of www.mathworks.com
|
---|
29 | %
|
---|
30 | % See also M_FDIST, M_GEODESIC, M_LLDIST
|
---|
31 |
|
---|
32 | % (Original notes)
|
---|
33 | % Notes: (1) lat1,lon1,lat2,lon2 can be any (identical) size/shape. Outputs
|
---|
34 | % will have the same size and shape.
|
---|
35 | % (2) Error correcting code, convergence failure traps, antipodal
|
---|
36 | % corrections, polar error corrections, WGS84 ellipsoid
|
---|
37 | % parameters, testing, and comments: Michael Kleder, 2004.
|
---|
38 | % (3) Azimuth implementation (including quadrant abiguity
|
---|
39 | % resolution) and code vectorization, Michael Kleder, Sep 2005.
|
---|
40 | % (4) Vectorization is convergence sensitive; that is, quantities
|
---|
41 | % which have already converged to within tolerance are not
|
---|
42 | % recomputed during subsequent iterations (while other
|
---|
43 | % quantities are still converging).
|
---|
44 | % (5) Vincenty describes his distance algorithm as precise to within
|
---|
45 | % 0.01 millimeters, subject to the ellipsoidal model.
|
---|
46 | % (6) For distance calculations, essentially antipodal points are
|
---|
47 | % treated as exactly antipodal, potentially reducing accuracy
|
---|
48 | % slightly.
|
---|
49 | % (7) Distance failures for points exactly at the poles are
|
---|
50 | % eliminated by moving the points by 0.6 millimeters.
|
---|
51 | % (8) The Vincenty distance algorithm was transcribed verbatim by
|
---|
52 | % Peter Cederholm, August 12, 2003. It was modified and
|
---|
53 | % translated to English by Michael Kleder.
|
---|
54 | % Mr. Cederholm's website is http://www.plan.aau.dk/~pce/
|
---|
55 | % (9) Distances agree with the Mapping Toolbox, version 2.2 (R14SP3)
|
---|
56 | % with a max relative difference of about 5e-9, except when the
|
---|
57 | % two points are nearly antipodal, and except when one point is
|
---|
58 | % near the equator and the two longitudes are nearly 180 degrees
|
---|
59 | % apart. This function (vdist) is more accurate in such cases.
|
---|
60 | % For example, note this difference (as of this writing):
|
---|
61 | % >>vdist(0.2,305,15,125)
|
---|
62 | % 18322827.0131551
|
---|
63 | % >>distance(0.2,305,15,125,[6378137 0.08181919])
|
---|
64 | % 0
|
---|
65 | % (10) Azimuths FROM the north pole (either forward starting at the
|
---|
66 | % north pole or backward when ending at the north pole) are set
|
---|
67 | % to 180 degrees by convention. Azimuths FROM the south pole are
|
---|
68 | % set to 0 degrees by convention.
|
---|
69 | % (11) Azimuths agree with the Mapping Toolbox, version 2.2 (R14SP3)
|
---|
70 | % to within about a hundred-thousandth of a degree, except when
|
---|
71 | % traversing to or from a pole, where the convention for this
|
---|
72 | % function is described in (10), and except in the cases noted
|
---|
73 | % above in (9).
|
---|
74 | % (12) No warranties; use at your own risk.
|
---|
75 | %
|
---|
76 | % R. Pawlowicz
|
---|
77 | % 9/jan/2005 - changed name, altered inputs to m_map style (lon first),
|
---|
78 | % added ellipses, many minor simplifications to tighten code.
|
---|
79 | % 9/apr/2009 - distances was NaN if start and end points are the same,
|
---|
80 | % and I have fixed this.
|
---|
81 |
|
---|
82 | pi180=pi/180;
|
---|
83 |
|
---|
84 | MAP_ELLIP = struct ( 'normal', [1.0, 0], ...
|
---|
85 | 'sphere', [6370997.0, 0], ...
|
---|
86 | 'grs80' , [6378137.0, 1/298.257], ...
|
---|
87 | 'grs67' , [6378160.0, 1/247.247], ...
|
---|
88 | 'wgs84' , [6378137.0, 1/298.257223563], ...
|
---|
89 | 'wgs72' , [6378135.0, 1/298.260], ...
|
---|
90 | 'wgs66' , [6378145.0, 1/298.250], ...
|
---|
91 | 'wgs60' , [6378165.0, 1/298.300], ...
|
---|
92 | 'clrk66', [6378206.4, 1/294.980], ...
|
---|
93 | 'clrk80', [6378249.1, 1/293.466], ...
|
---|
94 | 'intl24', [6378388.0, 1/297.000], ...
|
---|
95 | 'intl67', [6378157.5, 1/298.250]);
|
---|
96 |
|
---|
97 |
|
---|
98 | if nargin<5,
|
---|
99 | spheroid='wgs84';
|
---|
100 | end;
|
---|
101 | ellip=getfield(MAP_ELLIP,spheroid);
|
---|
102 | if length(ellip)~=2,
|
---|
103 | disp(MAP_ELLIP);
|
---|
104 | error('Spheroid not chosen from above list');
|
---|
105 | end;
|
---|
106 |
|
---|
107 |
|
---|
108 | % Do the equivalent of meshgrid-type calls to make all
|
---|
109 | % matrix sizes match. To do this we can have no more than
|
---|
110 | % two different numbers in any particular dimension, and one
|
---|
111 | % of these has to be a '1'.
|
---|
112 | allsize=[size(lon1);size(lat1);size(lon2);size(lat2)];
|
---|
113 | i1=ones(1,size(allsize,2));
|
---|
114 | for k=1:size(allsize,2),
|
---|
115 | rs=unique(allsize(:,k));
|
---|
116 | if length(rs)==2 & rs(1)==1,
|
---|
117 | j1=i1;j1(k)=rs(2);
|
---|
118 | if allsize(1,k)==1,lon1=repmat(lon1,j1); end;
|
---|
119 | if allsize(2,k)==1,lat1=repmat(lat1,j1); end;
|
---|
120 | if allsize(3,k)==1,lon2=repmat(lon2,j1); end;
|
---|
121 | if allsize(4,k)==1,lat2=repmat(lat2,j1); end;
|
---|
122 | elseif length(rs)>2,
|
---|
123 | error('incompatible array sizes!');
|
---|
124 | end;
|
---|
125 | end;
|
---|
126 |
|
---|
127 | % reshape inputs
|
---|
128 | keepsize = size(lat1);
|
---|
129 | lat1=lat1(:);
|
---|
130 | lon1=lon1(:);
|
---|
131 | lat2=lat2(:);
|
---|
132 | lon2=lon2(:);
|
---|
133 | % Input check:
|
---|
134 | if any(abs(lat1)>90 | abs(lat2)>90)
|
---|
135 | error('Input latitudes must be between -90 and 90 degrees, inclusive.')
|
---|
136 | end
|
---|
137 |
|
---|
138 |
|
---|
139 |
|
---|
140 |
|
---|
141 | % correct for errors at exact poles by adjusting 0.6 millimeters:
|
---|
142 | kidx = abs(90-abs(lat1)) < 1e-10;
|
---|
143 | if any(kidx);
|
---|
144 | lat1(kidx) = sign(lat1(kidx))*(90-(1e-10));
|
---|
145 | end
|
---|
146 | kidx = abs(90-abs(lat2)) < 1e-10;
|
---|
147 | if any(kidx)
|
---|
148 | lat2(kidx) = sign(lat2(kidx))*(90-(1e-10));
|
---|
149 | end
|
---|
150 |
|
---|
151 | % Algorithm begins here
|
---|
152 |
|
---|
153 | a=ellip(1);
|
---|
154 | b=a*(1-ellip(2));
|
---|
155 | f = (a-b)/a;
|
---|
156 | U1 = atan((1-f)*tan(lat1*pi180));
|
---|
157 | U2 = atan((1-f)*tan(lat2*pi180));
|
---|
158 |
|
---|
159 | % Get longitude difference (short way around)
|
---|
160 | L = abs(mod(lon2,360)-mod(lon1,360))*pi180;
|
---|
161 | kidx = L > pi;
|
---|
162 | if any(kidx)
|
---|
163 | L(kidx) = 2*pi - L(kidx);
|
---|
164 | end
|
---|
165 |
|
---|
166 | lambda = L;
|
---|
167 | lambdaold = zeros(size(lat1));
|
---|
168 | alpha = lambdaold;
|
---|
169 | sinsigma=lambdaold;
|
---|
170 | cossigma=lambdaold;
|
---|
171 | sigma = lambdaold;
|
---|
172 | cos2sigmam = lambdaold;
|
---|
173 | C = lambdaold;
|
---|
174 | k = logical(ones(size(lat1)));
|
---|
175 | itercount = 0;
|
---|
176 | warninggiven = logical(0);
|
---|
177 | while any(k) % force at least one execution
|
---|
178 | itercount = itercount+1;
|
---|
179 | if itercount > 50
|
---|
180 | if ~warninggiven
|
---|
181 | warning(['Essentially antipodal points encountered. ' ...
|
---|
182 | 'Precision may be reduced slightly.']);
|
---|
183 | end
|
---|
184 | lambda(k) = pi;
|
---|
185 | break
|
---|
186 | end
|
---|
187 | % eliminate rare imaginary portions at limit of numerical precision:
|
---|
188 | sinsigma(k) = real( sqrt((cos(U2(k)).*sin(lambda(k))).^2+ ...
|
---|
189 | (cos(U1(k)).*sin(U2(k))-sin(U1(k)).*cos(U2(k)).*cos(lambda(k))).^2) );
|
---|
190 | cossigma(k) = real( sin(U1(k)).*sin(U2(k))+cos(U1(k)).*cos(U2(k)).*cos(lambda(k)) );
|
---|
191 | sigma(k) = atan2(sinsigma(k),cossigma(k));
|
---|
192 | alpha(k) = asin(cos(U1(k)).*cos(U2(k)).*sin(lambda(k))./sinsigma(k));
|
---|
193 | if isnan(alpha(k)), alpha(k)=0; end; % this occurs when points are colocated (RP 9/apr/09)
|
---|
194 | cos2sigmam(k) = cossigma(k)-2*sin(U1(k)).*sin(U2(k))./cos(alpha(k)).^2;
|
---|
195 | C(k) = f/16*cos(alpha(k)).^2.*(4+f*(4-3*cos(alpha(k)).^2));
|
---|
196 | lambdaold(k) = lambda(k);
|
---|
197 | lambda(k) = L(k)+(1-C(k)).*f.*sin(alpha(k)).*(sigma(k)+ ...
|
---|
198 | C(k).*sinsigma(k).*(cos2sigmam(k)+ ...
|
---|
199 | C(k).*cossigma(k).*(-1+2.*cos2sigmam(k).^2)));
|
---|
200 |
|
---|
201 | % correct for convergence failure in the case of essentially antipodal
|
---|
202 | % points
|
---|
203 | if any(lambda(k) > pi)
|
---|
204 | warning(['Essentially antipodal points encountered. ' ...
|
---|
205 | 'Precision may be reduced slightly.']);
|
---|
206 | warninggiven = logical(1);
|
---|
207 | lambdaold(lambda>pi) = pi;
|
---|
208 | lambda(lambda>pi) = pi;
|
---|
209 | end
|
---|
210 | k = abs(lambda-lambdaold) > 1e-12;
|
---|
211 | end
|
---|
212 |
|
---|
213 | u2 = cos(alpha).^2.*(a^2-b^2)/b^2;
|
---|
214 | A = 1+u2./16384.*(4096+u2.*(-768+u2.*(320-175.*u2)));
|
---|
215 | B = u2./1024.*(256+u2.*(-128+u2.*(74-47.*u2)));
|
---|
216 | deltasigma = B.*sin(sigma).*(cos2sigmam+B./4.*(cos(sigma).*(-1+2.*cos2sigmam.^2) ...
|
---|
217 | -B./6.*cos2sigmam.*(-3+4.*sin(sigma).^2).*(-3+4*cos2sigmam.^2)));
|
---|
218 | s = reshape(b.*A.*(sigma-deltasigma),keepsize);
|
---|
219 |
|
---|
220 | if nargout > 1
|
---|
221 | % From point #1 to point #2
|
---|
222 | % correct sign of lambda for azimuth calcs:
|
---|
223 | lambda = abs(lambda);
|
---|
224 | kidx=sign(sin((lon2-lon1)*pi180)) .* sign(sin(lambda)) < 0;
|
---|
225 | lambda(kidx) = -lambda(kidx);
|
---|
226 | numer = cos(U2).*sin(lambda);
|
---|
227 | denom = cos(U1).*sin(U2)-sin(U1).*cos(U2).*cos(lambda);
|
---|
228 | a12 = reshape( mod( atan2(numer,denom)/pi180 , 360 ), keepsize);
|
---|
229 | % from poles:
|
---|
230 | a12(lat1 <= -90) = 0;
|
---|
231 | a12(lat1 >= 90 ) = 180;
|
---|
232 | end
|
---|
233 |
|
---|
234 | if nargout > 2
|
---|
235 | % From point #2 to point #1
|
---|
236 | % correct sign of lambda for azimuth calcs:
|
---|
237 | lambda = abs(lambda);
|
---|
238 | kidx=sign(sin((lon1-lon2)*pi180)) .* sign(sin(lambda)) < 0;
|
---|
239 | lambda(kidx)=-lambda(kidx);
|
---|
240 | numer = cos(U1).*sin(lambda);
|
---|
241 | denom = sin(U1).*cos(U2)-cos(U1).*sin(U2).*cos(lambda);
|
---|
242 | a21 = reshape( mod(atan2(numer,denom)/pi180, 360 ), keepsize);
|
---|
243 | % backwards from poles:
|
---|
244 | a21(lat2 >= 90) = pi;
|
---|
245 | a21(lat2 <= -90) = 0;
|
---|
246 | end
|
---|
247 | return
|
---|
248 |
|
---|