Add p_EB_E2n_EB_E function

src/index.ts

@@ -1,3 +1,4 @@
 export { lat_lon2n_E } from "./lat_lon2n_E.js";
 export { n_E2lat_lon } from "./n_E2lat_lon.js";
 export { n_EB_E2p_EB_E } from "./n_EB_E2p_EB_E.js";
+export { p_EB_E2n_EB_E } from "./p_EB_E2n_EB_E.js";

src/p_EB_E2n_EB_E.ts

@@ -0,0 +1,62 @@
+import { WGS_84 } from "./ellipsoid.js";
+import type { Matrix3x3 } from "./matrix.js";
+import { ROTATION_MATRIX_e, rotate, unrotate } from "./rotation.js";
+import type { Vector3 } from "./vector.js";
+
+/**
+ * Converts Cartesian position vector in meters to n-vector
+ *
+ * Defaults to the WGS-84 ellipsoid. If `f` is `0`, then spherical Earth with
+ * radius `a` is used instead of WGS-84.
+ *
+ * @param p_EB_E - Cartesian position vector from E to B, decomposed in E
+ * @param a - Semi-major axis of the Earth ellipsoid given in meters
+ * @param f - Flattening of the Earth ellipsoid
+ * @param R_Ee - Rotation matrix defining the axes of the coordinate frame E
+ *
+ * @returns n-vector of position B, decomposed in E, and depth in meters of system B, relative to the ellipsoid
+ */
+export function p_EB_E2n_EB_E(
+  p_EB_E: Vector3,
+  a: number = WGS_84.a,
+  f: number = WGS_84.f,
+  R_Ee: Matrix3x3 = ROTATION_MATRIX_e,
+): [n_EB_E: Vector3, depth: number] {
+  // based on https://github.com/pbrod/nvector/blob/b8afd89a860a4958d499789607aacb4168dcef87/src/nvector/core.py#L212
+  const p_EB_e = rotate(R_Ee, p_EB_E);
+
+  // equation (23) from Gade (2010)
+  const Ryz_2 = p_EB_e[1] ** 2 + p_EB_e[2] ** 2;
+  const Rx_2 = p_EB_e[0] ** 2;
+  const e_2 = (2.0 - f) * f;
+  const q = ((1 - e_2) / a ** 2) * Rx_2;
+  const Ryz = Math.sqrt(Ryz_2);
+  const p = Ryz_2 / a ** 2;
+  const r = (p + q - e_2 ** 2) / 6;
+  const s = (e_2 ** 2 * p * q) / (4 * r ** 3);
+  const t = Math.cbrt(1 + s + Math.sqrt(s * (2 + s)));
+  const u = r * (1 + t + 1.0 / t);
+  const v = Math.sqrt(u ** 2 + e_2 ** 2 * q);
+  const w = (e_2 * (u + v - q)) / (2 * v);
+  const k = Math.sqrt(u + v + w ** 2) - w;
+  const d = (k * Ryz) / (k + e_2);
+  const temp0 = Math.sqrt(d ** 2 + Rx_2);
+  const height = ((k + e_2 - 1) / k) * temp0;
+  const x_scale = 1.0 / temp0;
+  const yz_scale = (x_scale * k) / (k + e_2);
+  const depth = -height;
+
+  const n_EB_e: Vector3 = [
+    x_scale * p_EB_e[0],
+    yz_scale * p_EB_e[1],
+    yz_scale * p_EB_e[2],
+  ];
+
+  const [x, y, z] = unrotate(R_Ee, n_EB_e);
+
+  // ensure unit length
+  const norm = Math.hypot(x, y, z);
+  const n_EB_E: Vector3 = [x / norm, y / norm, z / norm];
+
+  return [n_EB_E, depth];
+}

test/arbitrary.ts

@@ -25,6 +25,14 @@ export function arbitrary3dRotationMatrix(): fc.Arbitrary<Matrix3x3> {
   });
 }
 
