...HEADmaster

Signed-off-by: Mattias Andrée <m@maandree.se>

19 files changed, 400 insertions, 61 deletions

diff --git a/TODO b/TODO
index f022c99..c84ef17 100644
--- a/TODO
+++ b/TODO
@@ -1 +1,52 @@
 Finish libtellurian_vincenty_inverse__
+Document the details of NaN returns in libtellurian_distance.3
+Document what happens to the azimuths when the starting point is a pole.
+
+libtellurian_coarse_distance can be simplified to (d/2R)²
+which would be an good alternative for simply sorting distances,
+and it would still be easy to afterwards convert the values for
+a selection of them into approximate distances.
+
+Add inverse function of libtellurian_effective_gravity_radians
+
+Add inverse function of libtellurian_elevated_gravity_radians
+
+And functions for converting between coordinate systems
+	Geodetic (h, φ, λ) : Angle between normal and equatorial plane
+		Inverse of Cartesian (see Cartesian for definition of N)
+		   Iteration is required unless φ or h is known
+			λ = atan2(Y, X)
+			h = p / cos φ - N
+			φ = tan⁻¹(Zp⁻¹/(1 - e²N/(N + h)))
+			where p = √(X² + Y²)
+			https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_geodetic_coordinates
+		   When h=0
+			λ = atan2(Y, X)
+			φ = tan⁻¹(Zp⁻¹/(1 - e²))
+			where p = √(X² + Y²)
+		   wouldn't it easier to convert to geocentric intermittently
+	Geocentric ϕ : Angle from centre of earth
+		ϕ = tan⁻¹((1 - e²) tan φ)
+	Geometric : Arc length = geodetic
+	Parametric/reduced β : spherical angle resulting in same distance from polar axis as
+		β = tan⁻¹((1 - f) tan φ)
+	Ellipsoidal-harmonic
+		TODO
+	Rectifying μ
+		μ = πm(φ) / 2m(½π), where m(u) = a(1 - e²) ∫{0→u} √(1 - e² sin² v)⁻³ dv
+	Authalic ξ
+		ξ = sin⁻¹(q(φ) / q(½π)), where q(u) = ((1 - e²) sin u)/(1 - e² sin u) + (1 - e²) e⁻¹ tanh⁻¹ (e sin u)
+		q(½π) = 1 + (1 - e²) e⁻¹ tanh⁻¹ e
+	Conformal χ
+		χ = tan⁻¹ (sinh [sinh⁻¹ tan φ - e tanh⁻¹ (e sin φ)])
+	Isometric ψ
+		ψ = sinh⁻¹ tan φ - e than⁻¹ (e sin φ)
+	Astronomical Φ : angle between equatorial plane and the true vertical direction
+		the true vertical direction, is the direction of gravity which is affect
+		byu the centrifugal acceleration in addition to the gravitational acceleration.
+	Cartesian (geocentric Cartesian)
+		X = (N + h) cos φ cos λ
+		Y = (N + h) cos φ sin λ
+		Z = (Nb²/a² + h) sin φ
+		where
+			N = a²/√(a² cos² φ + b² sin² φ)
diff --git a/common.h b/common.h
index 332e19a..2668d91 100644
--- a/common.h
+++ b/common.h
@@ -14,6 +14,10 @@
 #define degrees(RAD)  (180.0 / (double)M_PI * (RAD))
 #define haversin(X)   (fma(-0.5, cos(X), 0.5))
 
+#define geodetic(GEOCENTRIC)\
+	atan(tan(GEOCENTRIC) * (LIBTELLURIAN_EQUATORIAL_RADIUS * LIBTELLURIAN_EQUATORIAL_RADIUS /\
+	                        (LIBTELLURIAN_POLAR_RADIUS * LIBTELLURIAN_POLAR_RADIUS)))
+
 
 void libtellurian_vincenty_inverse__(double latitude1, double longitude1, double latitude2, double longitude2,
                                      double *distance_out, double *azimuth1_out, double *azimuth2_out);
