1function [s12, azi1, azi2, S12, m12, M12, M21, a12] = geoddistance ...
2      (lat1, lon1, lat2, lon2, ellipsoid)
3%GEODDISTANCE  Distance between points on an ellipsoid
4%
5%   [s12, azi1, azi2] = GEODDISTANCE(lat1, lon1, lat2, lon2)
6%   [s12, azi1, azi2, S12, m12, M12, M21, a12] =
7%      GEODDISTANCE(lat1, lon1, lat2, lon2, ellipsoid)
8%
9%   solves the inverse geodesic problem of finding of length and azimuths
10%   of the shortest geodesic between points specified by lat1, lon1, lat2,
11%   lon2.  The input latitudes and longitudes, lat1, lon1, lat2, lon2, can
12%   be scalars or arrays of equal size and must be expressed in degrees.
13%   The ellipsoid vector is of the form [a, e], where a is the equatorial
14%   radius in meters, e is the eccentricity.  If ellipsoid is omitted, the
15%   WGS84 ellipsoid (more precisely, the value returned by
16%   defaultellipsoid) is used.  The output s12 is the distance in meters
17%   and azi1 and azi2 are the forward azimuths at the end points in
18%   degrees.  The other optional outputs, S12, m12, M12, M21, a12 are
19%   documented in geoddoc.  geoddoc also gives the restrictions on the
20%   allowed ranges of the arguments.
21%
22%   When given a combination of scalar and array inputs, the scalar inputs
23%   are automatically expanded to match the size of the arrays.
24%
25%   This is an implementation of the algorithm given in
26%
27%     C. F. F. Karney, Algorithms for geodesics,
28%     J. Geodesy 87, 43-55 (2013);
29%     https://doi.org/10.1007/s00190-012-0578-z
30%     Addenda: https://geographiclib.sourceforge.io/geod-addenda.html
31%
32%   This function duplicates some of the functionality of the distance
33%   function in the MATLAB mapping toolbox.  Differences are
34%
35%     * When the ellipsoid argument is omitted, use the WGS84 ellipsoid.
36%     * The routines work for prolate (as well as oblate) ellipsoids.
37%     * The azimuth at the second end point azi2 is returned.
38%     * The solution is accurate to round off for abs(e) < 0.2.
39%     * The algorithm converges for all pairs of input points.
40%     * Additional properties of the geodesic are calcuated.
41%
42%   See also GEODDOC, GEODRECKON, GEODAREA, DEFAULTELLIPSOID, FLAT2ECC.
43
44% Copyright (c) Charles Karney (2012-2021) <charles@karney.com>.
45%
46% This is a straightforward transcription of the C++ implementation in
47% GeographicLib and the C++ source should be consulted for additional
48% documentation.  This is a vector implementation and the results returned
49% with array arguments are identical to those obtained with multiple calls
50% with scalar arguments.  The biggest change was to eliminate the branching
51% to allow a vectorized solution.
52
53  narginchk(4, 5)
54  if nargin < 5, ellipsoid = defaultellipsoid; end
55  try
56    S = size(lat1 + lon1 + lat2 + lon2);
57  catch
58    error('lat1, lon1, s12, azi1 have incompatible sizes')
59  end
60  if length(ellipsoid(:)) ~= 2
61    error('ellipsoid must be a vector of size 2')
62  end
63  Z = zeros(S);
64  lat1 = lat1 + Z; lon1 = lon1 + Z;
65  lat2 = lat2 + Z; lon2 = lon2 + Z;
66  Z = Z(:);
67
68  degree = pi/180;
69  tiny = sqrt(realmin);
70  tol0 = eps;
71  tolb = eps * sqrt(eps);
72  maxit1 = 20;
73  maxit2 = maxit1 + (-log2(eps) + 1) + 10;
74
75  a = ellipsoid(1);
76  e2 = real(ellipsoid(2)^2);
77  f = e2 / (1 + sqrt(1 - e2));
78
79  f1 = 1 - f;
80  ep2 = e2 / (1 - e2);
81  n = f / (2 - f);
82  b = a * f1;
83
84  distp = true;
85  areap = nargout >= 4;
86  redp = nargout >= 5;
87  scalp = nargout >= 6;
88
89  % mask for Lengths: 1 = distance, 2 = reduced length, 4 = geodesic scale
90  lengthmask = distp;
91  if redp
92    lengthmask = 2;
93  end
94  if scalp
95    lengthmask = lengthmask + 4;
96  end
97  A3x = A3coeff(n);
98  C3x = C3coeff(n);
99
100  [lon12, lon12s] = AngDiff(lon1(:), lon2(:));
101  lonsign = 2 * (lon12 >= 0) - 1;
102  lon12 = lonsign .* AngRound(lon12);
103  lon12s = AngRound((180 -lon12) - lonsign .* lon12s);
104  lam12 = lon12 * degree; slam12 = Z; clam12 = Z;
105  l = lon12 > 90;
106  [slam12( l), clam12( l)] = sincosdx(lon12s(l)); clam12(l) = -clam12(l);
107  [slam12(~l), clam12(~l)] = sincosdx(lon12(~l));
108
109  lat1 = AngRound(LatFix(lat1(:)));
110  lat2 = AngRound(LatFix(lat2(:)));
111  swapp = 2 * ~(abs(lat1) < abs(lat2)) - 1;
112  lonsign(swapp < 0) = - lonsign(swapp < 0);
113  [lat1(swapp < 0), lat2(swapp < 0)] = swap(lat1(swapp < 0), lat2(swapp < 0));
114
115  latsign = 2 * (lat1 < 0) - 1;
116  lat1 = latsign .* lat1;
117  lat2 = latsign .* lat2;
118
119  [sbet1, cbet1] = sincosdx(lat1); sbet1 = f1 * sbet1;
120  [sbet1, cbet1] = norm2(sbet1, cbet1); cbet1 = max(tiny, cbet1);
121
122  [sbet2, cbet2] = sincosdx(lat2); sbet2 = f1 * sbet2;
123  [sbet2, cbet2] = norm2(sbet2, cbet2); cbet2 = max(tiny, cbet2);
124
125  c = cbet1 < -sbet1 & cbet2 == cbet1;
126  sbet2(c) = (2 * (sbet2(c) < 0) - 1) .* sbet1(c);
127  c = ~(cbet1 < -sbet1) & abs(sbet2) == - sbet1;
128  cbet2(c) = cbet1(c);
129
130  dn1 = sqrt(1 + ep2 * sbet1.^2);
131  dn2 = sqrt(1 + ep2 * sbet2.^2);
132
133  sig12 = Z; ssig1 = Z; csig1 = Z; ssig2 = Z; csig2 = Z;
134  calp1 = Z; salp1 = Z; calp2 = Z; salp2 = Z;
135  s12 = Z; m12 = Z; M12 = Z; M21 = Z;
136  omg12 = Z; somg12 = 2+Z; comg12 = Z; domg12 = Z;
137
138  m = lat1 == -90 | slam12 == 0;
139
140  if any(m)
141    calp1(m) = clam12(m); salp1(m) = slam12(m);
142    calp2(m) = 1; salp2(m) = 0;
143
144    ssig1(m) = sbet1(m); csig1(m) = calp1(m) .* cbet1(m);
145    ssig2(m) = sbet2(m); csig2(m) = calp2(m) .* cbet2(m);
146
147    sig12(m) = ...
148        atan2(0 + max(0, csig1(m) .* ssig2(m) - ssig1(m) .* csig2(m)), ...
149              csig1(m) .* csig2(m) + ssig1(m) .* ssig2(m));
150
151    [s12(m), m12(m), ~, M12(m), M21(m)] = ...
152        Lengths(n, sig12(m), ...
153                ssig1(m), csig1(m), dn1(m), ssig2(m), csig2(m), dn2(m), ...
154                cbet1(m), cbet2(m), bitor(1+2, lengthmask), ep2);
155    m = m & (sig12 < 1 | m12 >= 0);
156    g = m & (sig12 < 3 * tiny | ...
157             (sig12 < tol0 & (s12 < 0 | m12 < 0)));
158    sig12(g) = 0; s12(g) = 0; m12(g) = 0;
159    m12(m) = m12(m) * b;
160    s12(m) = s12(m) * b;
161  end
162
163  eq = ~m & sbet1 == 0;
164  if f > 0
165    eq = eq & lon12s >= f * 180;
166  end
167  calp1(eq) = 0; calp2(eq) = 0; salp1(eq) = 1; salp2(eq) = 1;
168  s12(eq) = a * lam12(eq); sig12(eq) = lam12(eq) / f1; omg12(eq) = sig12(eq);
169  m12(eq) = b * sin(omg12(eq)); M12(eq) = cos(omg12(eq)); M21(eq) = M12(eq);
170
171  g = ~eq & ~m;
172
173  if any(g)
174    dnm = Z;
175    [sig12(g), salp1(g), calp1(g), salp2(g), calp2(g), dnm(g)] = ...
176        InverseStart(sbet1(g), cbet1(g), dn1(g), ...
177                     sbet2(g), cbet2(g), dn2(g), ...
178                     lam12(g), slam12(g), clam12(g), f, A3x);
179
180    s = g & sig12 >= 0;
181    s12(s) = b * sig12(s) .* dnm(s);
182    m12(s) = b * dnm(s).^2 .* sin(sig12(s) ./ dnm(s));
183    if scalp
184      M12(s) = cos(sig12(s) ./ dnm(s)); M21(s) = M12(s);
185    end
186    omg12(s) = lam12(s) ./ (f1 * dnm(s));
187
188    g = g & sig12 < 0;
189
190    salp1a = Z + tiny; calp1a = Z + 1;
191    salp1b = Z + tiny; calp1b = Z - 1;
192    ssig1 = Z; csig1 = Z; ssig2 = Z; csig2 = Z;
193    epsi = Z; v = Z; dv = Z;
194    numit = Z;
195    tripn = Z > 0;
196    tripb = tripn;
197    if any(g)
198      gsave = g;
199      for k = 0 : maxit2 - 1
200        if k == 0 && ~any(g), break, end
201        numit(g) = k;
202        [v(g), dv(g), ...
203         salp2(g), calp2(g), sig12(g), ...
204         ssig1(g), csig1(g), ssig2(g), csig2(g), epsi(g), domg12(g)] = ...
205            Lambda12(sbet1(g), cbet1(g), dn1(g), ...
206                     sbet2(g), cbet2(g), dn2(g), ...
207                     salp1(g), calp1(g), slam12(g), clam12(g), f, A3x, C3x);
208        g = g & ~(tripb | ~(abs(v) >= ((tripn * 7) + 1) * tol0));
209        if ~any(g), break, end
210
211        c = g & v > 0;
212        if k <= maxit1
213          c = c & calp1 ./ salp1 > calp1b ./ salp1b;
214        end
215        salp1b(c) = salp1(c); calp1b(c) = calp1(c);
216
217        c = g & v < 0;
218        if k <= maxit1
219          c = c & calp1 ./ salp1 < calp1a ./ salp1a;
220        end
221        salp1a(c) = salp1(c); calp1a(c) = calp1(c);
222
223        if k == maxit1, tripn(g) = false; end
224        if k < maxit1
225          dalp1 = -v ./ dv;
226          sdalp1 = sin(dalp1); cdalp1 = cos(dalp1);
227          nsalp1 = salp1 .* cdalp1 + calp1 .* sdalp1;
228          calp1(g) = calp1(g) .* cdalp1(g) - salp1(g) .* sdalp1(g);
229          salp1(g) = nsalp1(g);
230          tripn = g & abs(v) <= 16 * tol0;
231          c = g & ~(dv > 0 & nsalp1 > 0 & abs(dalp1) < pi);
232          tripn(c) = false;
233        else
234          c = g;
235        end
236
237        salp1(c) = (salp1a(c) + salp1b(c))/2;
238        calp1(c) = (calp1a(c) + calp1b(c))/2;
239        [salp1(g), calp1(g)] = norm2(salp1(g), calp1(g));
240        tripb(c) = abs(salp1a(c)-salp1(c)) + (calp1a(c)-calp1(c)) < tolb ...
241            |      abs(salp1(c)-salp1b(c)) + (calp1(c)-calp1b(c)) < tolb;
242      end
243
244      g = gsave;
245      if bitand(2+4, lengthmask)
246        % set distance bit if redp or scalp, so that J12 is computed in a
247        % canonical way.
248        lengthmask = bitor(1, lengthmask);
249      end
250
251      [s12(g), m12(g), ~, M12(g), M21(g)] = ...
252          Lengths(epsi(g), sig12(g), ...
253                  ssig1(g), csig1(g), dn1(g), ssig2(g), csig2(g), dn2(g), ...
254                  cbet1(g), cbet2(g), lengthmask, ep2);
255
256      m12(g) = m12(g) * b;
257      s12(g) = s12(g) * b;
258      if areap
259        sdomg12 = sin(domg12(g)); cdomg12 = cos(domg12(g));
260        somg12(g) = slam12(g) .* cdomg12 - clam12(g) .* sdomg12;
261        comg12(g) = clam12(g) .* cdomg12 + slam12(g) .* sdomg12;
262      end
263    end
264  end
265
266  s12 = 0 + s12;
267
268  if areap
269    salp0 = salp1 .* cbet1; calp0 = hypot(calp1, salp1 .* sbet1);
270    ssig1 = sbet1; csig1 = calp1 .* cbet1;
271    ssig2 = sbet2; csig2 = calp2 .* cbet2;
272    % Stop complaints from norm2 for equatorial geodesics
273    csig1(calp0 == 0) = 1; csig2(calp0 == 0) = 1;
274    k2 = calp0.^2 * ep2;
275    epsi = k2 ./ (2 * (1 + sqrt(1 + k2)) + k2);
276    A4 = (a^2 * e2) * calp0 .* salp0;
277    [ssig1, csig1] = norm2(ssig1, csig1);
278    [ssig2, csig2] = norm2(ssig2, csig2);
279
280    C4x = C4coeff(n);
281    Ca = C4f(epsi, C4x);
282    B41 = SinCosSeries(false, ssig1, csig1, Ca);
283    B42 = SinCosSeries(false, ssig2, csig2, Ca);
284    S12 = A4 .* (B42 - B41);
285    S12(calp0 == 0 | salp0 == 0) = 0;
286
287    l = ~m & somg12 > 1;
288    somg12(l) = sin(omg12(l)); comg12(l) = cos(omg12(l));
289
290    l = ~m & comg12 > -0.7071 & sbet2 - sbet1 < 1.75;
291    alp12 = Z;
292    domg12 = 1 + comg12(l);
293    dbet1 = 1 + cbet1(l); dbet2 = 1 + cbet2(l);
294    alp12(l) = 2 * ...
295        atan2(somg12(l) .* (sbet1(l) .* dbet2 + sbet2(l) .* dbet1), ...
296              domg12    .* (sbet1(l) .* sbet2(l) + dbet1 .* dbet2));
297    l = ~l;
298    salp12 = salp2(l) .* calp1(l) - calp2(l) .* salp1(l);
299    calp12 = calp2(l) .* calp1(l) + salp2(l) .* salp1(l);
300    s = salp12 == 0 & calp12 < 0;
301    salp12(s) = tiny * calp1(s); calp12(s) = -1;
302    alp12(l) = atan2(salp12, calp12);
303    if e2 ~= 0
304      c2 = (a^2 + b^2 * eatanhe(1, e2) / e2) / 2;
305    else
306      c2 = a^2;
307    end
308    S12 = 0 + swapp .* lonsign .* latsign .* (S12 + c2 * alp12);
309  end
310
311  [salp1(swapp<0), salp2(swapp<0)] = swap(salp1(swapp<0), salp2(swapp<0));
312  [calp1(swapp<0), calp2(swapp<0)] = swap(calp1(swapp<0), calp2(swapp<0));
313  if scalp
314    [M12(swapp<0), M21(swapp<0)] = swap(M12(swapp<0), M21(swapp<0));
315    M12 = reshape(M12, S); M21 = reshape(M21, S);
316  end
317  salp1 = salp1 .* swapp .* lonsign; calp1 = calp1 .* swapp .* latsign;
318  salp2 = salp2 .* swapp .* lonsign; calp2 = calp2 .* swapp .* latsign;
319
320  azi1 = atan2dx(salp1, calp1);
321  azi2 = atan2dx(salp2, calp2);
322  a12 = sig12 / degree;
323
324  s12 = reshape(s12, S); azi1 = reshape(azi1, S); azi2 = reshape(azi2, S);
325  if redp
326    m12 = reshape(m12, S);
327  end
328  if nargout >= 8
329    a12 = reshape(a12, S);
330  end
331  if areap
332    S12 = reshape(S12, S);
333  end
334end
335
336function [sig12, salp1, calp1, salp2, calp2, dnm] = ...
337      InverseStart(sbet1, cbet1, dn1, sbet2, cbet2, dn2, ...
338                   lam12, slam12, clam12, f, A3x)
339%INVERSESTART  Compute a starting point for Newton's method
340
341  N = length(sbet1);
342  f1 = 1 - f;
343  e2 = f * (2 - f);
344  ep2 = e2 / (1 - e2);
345  n = f / (2 - f);
346  tol0 = eps;
347  tol1 = 200 * tol0;
348  tol2 = sqrt(eps);
349  etol2 = 0.1 * tol2 / sqrt( max(0.001, abs(f)) * min(1, 1 - f/2) / 2 );
350  xthresh = 1000 * tol2;
351
352  sig12 = -ones(N, 1); salp2 = nan(N, 1); calp2 = nan(N, 1);
353  sbet12 = sbet2 .* cbet1 - cbet2 .* sbet1;
354  cbet12 = cbet2 .* cbet1 + sbet2 .* sbet1;
355  sbet12a = sbet2 .* cbet1 + cbet2 .* sbet1;
356  s = cbet12 >= 0 & sbet12 < 0.5 & cbet2 .* lam12 < 0.5;
357  omg12 = lam12;
358  dnm = nan(N, 1);
359  sbetm2 = (sbet1(s) + sbet2(s)).^2;
360  sbetm2 = sbetm2 ./ (sbetm2 + (cbet1(s) + cbet2(s)).^2);
361  dnm(s) = sqrt(1 + ep2 * sbetm2);
362  omg12(s) = omg12(s) ./ (f1 * dnm(s));
363  somg12 = slam12; comg12 = clam12;
364  somg12(s) = sin(omg12(s)); comg12(s) = cos(omg12(s));
365
366  salp1 = cbet2 .* somg12;
367  t = cbet2 .* sbet1 .* somg12.^2;
368  calp1 = cvmgt(sbet12  + t ./ max(1, 1 + comg12), ...
369                sbet12a - t ./ max(1, 1 - comg12), ...
370                comg12 >= 0);
371
372  ssig12 = hypot(salp1, calp1);
373  csig12 = sbet1 .* sbet2 + cbet1 .* cbet2 .* comg12;
374
375  s = s & ssig12 < etol2;
376  salp2(s) = cbet1(s) .* somg12(s);
377  calp2(s) = somg12(s).^2 ./ (1 + comg12(s));
378  calp2(s & comg12 < 0) = 1 - comg12(s & comg12 < 0);
379  calp2(s) = sbet12(s) - cbet1(s) .* sbet2(s) .* calp2(s);
380  [salp2, calp2] = norm2(salp2, calp2);
381  sig12(s) = atan2(ssig12(s), csig12(s));
382
383  s = ~(s | abs(n) > 0.1 | csig12 >= 0 | ssig12 >= 6 * abs(n) * pi * cbet1.^2);
384
385  if any(s)
386    lam12x = atan2(-slam12(s), -clam12(s));
387    if f >= 0
388      k2 = sbet1(s).^2 * ep2;
389      epsi = k2 ./ (2 * (1 + sqrt(1 + k2)) + k2);
390      lamscale = f * cbet1(s) .* A3f(epsi, A3x) * pi;
391      betscale = lamscale .* cbet1(s);
392      x = lam12x ./ lamscale;
393      y = sbet12a(s) ./ betscale;
394    else
395      cbet12a = cbet2(s) .* cbet1(s) - sbet2(s) .* sbet1(s);
396      bet12a = atan2(sbet12a(s), cbet12a);
397      [~, m12b, m0] = ...
398          Lengths(n, pi + bet12a, ...
399                  sbet1(s), -cbet1(s), dn1(s), sbet2(s), cbet2(s), dn2(s), ...
400                  cbet1(s), cbet2(s), 2);
401      x = -1 + m12b ./ (cbet1(s) .* cbet2(s) .* m0 * pi);
402      betscale = cvmgt(sbet12a(s) ./ min(-0.01, x), - f * cbet1(s).^2 * pi, ...
403                       x < -0.01);
404     lamscale = betscale ./ cbet1(s);
405      y = lam12x ./ lamscale;
406    end
407    k = Astroid(x, y);
408    str = y > -tol1 & x > -1 - xthresh;
409    k(str) = 1;
410    if f >= 0
411      omg12a = -x .* k ./ (1 + k);
412    else
413      omg12a = -y .* (1 + k) ./ k;
414    end
415    omg12a = lamscale .* omg12a;
416    somg12 = sin(omg12a); comg12 = -cos(omg12a);
417    salp1(s) = cbet2(s) .* somg12;
418    calp1(s) = sbet12a(s) - cbet2(s) .* sbet1(s) .* somg12.^2 ./ (1 - comg12);
419
420    if any(str)
421      salp1s = salp1(s); calp1s = calp1(s);
422      if f >= 0
423        salp1s(str) = min(1, -x(str));
424        calp1s(str) = -sqrt(1 - salp1s(str).^2);
425      else
426        calp1s(str) = max(cvmgt(0, -1, x(str) > -tol1), x(str));
427        salp1s(str) = sqrt(1 - calp1s(str).^2);
428      end
429      salp1(s) = salp1s; calp1(s) = calp1s;
430    end
431  end
432
433  calp1(salp1 <= 0) = 0; salp1(salp1 <= 0) = 1;
434  [salp1, calp1] = norm2(salp1, calp1);
435end
436
437function k = Astroid(x, y)
438% ASTROID  Solve the astroid equation
439%
440%   K = ASTROID(X, Y) solves the quartic polynomial Eq. (55)
441%
442%     K^4 + 2 * K^3 - (X^2 + Y^2 - 1) * K^2 - 2*Y^2 * K - Y^2 = 0
443%
444%   for the positive root K.  X and Y are column vectors of the same size
445%   and the returned value K has the same size.
446
447  k = zeros(length(x), 1);
448  p = x.^2;
449  q = y.^2;
450  r = (p + q - 1) / 6;
451  fl1 = ~(q == 0 & r <= 0);
452  p = p(fl1);
453  q = q(fl1);
454  r = r(fl1);
455  S = p .* q / 4;
456  r2 = r.^2;
457  r3 = r .* r2;
458  disc = S .* (S + 2 * r3);
459  u = r;
460  fl2 = disc >= 0;
461  T3 = S(fl2) + r3(fl2);
462  T3 = T3 + (1 - 2 * (T3 < 0)) .* sqrt(disc(fl2));
463  T = cbrtx(T3);
464  u(fl2) = u(fl2) + T + r2(fl2) ./ cvmgt(T, inf, T ~= 0);
465  ang = atan2(sqrt(-disc(~fl2)), -(S(~fl2) + r3(~fl2)));
466  u(~fl2) = u(~fl2) + 2 * r(~fl2) .* cos(ang / 3);
467  v = sqrt(u.^2 + q);
468  uv = u + v;
469  fl2 = u < 0;
470  uv(fl2) = q(fl2) ./ (v(fl2) - u(fl2));
471  w = (uv - q) ./ (2 * v);
472  k(fl1) = uv ./ (sqrt(uv + w.^2) + w);
473end
474
475function [lam12, dlam12, ...
476          salp2, calp2, sig12, ssig1, csig1, ssig2, csig2, epsi, ...
477          domg12] = ...
478    Lambda12(sbet1, cbet1, dn1, sbet2, cbet2, dn2, salp1, calp1, ...
479             slam120, clam120, f, A3x, C3x)
480%LAMBDA12  Solve the hybrid problem
481
482  tiny = sqrt(realmin);
483  f1 = 1 - f;
484  e2 = f * (2 - f);
485  ep2 = e2 / (1 - e2);
486
487  calp1(sbet1 == 0 & calp1 == 0) = -tiny;
488
489  salp0 = salp1 .* cbet1;
490  calp0 = hypot(calp1, salp1 .* sbet1);
491
492  ssig1 = sbet1; somg1 = salp0 .* sbet1;
493  csig1 = calp1 .* cbet1; comg1 = csig1;
494  [ssig1, csig1] = norm2(ssig1, csig1);
495
496  salp2 = cvmgt(salp0 ./ cbet2, salp1, cbet2 ~= cbet1);
497  calp2 = cvmgt(sqrt((calp1 .* cbet1).^2 + ...
498                     cvmgt((cbet2 - cbet1) .* (cbet1 + cbet2), ...
499                           (sbet1 - sbet2) .* (sbet1 + sbet2), ...
500                           cbet1 < -sbet1)) ./ cbet2, ...
501                abs(calp1), cbet2 ~= cbet1 | abs(sbet2) ~= -sbet1);
502  ssig2 = sbet2; somg2 = salp0 .* sbet2;
503  csig2 = calp2 .* cbet2;  comg2 = csig2;
504  [ssig2, csig2] = norm2(ssig2, csig2);
505
506  sig12 = atan2(0 + max(0, csig1 .* ssig2 - ssig1 .* csig2), ...
507                csig1 .* csig2 + ssig1 .* ssig2);
508
509  somg12 = 0 + max(0, comg1 .* somg2 - somg1 .* comg2);
510  comg12 =            comg1 .* comg2 + somg1 .* somg2;
511  eta = atan2(somg12 .* clam120 - comg12 .* slam120, ...
512              comg12 .* clam120 + somg12 .* slam120);
513  k2 = calp0.^2 * ep2;
514  epsi = k2 ./ (2 * (1 + sqrt(1 + k2)) + k2);
515  Ca = C3f(epsi, C3x);
516  B312 = SinCosSeries(true, ssig2, csig2, Ca) - ...
517         SinCosSeries(true, ssig1, csig1, Ca);
518  domg12 = -f * A3f(epsi, A3x) .* salp0 .* (sig12 + B312);
519  lam12 = eta + domg12;
520
521  [~, dlam12] = ...
522      Lengths(epsi, sig12, ...
523              ssig1, csig1, dn1, ssig2, csig2, dn2, cbet1, cbet2, 2);
524  z = calp2 == 0;
525  dlam12(~z) = dlam12(~z) .* f1 ./ (calp2(~z) .* cbet2(~z));
526  dlam12(z) = - 2 * f1 .* dn1(z) ./ sbet1(z);
527end
528
529function [s12b, m12b, m0, M12, M21] = ...
530      Lengths(epsi, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2, ...
531              cbet1, cbet2, outmask, ep2)
532%LENGTHS  Compute various lengths associate with a geodesic
533
534  N = nan(size(sig12));
535  if bitand(1+2+4, outmask)
536    A1 = A1m1f(epsi);
537    Ca = C1f(epsi);
538    if bitand(2+4, outmask)
539      A2 = A2m1f(epsi);
540      Cb = C2f(epsi);
541      m0 = A1 - A2;
542      A2 = 1 + A2;
543    end
544    A1 = 1 + A1;
545  end
546  if bitand(1, outmask)
547    B1 = SinCosSeries(true, ssig2, csig2, Ca) - ...
548         SinCosSeries(true, ssig1, csig1, Ca);
549    s12b = A1 .* (sig12 + B1);
550    if bitand(2+4, outmask)
551      B2 = SinCosSeries(true, ssig2, csig2, Cb) - ...
552           SinCosSeries(true, ssig1, csig1, Cb);
553      J12 = m0 .* sig12 + (A1 .* B1 - A2 .* B2);
554    end
555  else
556    s12b = N;                           % assign arbitrary unused result
557    if bitand(2+4, outmask)
558      for l = 1 : size(Cb, 2)
559        % Assume here that size(Ca, 2) >= size(Cb, 2)
560        Cb(:, l) = A1 .* Ca(:, l) - A2 .* Cb(:, l);
561      end
562      J12 = m0 .* sig12 + (SinCosSeries(true, ssig2, csig2, Cb) - ...
563                           SinCosSeries(true, ssig1, csig1, Cb));
564    end
565  end
566  if bitand(2, outmask)
567    m12b = dn2 .* (csig1 .* ssig2) - dn1 .* (ssig1 .* csig2) - ...
568           csig1 .* csig2 .* J12;
569  else
570    m0 = N; m12b = N;                   % assign arbitrary unused result
571  end
572  if bitand(4, outmask)
573    csig12 = csig1 .* csig2 + ssig1 .* ssig2;
574    t = ep2 * (cbet1 - cbet2) .* (cbet1 + cbet2) ./ (dn1 + dn2);
575    M12 = csig12 + (t .* ssig2 - csig2 .* J12) .* ssig1 ./ dn1;
576    M21 = csig12 - (t .* ssig1 - csig1 .* J12) .* ssig2 ./ dn2;
577  else
578    M12 = N; M21 = N;                   % assign arbitrary unused result
579  end
580end
581