Add n_EA_E_and_p_AB_E2n_EB_E function

src/index.ts

@@ -1,5 +1,6 @@
 export { lat_lon2n_E } from "./lat_lon2n_E.js";
 export { n_E2lat_lon } from "./n_E2lat_lon.js";
 export { n_EA_E_and_n_EB_E2p_AB_E } from "./n_EA_E_and_n_EB_E2p_AB_E.js";
+export { n_EA_E_and_p_AB_E2n_EB_E } from "./n_EA_E_and_p_AB_E2n_EB_E.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/n_EA_E_and_p_AB_E2n_EB_E.ts

@@ -0,0 +1,43 @@
+import { WGS_84 } from "./ellipsoid.js";
+import type { Matrix3x3 } from "./matrix.js";
+import { n_EB_E2p_EB_E } from "./n_EB_E2p_EB_E.js";
+import { p_EB_E2n_EB_E } from "./p_EB_E2n_EB_E.js";
+import { ROTATION_MATRIX_e } from "./rotation.js";
+import type { Vector3 } from "./vector.js";
+
+/**
+ * Calculates position B from position A and a delta vector decomposed in E.
+ *
+ * @param n_EA_E - An n-vector of position A, decomposed in E.
+ * @param p_AB_E - A Cartesian position vector in meters from A to B, decomposed in E.
+ * @param z_EA - The depth in meters of system A, relative to the ellipsoid.
+ * @param a - The semi-major axis of the Earth ellipsoid given in meters.
+ * @param f - The flattening of the Earth ellipsoid.
+ * @param R_Ee - A rotation matrix defining the axes of the coordinate frame E.
+ *
+ * @returns An n-vector of position B, decomposed in E, and the depth in meters of system B, relative to the ellipsoid.
+ */
+export function n_EA_E_and_p_AB_E2n_EB_E(
+  n_EA_E: Vector3,
+  p_AB_E: Vector3,
+  z_EA: number = 0,
+  a: number = WGS_84.a,
+  f: number = WGS_84.f,
+  R_Ee: Matrix3x3 = ROTATION_MATRIX_e,
+): [n_EB_E: Vector3, z_EB: number] {
+  // Based on https://github.com/pbrod/nvector/blob/b8afd89a860a4958d499789607aacb4168dcef87/src/nvector/core.py#L399
+  const [p_EA_E_x, p_EA_E_y, p_EA_E_z] = n_EB_E2p_EB_E(
+    n_EA_E,
+    z_EA,
+    a,
+    f,
+    R_Ee,
+  );
+  const p_EB_E: Vector3 = [
+    p_EA_E_x + p_AB_E[0],
+    p_EA_E_y + p_AB_E[1],
+    p_EA_E_z + p_AB_E[2],
+  ];
+
+  return p_EB_E2n_EB_E(p_EB_E, a, f, R_Ee);
+}

test/nvector-test-api.ts

@@ -3,6 +3,7 @@ import type {
   lat_lon2n_E,
   n_E2lat_lon,
   n_EA_E_and_n_EB_E2p_AB_E,
+  n_EA_E_and_p_AB_E2n_EB_E,
   n_EB_E2p_EB_E,
   p_EB_E2n_EB_E,
 } from "../src/index.js";
@@ -12,6 +13,7 @@ export type NvectorTestClient = {
   lat_lon2n_E: Async<typeof lat_lon2n_E>;
   n_E2lat_lon: Async<typeof n_E2lat_lon>;
   n_EA_E_and_n_EB_E2p_AB_E: Async<typeof n_EA_E_and_n_EB_E2p_AB_E>;
+  n_EA_E_and_p_AB_E2n_EB_E: Async<typeof n_EA_E_and_p_AB_E2n_EB_E>;
   n_EB_E2p_EB_E: Async<typeof n_EB_E2p_EB_E>;
   p_EB_E2n_EB_E: Async<typeof p_EB_E2n_EB_E>;
 
@@ -64,6 +66,22 @@ export async function createNvectorTestClient(): Promise<NvectorTestClient> {
       );
     },
 
