...HEADmaster

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

1 files changed, 126 insertions, 35 deletions

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