+export function arbitrary3dVector(
+  magnitudeConstraints: fc.DoubleConstraints,
+): fc.Arbitrary<Vector3> {
+  return fc
+    .tuple(arbitrary3dUnitVector(), fc.double(magnitudeConstraints))
+    .map(([[x, y, z], m]) => [x * m, y * m, z * m]);
+}
+
 export function arbitrary3dUnitVector(): fc.Arbitrary<Vector3> {
   // based on https://github.com/mrdoob/three.js/blob/a2e9ee8204b67f9dca79f48cf620a34a05aa8126/src/math/Vector3.js#L695
   // https://mathworld.wolfram.com/SpherePointPicking.html

test/nvector-test-api.ts

@@ -1,23 +1,17 @@
 import { WebSocket } from "ws";
-import type { Matrix3x3 } from "../src/matrix.js";
+import type {
+  lat_lon2n_E,
+  n_E2lat_lon,
+  n_EB_E2p_EB_E,
+  p_EB_E2n_EB_E,
+} from "../src/index.js";
 import type { Vector3 } from "../src/vector.js";
 
 export type NvectorTestClient = {
-  lat_lon2n_E: (
-    latitude: number,
-    longitude: number,
-    R_Ee?: Matrix3x3,
-  ) => Promise<Vector3>;
-
-  n_E2lat_lon: (n_E: Vector3, R_Ee?: Matrix3x3) => Promise<LatLonTuple>;
-
-  n_EB_E2p_EB_E: (
-    n_EB_E: Vector3,
-    depth?: number,
-    a?: number,
-    f?: number,
-    R_Ee?: Matrix3x3,
-  ) => Promise<Vector3>;
+  lat_lon2n_E: Async<typeof lat_lon2n_E>;
+  n_E2lat_lon: Async<typeof n_E2lat_lon>;
+  n_EB_E2p_EB_E: Async<typeof n_EB_E2p_EB_E>;
+  p_EB_E2n_EB_E: Async<typeof p_EB_E2n_EB_E>;
 
   close: () => void;
 };
@@ -43,12 +37,15 @@ export async function createNvectorTestClient(): Promise<NvectorTestClient> {
     },
 
     async n_E2lat_lon(n_E, R_Ee) {
-      return latLonObjectToTuple(
-        await call<LatLonObject>("n_E2lat_lon", {
-          n_E: wrapVector3(n_E),
-          R_Ee,
-        }),
-      );
+      const { latitude, longitude } = await call<{
+        latitude: number;
+        longitude: number;
+      }>("n_E2lat_lon", {
+        n_E: wrapVector3(n_E),
+        R_Ee,
+      });
+
+      return [latitude, longitude];
     },
 
     async n_EB_E2p_EB_E(n_EB_E, depth, a, f, R_Ee) {
@@ -63,6 +60,20 @@ export async function createNvectorTestClient(): Promise<NvectorTestClient> {
       );
     },
 
+    async p_EB_E2n_EB_E(p_EB_E, a, f, R_Ee) {
+      const { n_EB_E, depth } = await call<{
+        n_EB_E: WrappedVector3;
+        depth: number;
+      }>("p_EB_E2n_EB_E", {
+        p_EB_E: wrapVector3(p_EB_E),
+        a,
+        f,
+        R_Ee,
+      });
+
+      return [unwrapVector3(n_EB_E), depth];
+    },
+
     close: () => ws.close(),
   };
 
@@ -104,19 +115,6 @@ type ErrorMessage = {
   result: undefined;
 };
 