+    async n_EA_E_and_p_AB_E2n_EB_E(n_EA_E, p_AB_E, z_EA, a, f, R_Ee) {
+      const { n_EB_E, z_EB } = await call<{
+        n_EB_E: WrappedVector3;
+        z_EB: number;
+      }>("n_EA_E_and_p_AB_E2n_EB_E", {
+        n_EA_E: wrapVector3(n_EA_E),
+        p_AB_E: wrapVector3(p_AB_E),
+        z_EA,
+        a,
+        f,
+        R_Ee,
+      });
+
+      return [unwrapVector3(n_EB_E), z_EB];
+    },
+
     async n_EB_E2p_EB_E(n_EB_E, depth, a, f, R_Ee) {
       return unwrapVector3(
         await call<WrappedVector3>("n_EB_E2p_EB_E", {

test/vitest/n_EA_E_and_p_AB_E2n_EB_E.spec.ts

@@ -0,0 +1,135 @@
+import { fc, it } from "@fast-check/vitest";
+import { afterAll, beforeAll, describe, expect } from "vitest";
+import { WGS_84 } from "../../src/ellipsoid.js";
+import { n_EA_E_and_p_AB_E2n_EB_E, n_EB_E2p_EB_E } from "../../src/index.js";
+import { ROTATION_MATRIX_e, rotate } from "../../src/rotation.js";
+import type { Vector3 } from "../../src/vector.js";
+import {
+  arbitrary3dRotationMatrix,
+  arbitrary3dUnitVector,
+  arbitraryEllipsoid,
+  arbitraryEllipsoidDepth,
+  arbitraryEllipsoidECEFVector,
+} from "../arbitrary.js";
+import {
+  NvectorTestClient,
+  createNvectorTestClient,
+} from "../nvector-test-api.js";
+
+const TEST_DURATION = 5000;
+
+describe("n_EA_E_and_p_AB_E2n_EB_E()", () => {
+  let nvectorTestClient: NvectorTestClient;
+
+  beforeAll(async () => {
+    nvectorTestClient = await createNvectorTestClient();
+  });
+
+  afterAll(() => {
+    nvectorTestClient?.close();
+  });
+
+  it.prop(
+    [
+      arbitraryEllipsoid()
+        .chain((ellipsoid) => {
+          return fc.tuple(
+            arbitrary3dUnitVector(),
+            arbitraryEllipsoidECEFVector(ellipsoid),
+            fc.option(arbitraryEllipsoidDepth(ellipsoid), { nil: undefined }),
+            fc.option(fc.constant(ellipsoid.a), { nil: undefined }),
+            fc.option(fc.constant(ellipsoid.f), { nil: undefined }),
+            fc.option(arbitrary3dRotationMatrix(), { nil: undefined }),
+          );
+        })
+        .filter(
+          ([
+            n_EA_E,
+            p_AB_E,
+            z_EA,
+            a = WGS_84.a,
+            f = WGS_84.f,
+            R_Ee = ROTATION_MATRIX_e,
+          ]) => {
+            const [p_EA_E_x, p_EA_E_y, p_EA_E_z] = n_EB_E2p_EB_E(
+              n_EA_E,
+              z_EA,
+              a,
+              f,
+              R_Ee,
+            );
+            const p_EB_E: Vector3 = [
+              p_EA_E_x + p_AB_E[0],
+              p_EA_E_y + p_AB_E[1],
+              p_EA_E_z + p_AB_E[2],
+            ];
+
+            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 (Number.isNaN(s) || s <= 0) return false;
+
+            return true;
+          },
+        ),
+    ],
+    { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity },
+  )(
+    "matches the Python implementation",
+    async ([n_EA_E, p_AB_E, z_EA, a, f, R_Ee]) => {
+      const [expectedVector, expectedDepth] =
+        await nvectorTestClient.n_EA_E_and_p_AB_E2n_EB_E(
+          n_EA_E,
+          p_AB_E,
+          z_EA,
+          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] = n_EA_E_and_p_AB_E2n_EB_E(
+        n_EA_E,
+        p_AB_E,
+        z_EA,
+        a,
+        f,
+        R_Ee,
+      );
+
+      expect(actualVector).toMatchObject([
+        expect.any(Number),
+        expect.any(Number),
+        expect.any(Number),
+      ]);
+      expect(actualVector[0]).toBeCloseTo(expectedVector[0], 13);
+      expect(actualVector[1]).toBeCloseTo(expectedVector[1], 13);
+      expect(actualVector[2]).toBeCloseTo(expectedVector[2], 13);
+      expect(actualDepth).toBeCloseTo(expectedDepth, 7);
+    },
+    TEST_DURATION + 1000,
+  );
+});

test/vitest/n_EB_E2p_EB_E.spec.ts

@@ -30,7 +30,7 @@ describe("n_EB_E2p_EB_E()", () => {
       arbitrary3dUnitVector(),
       arbitraryEllipsoid().chain((ellipsoid) => {
         return fc.tuple(
-          arbitraryEllipsoidDepth(ellipsoid),
+          fc.option(arbitraryEllipsoidDepth(ellipsoid), { nil: undefined }),
           fc.option(fc.constant(ellipsoid.a), { nil: undefined }),
           fc.option(fc.constant(ellipsoid.f), { nil: undefined }),
         );

test/vitest/p_EB_E2n_EB_E.spec.ts

@@ -86,9 +86,9 @@ describe("p_EB_E2n_EB_E()", () => {
         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(actualVector[0]).toBeCloseTo(expectedVector[0], 15);
+      expect(actualVector[1]).toBeCloseTo(expectedVector[1], 15);
+      expect(actualVector[2]).toBeCloseTo(expectedVector[2], 15);
       expect(actualDepth).toBeCloseTo(expectedDepth, 8);
     },
     TEST_DURATION + 1000,