diff --git a/libtellurian.7 b/libtellurian.7
index 0933aa9..ae3dff0 100644
--- a/libtellurian.7
+++ b/libtellurian.7
@@ -95,3 +95,16 @@ radius of curvature for some latitude.
 See
 .BR libtellurian.h (0)
 for a listing of available constants.
+.SH NOTES
+Users, and developers in particular, shall be aware that
+this library uses GPS coordinates which are geocentric.
+This means that the latitude is base one the angle from
+the centre on the earth. There are a number of other
+systems, most famously as a linear function of the
+arc length, or more commonly in geodesy, the geodesic
+latitude which is the enable between the local normal
+from the surface and equatorial plane. Users should
+take great care ensure the correct correct system (GPS)
+is used, and developers should be careful designing
+functions for the GPS coordinate system and be aware
+the geodesic formulae usually use geodesic latitudes.
diff --git a/libtellurian.h b/libtellurian.h
index b294e9c..98309be 100644
--- a/libtellurian.h
+++ b/libtellurian.h
@@ -219,6 +219,15 @@ double libtellurian_coarse_distance_radians(double latitude1, double longitude1,
  * performed. If you have no need for an azimuth you can set
  * the corresponding output parameter to `NULL`, and the
  * function will not compute it.
+ * 
+ * If the two points are they same, the distance will be 0
+ * and the azimuths will be NaN, denoting that any direction
+ * can be taken. If the two points are antipodal, the distance
+ * will be `LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE` and the
+ * azimuths will be NaN denoting that are multiple directions
+ * to take, specifying either any (if the two points are the
+ * poles) or either north or south (and the two azimuths shall
+ * be the opposite of each other).
  */
 LIBTELLURIAN_WUR__
 double libtellurian_distance(double latitude1, double longitude1,
@@ -247,6 +256,15 @@ double libtellurian_distance(double latitude1, double longitude1,
  * performed. If you have no need for an azimuth you can set
  * the corresponding output parameter to `NULL`, and the
  * function will not compute it.
+ * 
+ * If the two points are they same, the distance will be 0
+ * and the azimuths will be NaN, denoting that any direction
+ * can be taken. If the two points are antipodal, the distance
+ * will be `LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE` and the
+ * azimuths will be NaN denoting that are multiple directions
+ * to take, specifying either any (if the two points are the
+ * poles) or either north or south (and the two azimuths shall
+ * be the opposite of each other).
  */
 LIBTELLURIAN_WUR__
 double libtellurian_distance_radians(double latitude1, double longitude1,
@@ -271,6 +289,13 @@ double libtellurian_distance_radians(double latitude1, double longitude1,
  * This function is identical to libtellurian_distance`
  * except it does not compute the distance between the
  * points
+ * 
+ * If the two points are they same, the azimuths will be NaN,
+ * denoting that any direction can be taken. If the two points
+ * are antipodal, the azimuths will be NaN denoting that are
+ * multiple directions to take, specifying either any (if the
+ * two points are the poles) or either north or south (and
+ * the two azimuths shall be the opposite of each other).
  */
 void libtellurian_azimuth(double latitude1, double longitude1,
                           double latitude2, double longitude2,
@@ -294,6 +319,13 @@ void libtellurian_azimuth(double latitude1, double longitude1,
  * 
  * This function is identical to libtellurian_distance_radians`
  * except it does not compute the distance between the points
+ * 
+ * If the two points are they same, the azimuths will be NaN,
+ * denoting that any direction can be taken. If the two points
+ * are antipodal, the azimuths will be NaN denoting that are
+ * multiple directions to take, specifying either any (if the
+ * two points are the poles) or either north or south (and
+ * the two azimuths shall be the opposite of each other).
  */
 void libtellurian_azimuth_radians(double latitude1, double longitude1,
                                   double latitude2, double longitude2,
diff --git a/libtellurian_azimuth.c b/libtellurian_azimuth.c
index 4bfc743..c39596a 100644
--- a/libtellurian_azimuth.c
+++ b/libtellurian_azimuth.c
@@ -21,5 +21,40 @@ libtellurian_azimuth(double latitude1, double longitude1,
 
 
 #else
-TODO_TEST
+
+
+static int
+check(double da, double db, double dc, double dd,
+      double ra, double rb, double rc, double rd)
+{
+	double az1_ref = 1, az2_ref = 2, az1 = 3, az2 = 4, az1b = 5, az2b = 6;
+
+	libtellurian_azimuth_radians(ra, rb, rc, rd, &az1_ref, &az2_ref);
+	az1_ref *= 180 / M_PI;
+	az2_ref *= 180 / M_PI;
+
+	libtellurian_azimuth(da, db, dc, dd, &az1, &az2);
+	libtellurian_azimuth(da, db, dc, dd, &az1b, NULL);
+	libtellurian_azimuth(da, db, dc, dd, NULL, &az2b);
+	libtellurian_azimuth(da, db, dc, dd, NULL, NULL);
+	return az1 == az1_ref && az2 == az2_ref && az1 == az1b && az2 == az2b;
+}
+
+int
+main(void)
+{
+	double a1 = 1, a2 = 2;
+
+	ASSERT(check(30, 45, 60, 90,  D30, D45, D60, D90));
+	ASSERT(check(30, 45, 60, 45,  D30, D45, D60, D45));
+	ASSERT(check(45, 30, 60, 45,  D45, D30, D60, D45));
+	ASSERT(check(45, 30, 30, 45,  D45, D30, D30, D45));
+
+	libtellurian_azimuth(0, 0, 0, 90, &a1, &a2);
+	ASSERT(a1 == a2);
+	ASSERT(a1 == 90);
+	return 0;
+}
+
+
 #endif
diff --git a/libtellurian_azimuth_radians.c b/libtellurian_azimuth_radians.c
index 18c954e..469eb45 100644
--- a/libtellurian_azimuth_radians.c
+++ b/libtellurian_azimuth_radians.c
@@ -17,9 +17,16 @@ libtellurian_azimuth_radians(double latitude1, double longitude1,
 #else
 
 
-#if 1
-TODO_TEST
-#else
+static int
+eq(double a, double b)
+{
+	if (isfinite(a) && isfinite(b))
+		return a == b;
+	if (isnan(a) && isnan(b))
+		return 1;
+	printf("Unexpected floating-point class\n");
+	return 0;
+}
 
 static int
 check(double a, double b, double c, double d)
@@ -34,7 +41,7 @@ check(double a, double b, double c, double d)
 	libtellurian_azimuth_radians(a, b, c, d, &az1b, NULL);
 	libtellurian_azimuth_radians(a, b, c, d, NULL, &az2b);
 	libtellurian_azimuth_radians(a, b, c, d, NULL, NULL);
-	return az1 == az1_ref && az2 == az2_ref && az1 == az1b && az2 == az2b;
+	return eq(az1, az1_ref) && eq(az2, az2_ref) && eq(az1, az1b) && eq(az2, az2b);
 }
 
 int
@@ -49,7 +56,5 @@ main(void)
 	return 0;
 }
 
-#endif
-
 
 #endif
diff --git a/libtellurian_azimuthal_radius_radians.c b/libtellurian_azimuthal_radius_radians.c
index d58a903..e9433e3 100644
--- a/libtellurian_azimuthal_radius_radians.c
+++ b/libtellurian_azimuthal_radius_radians.c
@@ -6,8 +6,9 @@
 double
 libtellurian_azimuthal_radius_radians(double latitude, double azimuth)
 {
-	double m = libtellurian_meridian_radius_radians(latitude);
-	double n = libtellurian_transverse_radius_radians(latitude);
+	double lat = geodetic(latitude);
+	double m = libtellurian_meridian_radius_radians(lat);
+	double n = libtellurian_transverse_radius_radians(lat);
 	double c2 = cos(azimuth) * cos(azimuth);
 	double s2 = sin(azimuth) * sin(azimuth);
 	double x = c2 / m;
diff --git a/libtellurian_distance.3 b/libtellurian_distance.3
index fc753af..d477f53 100644
--- a/libtellurian_distance.3
+++ b/libtellurian_distance.3
@@ -104,6 +104,29 @@ functions do not return any value.
 .SH ERRORS
 None.
 
+.SH APPLICATION USAGE
+The application should use
+.BR libtellurian_azimuth ()
+or
+.BR libtellurian_azimuth_radians ()
+whenever the distance will not be used by the
+application, likewise, if
+.I *azimuth1_out
+will not be used
+.I azimuth1_out
+should be set to
+.I NULL
+and if
+.I *azimuth2_out
+will not be used
+.I azimuth2_out
+should be set to
+.IR NULL .
+Although this currently has almost no impact on
+the application, future versions of the library
+may select different methods for performing the
+calculations depending on what output is enabled.
+
 .SH NOTES
 The (forward) azimuths are defined as the
 direction you travelling when you travel
diff --git a/libtellurian_distance.c b/libtellurian_distance.c
index 250dddc..dec0edd 100644
--- a/libtellurian_distance.c
+++ b/libtellurian_distance.c
@@ -23,5 +23,48 @@ libtellurian_distance(double latitude1, double longitude1,
 
 
 #else
-TODO_TEST
+
+
+static int
+check(double da, double db, double dc, double dd,
+      double ra, double rb, double rc, double rd)
+{
+	double az1_ref = 1, az2_ref = 2, az1 = 3, az2 = 4, az1b = 5, az2b = 6;
+	double s_ref, s1, s2, s3, s4;
+
+	s_ref = libtellurian_distance_radians(ra, rb, rc, rd, &az1_ref, &az2_ref);
+	az1_ref *= 180 / M_PI;
+	az2_ref *= 180 / M_PI;
+
+	s1 = libtellurian_distance(da, db, dc, dd, &az1, &az2);
+	s2 = libtellurian_distance(da, db, dc, dd, &az1b, NULL);
+	s3 = libtellurian_distance(da, db, dc, dd, NULL, &az2b);
+	s4 = libtellurian_distance(da, db, dc, dd, NULL, NULL);
+	ASSERT(s1 == s_ref);
+	ASSERT(s2 == s_ref);
+	ASSERT(s3 == s_ref);
+	ASSERT(s4 == s_ref);
+	return az1 == az1_ref && az2 == az2_ref && az1 == az1b && az2 == az2b;
+}
+
+int
+main(void)
+{
+	double a1 = 1, a2 = 2, s;
+
+	ASSERT(check(30, 45, 60, 90,  D30, D45, D60, D90));
+	ASSERT(check(30, 45, 60, 45,  D30, D45, D60, D45));
+	ASSERT(check(45, 30, 60, 45,  D45, D30, D60, D45));
+	ASSERT(check(45, 30, 30, 45,  D45, D30, D30, D45));
+
+	s = libtellurian_distance(0, 0, 0, 90, &a1, &a2);
+	fprintf(stderr, "%lg\n", s);
+	fprintf(stderr, "%lg\n", s / LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE);
+	(void) s;
+	ASSERT(a1 == a2);
+	ASSERT(a1 == 90);
+	return 0;
+}
+
+
 #endif
diff --git a/libtellurian_distance_radians.c b/libtellurian_distance_radians.c
index d7c5547..7401edd 100644
--- a/libtellurian_distance_radians.c
+++ b/libtellurian_distance_radians.c
@@ -17,5 +17,34 @@ libtellurian_distance_radians(double latitude1, double longitude1,
 
 
 #else
-TODO_TEST
+
+
+int
+main(void)
+{
+	double rs, ra1, ra2;
+	double s, a1, a2;
+
+	libtellurian_vincenty_inverse__(1, 2, 3, 4, &rs, &ra1, &ra2);
+
+	s = libtellurian_distance_radians(1, 2, 3, 4, &a1, &a2);
+	ASSERT(s == rs);
+	ASSERT(a1 == ra1);
+	ASSERT(a2 == ra2);
+
+	s = libtellurian_distance_radians(1, 2, 3, 4, &a1, NULL);
+	ASSERT(s == rs);
+	ASSERT(a1 == ra1);
+
+	s = libtellurian_distance_radians(1, 2, 3, 4, NULL, &a2);
+	ASSERT(s == rs);
+	ASSERT(a2 == ra2);
+
+	s = libtellurian_distance_radians(1, 2, 3, 4, NULL, NULL);
+	ASSERT(s == rs);
+
+	return 0;
+}
+
+
 #endif
diff --git a/libtellurian_effective_gravity_radians.c b/libtellurian_effective_gravity_radians.c
index 54e2b8e..cb23246 100644
--- a/libtellurian_effective_gravity_radians.c
+++ b/libtellurian_effective_gravity_radians.c
@@ -9,7 +9,8 @@
 double
 libtellurian_effective_gravity_radians(double gravity, double latitude)
 {
-	return fma(-K / gravity, cos(latitude) * cos(latitude), gravity);
+	double lat = geodetic(latitude); /* actual one is geodetic and one is geographical, there is no actual difference  */
+	return fma(-K / gravity, cos(lat) * cos(lat), gravity);
 }
 
 
diff --git a/libtellurian_elevated_gravity_radians.c b/libtellurian_elevated_gravity_radians.c
index 1c97f1a..edf9678 100644
--- a/libtellurian_elevated_gravity_radians.c
+++ b/libtellurian_elevated_gravity_radians.c
@@ -6,6 +6,7 @@
 double
 libtellurian_elevated_gravity_radians(double gravity, double latitude, double altitude)
 {
+	double lat = geodetic(latitude);
 	double a = LIBTELLURIAN_EQUATORIAL_RADIUS;
 	double b = LIBTELLURIAN_POLAR_RADIUS;
 	double omega = LIBTELLURIAN_ANGULAR_VELOCITY;
@@ -15,7 +16,7 @@ libtellurian_elevated_gravity_radians(double gravity, double latitude, double al
 	double neg_k1 = fma(-2.0, f + m, -2.0) / a;
 	double k2 = 4.0 * f / a;
 	double neg_k3 = -3.0 / (a * a);
-	double sin2_phi = sin(latitude) * sin(latitude);
+	double sin2_phi = sin(lat) * sin(lat);
 	double s = fma(neg_k3, altitude, fma(k2, sin2_phi, neg_k1));
 	return fma(s * altitude, gravity, gravity);
 }
diff --git a/libtellurian_end_point.3 b/libtellurian_end_point.3
index 3bf78b2..483c6b0 100644
--- a/libtellurian_end_point.3
+++ b/libtellurian_end_point.3
@@ -83,6 +83,22 @@ None.
 .SH ERRORS
 None.
 
+.SH APPLICATION USAGE
+If
+.I *azimuth2_out
+will not be used by the application
+.I azimuth2_out
+should be set to
+.I NULL
+and likwise for
+.I latitude2_out
+and
+.IR longitude2_out .
+Although this currently has almost no impact on
+the application, future versions of the library
+may select different methods for performing the
+calculations depending on what output is enabled.
+
 .SH NOTES
 The (forward) azimuths are defined as the
 direction you travelling when you travel
diff --git a/libtellurian_end_point_radians.c b/libtellurian_end_point_radians.c
index 036ac99..ff00f5c 100644
--- a/libtellurian_end_point_radians.c
+++ b/libtellurian_end_point_radians.c
@@ -37,7 +37,7 @@ libtellurian_end_point_radians(double latitude1, double longitude1, double azimu
 	double B = fma(fma(fma(-47.0, uu, 74.0), uu, -128.0), uu, 256.0) * (uu / 1024.0);
 
 	double sigma_0 = distance / (b * A);
-	double sigma_prev = 0;
+	double sigma_prev;
 	int max_interations = 20;
 
 	sigma = sigma_0;
diff --git a/libtellurian_gaussian_radius_radians.c b/libtellurian_gaussian_radius_radians.c
index 02d0586..7b14ed5 100644
--- a/libtellurian_gaussian_radius_radians.c
+++ b/libtellurian_gaussian_radius_radians.c
@@ -11,7 +11,7 @@ libtellurian_gaussian_radius_radians(double latitude)
 	double c = b / a;
 	double neg_e2 = fma(c, c, -1.0);
 	double neg_e2_plus_1 = c * c;
-	double s = sin(latitude);
+	double s = sin(geodetic(latitude));
 	double denom = fma(neg_e2, s * s, 1.0);
 	return a * sqrt(neg_e2_plus_1) / denom;
 }
diff --git a/libtellurian_meridian_radius_radians.c b/libtellurian_meridian_radius_radians.c
index f550289..d5213c1 100644
--- a/libtellurian_meridian_radius_radians.c
+++ b/libtellurian_meridian_radius_radians.c
@@ -9,7 +9,7 @@ libtellurian_meridian_radius_radians(double latitude)
 	double a = LIBTELLURIAN_EQUATORIAL_RADIUS;
 	double b = LIBTELLURIAN_POLAR_RADIUS;
 	double neg_e2 = fma(b / a, b / a, -1.0);
-	double s = sin(latitude);
+	double s = sin(geodetic(latitude));
 	return fma(a, neg_e2, a) / pow(fma(neg_e2, s * s, 1.0), 1.5);
 }
 
diff --git a/libtellurian_sea_level_radians.c b/libtellurian_sea_level_radians.c
index e536025..ec03985 100644
--- a/libtellurian_sea_level_radians.c
+++ b/libtellurian_sea_level_radians.c
@@ -6,15 +6,9 @@
 double
 libtellurian_sea_level_radians(double latitude)
 {
-	double a = LIBTELLURIAN_EQUATORIAL_RADIUS;
-	double b = LIBTELLURIAN_POLAR_RADIUS;
-	double c = cos(latitude);
-	double s = sin(latitude);
-	double x = a * c * a;
-	double y = b * s * b;
-	double num = fma(x, x, y * y);
-	double denom = fma(x, c, y * s);
-	return sqrt(num / denom);
+	double x = cos(latitude) * LIBTELLURIAN_EQUATORIAL_RADIUS;
+	double y = sin(latitude) * LIBTELLURIAN_POLAR_RADIUS;
+	return sqrt(fma(x, x, y * y));
 }
 
 
diff --git a/libtellurian_transverse_radius_radians.c b/libtellurian_transverse_radius_radians.c
index c4f80aa..3e4950e 100644
--- a/libtellurian_transverse_radius_radians.c
+++ b/libtellurian_transverse_radius_radians.c
@@ -9,7 +9,7 @@ libtellurian_transverse_radius_radians(double latitude)
 	double a = LIBTELLURIAN_EQUATORIAL_RADIUS;
 	double b = LIBTELLURIAN_POLAR_RADIUS;
 	double neg_e2 = fma(b / a, b / a, -1.0);
-	double s = sin(latitude);
+	double s = sin(geodetic(latitude));
 	return a * pow(fma(neg_e2, s * s, 1.0), -0.5);
 }
 
diff --git a/libtellurian_vincenty_inverse__.c b/libtellurian_vincenty_inverse__.c
index c50752e..4d8d6ea 100644
--- a/libtellurian_vincenty_inverse__.c
+++ b/libtellurian_vincenty_inverse__.c
@@ -1,10 +1,9 @@
 /* See LICENSE file for copyright and license details. */
 #include "common.h"
 #ifndef TEST
+#include <stdio.h> // TODO remove
 
 
-/* TODO complete the implementation */
-
 void
 libtellurian_vincenty_inverse__(double latitude1, double longitude1, double latitude2, double longitude2,
                                 double *distance_out, double *azimuth1_out, double *azimuth2_out)
@@ -34,42 +33,62 @@ libtellurian_vincenty_inverse__(double latitude1, double longitude1, double lati
 	double sin_u1_sin_u2 = sin_u1 * sin_u2;
 
 	double L = longitude2 - longitude1;
+	double lambda_prev;
+	int max_interations = 20;
 
 	lambda = L;
 
-	/* { */
-
-	cos_lambda = cos(lambda);
-	sin_lambda = sin(lambda);
-
-	y = cos_u2 * sin_lambda;
-	x = fma(-sin_u1_cos_u2, cos_lambda, cos_u1_sin_u2);
-
-	sin_sigma = sqrt(fma(y, y, x * x));
-	cos_sigma = fma(cos_u1_cos_u2, cos_lambda, sin_u1_sin_u2);
-	sigma = atan2(sin_sigma, cos_sigma);
-
-	sin_alpha = (cos_u1_cos_u2 * sin_lambda) / sin_sigma;
-	cos2_alpha = fma(-sin_alpha, sin_alpha, 1.0);
-	/*
-	 * If sin σ = 0 the value of sin α is indeterminate.
-	 * It represents an end point coincident with, or
-	 * diametrically opposed to, the start point.
-	 */
-
-	cos_2sigma_m = fma(-2.0 / cos2_alpha, sin_u1_sin_u2, cos_sigma);
-	C = 0.25 * f * cos2_alpha * fma(f, fma(-0.75, cos2_alpha, 1.0), 1.0);
-
-	cos2_2sigma_m = cos_2sigma_m * cos_2sigma_m;
-	t = fma(2.0, cos2_2sigma_m, -1.0);
-	t = fma(C * cos_sigma, t, cos_2sigma_m);
-	t = fma(C * sin_sigma, t, sigma);
-	lambda = fma(fma(C, f, -f) * sin_alpha, t, L);
-
-	/* } repeat until lambda converges */
+	do {
+		lambda_prev = lambda;
+
+		cos_lambda = cos(lambda);
+		sin_lambda = sin(lambda);
+
+		y = cos_u2 * sin_lambda;
+		x = fma(-sin_u1_cos_u2, cos_lambda, cos_u1_sin_u2);
+
+		sin_sigma = sqrt(fma(y, y, x * x));
+		cos_sigma = fma(cos_u1_cos_u2, cos_lambda, sin_u1_sin_u2);
+		sigma = atan2(sin_sigma, cos_sigma);
+
+		sin_alpha = (cos_u1_cos_u2 * sin_lambda) / sin_sigma;
+		if (!isfinite(sin_alpha)) {
+			fprintf(stderr, "BAD\n");
+			if (distance_out) {
+				/* Dot product of vectors is 0 if coincidental, but π if antipodal */
+				/* TODO it may be fast to see if latitude1+latitude2≋0 and longitude2-longitude1≋π */
+				t = sin(latitude1) * sin(latitude2);
+				t = fma(cos(latitude1) * cos(latitude2), cos(longitude2 - longitude1), t);
+				if (t > 0.0) {
+					fprintf(stderr, "  COINCIDENTAL\n");
+					*distance_out = 0;
+				} else {
+					fprintf(stderr, "  ANTIPODAL\n");
+					*distance_out = LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE;
+				}
+			}
+			if (azimuth1_out)
+				*azimuth1_out = nan("");
+			if (azimuth2_out)
+				*azimuth2_out = nan("");
+			return;
+		}
+		cos2_alpha = fma(-sin_alpha, sin_alpha, 1.0);
+
+		cos_2sigma_m = fma(-2.0 / cos2_alpha, sin_u1_sin_u2, cos_sigma);
+		C = 0.25 * f * cos2_alpha * fma(f, fma(-0.75, cos2_alpha, 1.0), 1.0);
+
+		cos2_2sigma_m = cos_2sigma_m * cos_2sigma_m;
+		t = fma(2.0, cos2_2sigma_m, -1.0);
+		t = fma(C * cos_sigma, t, cos_2sigma_m);
+		t = fma(C * sin_sigma, t, sigma);
+		lambda = fma(fma(C, f, -f) * sin_alpha, t, L);
+
+	} while (lambda != lambda_prev && --max_interations); /* repeat until lambda converges */
+	fprintf(stderr, "%lg => %lg -> %lg : %lg (%i)\n", L, lambda_prev, lambda, lambda - lambda_prev, max_interations);
 
 	if (distance_out) {
-		uu = cos2_alpha * fma(a / b, a / b, -1.0);
+		uu = cos2_alpha * fma(c, c, -1.0);
 		A = fma(fma(fma(fma(-175.0, uu, 320.0), uu, -768.0), uu, 4096.0), uu / 16384.0, 1.0);
 		B = fma(fma(fma(-47.0, uu, 74.0), uu, -128.0), uu, 256.0) * (uu / 1024.0);
 
@@ -89,7 +108,7 @@ libtellurian_vincenty_inverse__(double latitude1, double longitude1, double lati
 	if (azimuth2_out) {
 		y = cos_u1 * sin_lambda;
 		x = fma(cos_u1_sin_u2, cos_lambda, -sin_u1_cos_u2);
-		*azimuth1_out = atan2(y, x);
+		*azimuth2_out = atan2(y, x);
 	}
 
 	/*
@@ -106,5 +125,77 @@ libtellurian_vincenty_inverse__(double latitude1, double longitude1, double lati
 
 
 #else
-TODO_TEST
+
+
+# define PE LIBTELLURIAN_EQUATORIAL_CIRCUMFERENCE
+# define PM LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE
+
+static int
+approx(double a, double e)
+{
+        return fabs((a / e) - 1.0) <= 1.0e-8;
+}
+
+int
+main(void)
+{
+	double s, a1, a2;
+
+#define RESET ((void)(s = 111, a1 = 222, a2 = 333))
+
+	libtellurian_vincenty_inverse__(1, 2, 3, 4, &s, &a1, &a2);
+	libtellurian_vincenty_inverse__(1, 2, 3, 4, &s, &a1, NULL);
+	libtellurian_vincenty_inverse__(1, 2, 3, 4, &s, NULL, &a2);
+	libtellurian_vincenty_inverse__(1, 2, 3, 4, &s, NULL, NULL);
+	libtellurian_vincenty_inverse__(1, 2, 3, 4, NULL, &a1, &a2);
+	libtellurian_vincenty_inverse__(1, 2, 3, 4, NULL, &a1, NULL);
+	libtellurian_vincenty_inverse__(1, 2, 3, 4, NULL, NULL, &a2);
+	libtellurian_vincenty_inverse__(1, 2, 3, 4, NULL, NULL, NULL);
+
+	RESET;
+	libtellurian_vincenty_inverse__(0, 0, 0, 0, &s, &a1, &a2);
+	ASSERT(s == 0);
+	ASSERT(isnan(a1));
+	ASSERT(isnan(a2));
+
+	RESET;
+	libtellurian_vincenty_inverse__(0, 0, 0, 0, NULL, &a1, &a2);
+	ASSERT(isnan(a1));
+	ASSERT(isnan(a2));
+
+	RESET;
+	libtellurian_vincenty_inverse__(0, 0, 0, 0, &s, NULL, &a2);
+	ASSERT(s == 0);
+	ASSERT(isnan(a2));
+
+	RESET;
+	libtellurian_vincenty_inverse__(0, 0, 0, 0, NULL, NULL, &a2);
+	ASSERT(isnan(a2));
+
+	RESET;
+	libtellurian_vincenty_inverse__(0, 0, 0, 0, &s, &a1, NULL);
+	ASSERT(s == 0);
+	ASSERT(isnan(a1));
+
+	RESET;
+	libtellurian_vincenty_inverse__(0, 0, 0, 0, NULL, &a1, NULL);
+	ASSERT(isnan(a1));
+
+	RESET;
+	libtellurian_vincenty_inverse__(0, 0, 0, 0, &s, NULL, NULL);
+	ASSERT(s == 0);
+
+	libtellurian_vincenty_inverse__(0, 0, 0, 0, NULL, NULL, NULL);
+
+	fprintf(stderr, "--------\n");
+	RESET;
+	libtellurian_vincenty_inverse__(0, 0, D45, 0, &s, &a1, &a2);
+	ASSERT(approx(s, PM / 4));
+	ASSERT(isnan(a1));
+	ASSERT(isnan(a2));
+
+	return 0;
+}
+
+
 #endif