-type LatLonTuple = [latitude: number, longitude: number];
-type LatLonObject = {
-  latitude: number;
-  longitude: number;
-};
-
-function latLonObjectToTuple({
-  latitude,
-  longitude,
-}: LatLonObject): LatLonTuple {
-  return [latitude, longitude];
-}
-
 type WrappedVector3 = [[x: number], [y: number], [z: number]];
 
 function wrapVector3([x, y, z]: Vector3): WrappedVector3 {
@@ -126,3 +124,7 @@ function wrapVector3([x, y, z]: Vector3): WrappedVector3 {
 function unwrapVector3([[x], [y], [z]]: WrappedVector3): Vector3 {
   return [x, y, z];
 }
+
+type Async<T> = T extends (...args: infer A) => infer R
+  ? (...args: A) => Promise<R>
+  : never;

test/vitest/n_EB_E2p_EB_E.spec.ts

@@ -36,8 +36,8 @@ describe("n_EB_E2p_EB_E()", () => {
         )
         .chain(({ a, f }) =>
           fc.tuple(
-            // center of spheroid to 2X max radius
-            fc.option(fc.double({ min: -a, max: a, noNaN: true }), {
+            // 1m from center of spheroid to 2X max radius
+            fc.option(fc.double({ min: 1 - a, max: a, noNaN: true }), {
               nil: undefined,
             }),
             fc.option(fc.constant(a), { nil: undefined }),

test/vitest/p_EB_E2n_EB_E.spec.ts

@@ -0,0 +1,99 @@
+import { fc, it } from "@fast-check/vitest";
+import { afterAll, beforeAll, describe, expect } from "vitest";
+import { GRS_80, WGS_72, WGS_84, WGS_84_SPHERE } from "../../src/ellipsoid.js";
+import { p_EB_E2n_EB_E } from "../../src/index.js";
+import { ROTATION_MATRIX_e, rotate } from "../../src/rotation.js";
+import { arbitrary3dRotationMatrix, arbitrary3dVector } from "../arbitrary.js";
+import {
+  NvectorTestClient,
+  createNvectorTestClient,
+} from "../nvector-test-api.js";
+
+const TEST_DURATION = 5000;
+
+describe("p_EB_E2n_EB_E()", () => {
+  let nvectorTestClient: NvectorTestClient;
+
+  beforeAll(async () => {
+    nvectorTestClient = await createNvectorTestClient();
+  });
+
+  afterAll(() => {
+    nvectorTestClient?.close();
+  });
+
+  it.prop(
+    [
+      fc
+        .oneof(
+          fc.constant(WGS_84),
+          fc.constant(WGS_84_SPHERE),
+          fc.constant(WGS_72),
+          fc.constant(GRS_80),
+        )
+        .chain(({ a, f }) =>
+          fc.tuple(
+            // 1m from center of spheroid to 2X max radius
+            arbitrary3dVector({ min: 1, max: a * 2, noNaN: true }),
+            fc.option(fc.constant(a), { nil: undefined }),
+            fc.option(fc.constant(f), { nil: undefined }),
+            fc.option(arbitrary3dRotationMatrix(), { nil: undefined }),
+          ),
+        )
+        .filter(
+          ([p_EB_E, a = WGS_84.a, f = WGS_84.f, R_Ee = ROTATION_MATRIX_e]) => {
+            const p_EB_e = rotate(R_Ee, p_EB_E);
+
+            // filter vectors where the x or yz components are zero after
+            // rotation
+            // this causes a division by zero in the Python implementation
+            if (p_EB_e[0] === 0 || p_EB_e[1] + p_EB_e[2] === 0) return false;
+
+            // filter a case that makes the Python implementation try to find
+            // the square root of a negative number
+            // not sure why this happens, the math is beyond me
+            const s = (() => {
+              const Ryz_2 = p_EB_E[1] ** 2 + p_EB_E[2] ** 2;
+              const Rx_2 = p_EB_E[0] ** 2;
+              const e_2 = (2.0 - f) * f;
+              const q = ((1 - e_2) / a ** 2) * Rx_2;
+              const p = Ryz_2 / a ** 2;
+              const r = (p + q - e_2 ** 2) / 6;
+
+              return (e_2 ** 2 * p * q) / (4 * r ** 3);
+            })();
+            if (s <= 0) return false;
+
+            return true;
+          },
+        ),
+    ],
+    { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity },
+  )(
+    "matches the Python implementation",
+    async ([p_EB_E, a, f, R_Ee]) => {
+      const [expectedVector, expectedDepth] =
+        await nvectorTestClient.p_EB_E2n_EB_E(p_EB_E, a, f, R_Ee);
+
+      expect(expectedVector).toMatchObject([
+        expect.any(Number),
+        expect.any(Number),
+        expect.any(Number),
+      ]);
+      expect(expectedDepth).toEqual(expect.any(Number));
+
+      const [actualVector, actualDepth] = p_EB_E2n_EB_E(p_EB_E, a, f, R_Ee);
+
+      expect(actualVector).toMatchObject([
+        expect.any(Number),
+        expect.any(Number),
+        expect.any(Number),
+      ]);
+      expect(actualVector[0]).toBeCloseTo(expectedVector[0], 8);
+      expect(actualVector[1]).toBeCloseTo(expectedVector[1], 8);
+      expect(actualVector[2]).toBeCloseTo(expectedVector[2], 8);
+      expect(actualDepth).toBeCloseTo(expectedDepth, 8);
+    },
+    TEST_DURATION + 1000,
+  );
+});