1function [lat2, lon2, azi2, S12, m12, M12, M21, a12_s12] = geodreckon ...
2      (lat1, lon1, s12_a12, azi1, ellipsoid, flags)
3%GEODRECKON  Point at specified azimuth, range on an ellipsoid
4%
5%   [lat2, lon2, azi2] = GEODRECKON(lat1, lon1, s12, azi1)
6%   [lat2, lon2, azi2, S12, m12, M12, M21, a12_s12] =
7%     GEODRECKON(lat1, lon1, s12_a12, azi1, ellipsoid, flags)
8%
9%   solves the direct geodesic problem of finding the final point and
10%   azimuth given lat1, lon1, s12, and azi1.  The input arguments lat1,
11%   lon1, s12, azi1, can be scalars or arrays of equal size.  lat1, lon1,
12%   azi1 are given in degrees and s12 in meters.  The ellipsoid vector is
13%   of the form [a, e], where a is the equatorial radius in meters, e is
14%   the eccentricity.  If ellipsoid is omitted, the WGS84 ellipsoid (more
15%   precisely, the value returned by defaultellipsoid) is used.  lat2,
16%   lon2, and azi2 give the position and forward azimuths at the end point
17%   in degrees.  The other outputs, S12, m12, M12, M21, a12 are documented
18%   in geoddoc.  geoddoc also gives the restrictions on the allowed ranges
19%   of the arguments.
20%
21%   flags (default 0) is a combination of 2 flags:
22%      arcmode = bitand(flags, 1)
23%      long_unroll = bitand(flags, 2)
24%
25%   If arcmode is unset (the default), then, in the long form of the call,
26%   the input argument s12_a12 is the distance s12 (in meters) and the
27%   final output variable a12_s12 is the arc length on the auxiliary sphere
28%   a12 (in degrees).  If arcmode is set, then the roles of s12_a12 and
29%   a12_s12 are reversed; s12_a12 is interpreted as the arc length on the
30%   auxiliary sphere a12 (in degrees) and the corresponding distance s12 is
31%   returned in the final output variable a12_s12 (in meters).
32%
33%   If long_unroll is unset (the default), then the value lon2 is in the
34%   range [-180,180].  If long_unroll is set, the longitude is "unrolled"
35%   so that the quantity lon2 - lon1 indicates how many times and in what
36%   sense the geodesic encircles the ellipsoid.
37%
38%   The two optional arguments, ellipsoid and flags, may be given in any
39%   order and either or both may be omitted.
40%
41%   When given a combination of scalar and array inputs, GEODRECKON behaves
42%   as though the inputs were expanded to match the size of the arrays.
43%   However, in the particular case where lat1 and azi1 are the same for
44%   all the input points, they should be specified as scalars since this
45%   will considerably speed up the calculations.  (In particular a series
46%   of points along a single geodesic is efficiently computed by specifying
47%   an array for s12 only.)
48%
49%   This is an implementation of the algorithm given in
50%
51%     C. F. F. Karney, Algorithms for geodesics,
52%     J. Geodesy 87, 43-55 (2013);
53%     https://doi.org/10.1007/s00190-012-0578-z
54%     Addenda: https://geographiclib.sourceforge.io/geod-addenda.html
55%
56%   This function duplicates some of the functionality of the RECKON
57%   function in the MATLAB mapping toolbox.  Differences are
58%
59%     * When the ellipsoid argument is omitted, use the WGS84 ellipsoid.
60%     * The routines work for prolate (as well as oblate) ellipsoids.
61%     * The azimuth at the end point azi2 is returned.
62%     * The solution is accurate to round off for abs(e) < 0.2.
63%     * Redundant calculations are avoided when computing multiple
64%       points on a single geodesic.
65%     * Additional properties of the geodesic are calcuated.
66%
67%   See also GEODDOC, GEODDISTANCE, GEODAREA, DEFAULTELLIPSOID, FLAT2ECC.
68
69% Copyright (c) Charles Karney (2012-2018) <charles@karney.com>.
70%
71% This is a straightforward transcription of the C++ implementation in
72% GeographicLib and the C++ source should be consulted for additional
73% documentation.  This is a vector implementation and the results returned
74% with array arguments are identical to those obtained with multiple calls
75% with scalar arguments.
76
77  narginchk(4, 6)
78  try
79    S = size(lat1 + lon1 + s12_a12 + azi1);
80  catch
81    error('lat1, lon1, s12, azi1 have incompatible sizes')
82  end
83  if nargin <= 4
84    ellipsoid = defaultellipsoid; flags = 0;
85  elseif nargin == 5
86    arg5 = ellipsoid(:);
87    if length(arg5) == 2
88      ellipsoid = arg5; flags = 0;
89    else
90      flags = arg5; ellipsoid = defaultellipsoid;
91    end
92  else
93    arg5 = ellipsoid(:);
94    arg6 = flags;
95    if length(arg5) == 2
96      ellipsoid = arg5; flags = arg6;
97    else
98      flags = arg5; ellipsoid = arg6;
99    end
100  end
101  if length(ellipsoid) ~= 2
102    error('ellipsoid must be a vector of size 2')
103  end
104  if ~isscalar(flags)
105    error('flags must be a scalar')
106  end
107  arcmode = bitand(flags, 1);
108  long_unroll = bitand(flags, 2);
109  Z = zeros(prod(S),1);
110
111  degree = pi/180;
112  tiny = sqrt(realmin);
113
114  a = ellipsoid(1);
115  e2 = real(ellipsoid(2)^2);
116  f = e2 / (1 + sqrt(1 - e2));
117  f1 = 1 - f;
118  ep2 = e2 / (1 - e2);
119  n = f / (2 - f);
120  b = a * f1;
121
122  areap = nargout >= 4;
123  redlp = nargout >= 5;
124  scalp = nargout >= 6;
125
126  A3x = A3coeff(n);
127  C3x = C3coeff(n);
128
129  lat1 = AngRound(LatFix(lat1(:)));
130  lon1 = lon1(:);
131  azi1 = AngRound(azi1(:));
132  s12_a12 = s12_a12(:);
133
134  [salp1, calp1] = sincosdx(azi1);
135  [sbet1, cbet1] = sincosdx(lat1);
136  sbet1 = f1 * sbet1; cbet1 = max(tiny, cbet1);
137  [sbet1, cbet1] = norm2(sbet1, cbet1);
138  dn1 = sqrt(1 + ep2 * sbet1.^2);
139
140  salp0 = salp1 .* cbet1; calp0 = hypot(calp1, salp1 .* sbet1);
141  ssig1 = sbet1; somg1 = salp0 .* sbet1;
142  csig1 = cbet1 .* calp1; csig1(sbet1 == 0 & calp1 == 0) = 1; comg1 = csig1;
143  [ssig1, csig1] = norm2(ssig1, csig1);
144
145  k2 = calp0.^2 * ep2;
146  epsi = k2 ./ (2 * (1 + sqrt(1 + k2)) + k2);
147  A1m1 = A1m1f(epsi);
148  C1a = C1f(epsi);
149  B11 = SinCosSeries(true, ssig1, csig1, C1a);
150  s = sin(B11); c = cos(B11);
151  stau1 = ssig1 .* c + csig1 .* s; ctau1 = csig1 .* c - ssig1 .* s;
152
153  C1pa = C1pf(epsi);
154  C3a = C3f(epsi, C3x);
155  A3c = -f * salp0 .* A3f(epsi, A3x);
156  B31 = SinCosSeries(true, ssig1, csig1, C3a);
157
158  if arcmode
159    sig12 = s12_a12 * degree;
160    [ssig12, csig12] = sincosdx(s12_a12);
161  else
162    tau12 = s12_a12 ./ (b * (1 + A1m1));
163    s = sin(tau12); c = cos(tau12);
164    B12 = - SinCosSeries(true, stau1 .* c + ctau1 .* s, ...
165                         ctau1 .* c - stau1 .* s, C1pa);
166    sig12 = tau12 - (B12 - B11);
167    ssig12 = sin(sig12); csig12 = cos(sig12);
168    if abs(f) > 0.01
169      ssig2 = ssig1 .* csig12 + csig1 .* ssig12;
170      csig2 = csig1 .* csig12 - ssig1 .* ssig12;
171      B12 =  SinCosSeries(true, ssig2, csig2, C1a);
172      serr = (1 + A1m1) .* (sig12 + (B12 - B11)) - s12_a12/b;
173      sig12 = sig12 - serr ./ sqrt(1 + k2 .* ssig2.^2);
174      ssig12 = sin(sig12); csig12 = cos(sig12);
175    end
176  end
177
178  ssig2 = ssig1 .* csig12 + csig1 .* ssig12;
179  csig2 = csig1 .* csig12 - ssig1 .* ssig12;
180  dn2 = sqrt(1 + k2 .* ssig2.^2);
181  if arcmode || redlp || scalp
182    if arcmode || abs(f) > 0.01
183      B12 = SinCosSeries(true, ssig2, csig2, C1a);
184    end
185    AB1 = (1 + A1m1) .* (B12 - B11);
186  end
187  sbet2 = calp0 .* ssig2;
188  cbet2 = hypot(salp0, calp0 .* csig2);
189  cbet2(cbet2 == 0) = tiny;
190  somg2 = salp0 .* ssig2; comg2 = csig2;
191  salp2 = salp0; calp2 = calp0 .* csig2;
192  if long_unroll
193    E = copysignx(1, salp0);
194    omg12 = E .* (sig12 ...
195                  - (atan2(   ssig2, csig2) - atan2(   ssig1, csig1)) ...
196                  + (atan2(E.*somg2, comg2) - atan2(E.*somg1, comg1)));
197  else
198    omg12 = atan2(somg2 .* comg1 - comg2 .* somg1, ...
199                  comg2 .* comg1 + somg2 .* somg1);
200  end
201  lam12 = omg12 + ...
202          A3c .* ( sig12 + (SinCosSeries(true, ssig2, csig2, C3a) - B31));
203  lon12 = lam12 / degree;
204  if long_unroll
205    lon2 = lon1 + lon12;
206  else
207    lon12 = AngNormalize(lon12);
208    lon2 = AngNormalize(AngNormalize(lon1) + lon12);
209  end
210  lat2 = atan2dx(sbet2, f1 * cbet2);
211  azi2 = atan2dx(salp2, calp2);
212  if arcmode
213    a12_s12 = b * ((1 + A1m1) .* sig12 + AB1);
214  else
215    a12_s12 = sig12 / degree;
216  end
217  a12_s12 = reshape(a12_s12 + Z, S);
218
219  if redlp || scalp
220    A2m1 = A2m1f(epsi);
221    C2a = C2f(epsi);
222    B21 = SinCosSeries(true, ssig1, csig1, C2a);
223    B22 = SinCosSeries(true, ssig2, csig2, C2a);
224    AB2 = (1 + A2m1) .* (B22 - B21);
225    J12 = (A1m1 - A2m1) .* sig12 + (AB1 - AB2);
226    if redlp
227      m12 = b * ((dn2 .* (csig1 .* ssig2) - dn1 .* (ssig1 .* csig2)) ...
228                 - csig1 .* csig2 .* J12);
229      m12 = reshape(m12 + Z, S);
230    end
231    if scalp
232      t = k2 .* (ssig2 - ssig1) .* (ssig2 + ssig1) ./ (dn1 + dn2);
233      M12 = csig12 + (t .* ssig2 - csig2 .* J12) .* ssig1 ./ dn1;
234      M21 = csig12 - (t .* ssig1 - csig1 .* J12) .* ssig2 ./  dn2;
235      M12 = reshape(M12 + Z, S); M21 = reshape(M21 + Z, S);
236    end
237  end
238
239  if areap
240    C4x = C4coeff(n);
241    C4a = C4f(epsi, C4x);
242    A4 = (a^2 * e2) * calp0 .* salp0;
243    B41 = SinCosSeries(false, ssig1, csig1, C4a);
244    B42 = SinCosSeries(false, ssig2, csig2, C4a);
245    salp12 = calp0 .* salp0 .* ...
246             cvmgt(csig1 .* (1 - csig12) + ssig12 .* ssig1, ...
247                   ssig12 .* ...
248                   (csig1 .* ssig12 ./ max(1, (1 + csig12)) + ssig1), ...
249                   csig12 <= 0);
250    calp12 = salp0.^2 + calp0.^2 .* csig1 .* csig2;
251    % Deal with geodreckon(10, 0, [], 0) which has calp2 = []
252    if ~isempty(Z)
253      % Enlarge salp1, calp1 is case lat1 is an array and azi1 is a scalar
254      s = zeros(size(salp0)); salp1 = salp1 + s; calp1 = calp1 + s;
255      s = calp0 == 0 | salp0 == 0;
256      salp12(s) = salp2(s) .* calp1(s) - calp2(s) .* salp1(s);
257      calp12(s) = calp2(s) .* calp1(s) + salp2(s) .* salp1(s);
258    end
259    if e2 ~= 0
260      c2 = (a^2 + b^2 * eatanhe(1, e2) / e2) / 2;
261    else
262      c2 = a^2;
263    end
264    S12 = c2 * atan2(salp12, calp12) + A4 .* (B42 - B41);
265    S12 = reshape(S12 + Z, S);
266  end
267
268  lat2 = reshape(lat2 + Z, S);
269  lon2 = reshape(lon2, S);
270  azi2 = reshape(azi2 + Z, S);
271
272end
273