1 files changed, 79 insertions, 0 deletions
diff --git a/libtellurian_end_point_radians.c b/libtellurian_end_point_radians.c
new file mode 100644
index 0000000..79e759f
--- /dev/null
+++ b/libtellurian_end_point_radians.c
@@ -0,0 +1,79 @@
+/* See LICENSE file for copyright and license details. */
+#include "common.h"
+
+
+/* TODO complete the implementation */
+
+void
+libtellurian_end_point_radians(double latitude1, double longitude1, double azimuth1, double distance,
+                               double *latitude2_out, double *longitude2_out, double *azimuth2_out)
+{
+	/*
+	 * Vincenty's formula for the "direct problem"
+	 */
+	double t, sigma, cos_2sigma_m, sin_sigma, cos_sigma;
+	double x, x0, y, C, L, v, lambda;
+
+	double a = LIBTELLURIAN_EQUATORIAL_RADIUS;
+	double b = LIBTELLURIAN_POLAR_RADIUS;
+	double c = b / a;
+	double f = 1.0 - c;
+
+	double u1 = atan(c * tan(latitude1));
+
+	double sin_u1 = sin(u1);
+	double cos_u1 = cos(u1);
+	double sin_alpha1 = sin(azimuth1);
+	double cos_alpha1 = cos(azimuth1);
+
+	double sigma1 = atan2(tan(u1), cos_alpha1);
+
+	double sin_alpha = cos_u1 * sin_alpha1;
+	double cos2_alpha = fma(-sin_alpha, sin_alpha, 1.0);
+
+	double uu = cos2_alpha * fma(a / b, a / b, -1.0);
+
+	double A = fma(fma(fma(fma(-175.0, uu, 320.0), uu, -768.0), uu, 4096.0), uu / 16384.0, 1.0);
+	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);
+
+	sigma = sigma_0;
+
+	/* { */
+
+	cos_2sigma_m = cos(fma(2.0, sigma1, sigma));
+	sin_sigma = sin(sigma);
+	cos_sigma = cos(sigma);
+
+	v = cos_sigma * fma(2 * cos_2sigma_m, cos_2sigma_m, -1.0);
+	t = fma(2.0 * cos_2sigma_m, 2.0 * cos_2sigma_m, -3.0);
+	t *= fma(2.0 * sin_sigma, 2.0 * sin_sigma, -3.0);
+	t = fma(B * cos_2sigma_m / -6.0, t, v);
+	t = fma(0.25 * B, t, cos_2sigma_m);
+	sigma = fma(B * sin_sigma, t, sigma_0);
+
+	/* } repeat until sigma converges */
+
+	if (latitude2_out || azimuth2_out)
+		x0 = fma(cos_sigma * cos_alpha1, cos_u1, -sin_u1 * sin_alpha);
+
+	if (latitude2_out) {
+		y = fma(sin_sigma * cos_alpha1, cos_u1, sin_u1 * cos_sigma);
+		x = sqrt(fma(sin_alpha, sin_alpha, x0 * x0)) * c;
+		*latitude2_out = atan2(y, x);
+	}
+
+	if (azimuth2_out)
+		*azimuth2_out = atan2(sin_alpha, x0);
+
+	if (longitude2_out) {
+		C = fma(fma(-0.75, cos2_alpha, 1.0), f, 1.0) * cos2_alpha * f / 4.0;
+		y = sin_sigma * sin_alpha1;
+		x = fma(sin_sigma * cos_alpha1, sin_u1, cos_u1 * cos_sigma);
+		lambda = atan2(y, x);
+		t = fma(C * sin_sigma, fma(C, v, cos_2sigma_m), sigma);
+		L = fma(fma(C, f, -f) * sin_alpha, t, lambda);
+		*longitude2_out = longitude1 + L;
+	}
+}