Add n_EB_E2p_EB_E function

src/ellipsoid.ts

@@ -0,0 +1,28 @@
+/**
+ * Values from https://github.com/chrisveness/geodesy/blob/761587cd748bd9f7c9825195eba4a9fc5891b859/latlon-ellipsoidal-datum.js#L38
+ */
+
+export type Ellipsoid = {
+  a: number;
+  f: number;
+};
+
+export const GRS_80: Ellipsoid = {
+  a: 6378137,
+  f: 1 / 298.257222101,
+} as const;
+
+export const WGS_72: Ellipsoid = {
+  a: 6378135,
+  f: 1 / 298.26,
+} as const;
+
+export const WGS_84: Ellipsoid = {
+  a: 6378137,
+  f: 1 / 298.257223563,
+} as const;
+
+export const WGS_84_SPHERE: Ellipsoid = {
+  a: WGS_84.a,
+  f: 0,
+} as const;

src/index.ts

@@ -1,2 +1,3 @@
 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";

src/n_EB_E2p_EB_E.ts

@@ -0,0 +1,69 @@
+import { WGS_84 } from "./ellipsoid.js";
+import type { Matrix3x3 } from "./matrix.js";
+import { ROTATION_MATRIX_e } from "./rotation.js";
+import type { Vector3 } from "./vector.js";
+
+/**
+ * Converts n-vector to Cartesian position vector in meters
+ *
+ * Defaults to the WGS-84 ellipsoid. If `f` is `0`, then spherical Earth with
+ * radius `a` is used instead of WGS-84.
+ *
+ * @param n_EB_E - n-vector of position B, decomposed in E
+ * @param depth - Depth in meters of system B, relative to the ellipsoid
+ * @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 Cartesian position vector from E to B, decomposed in E
+ */
+export function n_EB_E2p_EB_E(
+  n_EB_E: Vector3,
+  depth: number = 0,
+  a: number = WGS_84.a,
+  f: number = WGS_84.f,
+  R_Ee: Matrix3x3 = ROTATION_MATRIX_e,
+): Vector3 {
+  // based on https://github.com/pbrod/nvector/blob/b8afd89a860a4958d499789607aacb4168dcef87/src/nvector/core.py#L108
+  const [x, y, z] = n_EB_E;
+  const [[n00, n01, n02], [n10, n11, n12], [n20, n21, n22]] = R_Ee;
+
+  // (flattened) n_EB_e = multiply(R_Ee, n_EB_E)
+  const rx = n00 * x + n01 * y + n02 * z;
+  const ry = n10 * x + n11 * y + n12 * z;
+  const rz = n20 * x + n21 * y + n22 * z;
+
+  // semi-minor axis
+  const b = a * (1 - f);
+
+  // equation (22) in Gade (2010)
+
+  // scale vector
+  const sx = 1;
+  const sy = 1 - f;
+  const sz = 1 - f;
+
+  // denominator = normalize(n_EB_e (vector) / scale (vector))
+  const denominator = Math.sqrt(
+    (rx / sx) ** 2 + (ry / sy) ** 2 + (rz / sz) ** 2,
+  );
+
+  // p_EL_e = b (scalar) / denominator (scalar) * n_EB_e (vector) / scale (vector) ** 2
+  const p_EL_e_ratio = b / denominator;
+  const p_EL_e_x = p_EL_e_ratio * (rx / sx ** 2);
+  const p_EL_e_y = p_EL_e_ratio * (ry / sy ** 2);
+  const p_EL_e_z = p_EL_e_ratio * (rz / sz ** 2);
+
+  // p_EL_e (vector) - n_EB_e (vector) * depth (scalar)
+  // guessing a name of p_EB_e (lowercase) is appropriate, unnamed in original code
+  const p_EB_e_x = p_EL_e_x - rx * depth;
+  const p_EB_e_y = p_EL_e_y - ry * depth;
+  const p_EB_e_z = p_EL_e_z - rz * depth;
+
+  // (flattened) p_EB_E = multiply(transpose(R_Ee), p_EB_e)
+  return [
+    n00 * p_EB_e_x + n10 * p_EB_e_y + n20 * p_EB_e_z,
+    n01 * p_EB_e_x + n11 * p_EB_e_y + n21 * p_EB_e_z,
+    n02 * p_EB_e_x + n12 * p_EB_e_y + n22 * p_EB_e_z,
+  ];
+}

test/nvector-test-api.ts

@@ -11,6 +11,14 @@ export type NvectorTestClient = {
 
   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>;
+
   close: () => void;
 };
 
@@ -43,6 +51,18 @@ export async function createNvectorTestClient(): Promise<NvectorTestClient> {
       );
     },
 
+    async n_EB_E2p_EB_E(n_EB_E, depth, a, f, R_Ee) {
+      return unwrapVector3(
+        await call<WrappedVector3>("n_EB_E2p_EB_E", {
+          n_EB_E: wrapVector3(n_EB_E),
+          depth,
+          a,
+          f,
+          R_Ee,
+        }),
+      );
+    },
+
     close: () => ws.close(),
   };
 

test/vitest/n_EB_E2p_EB_E.spec.ts

@@ -0,0 +1,80 @@
+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 { n_EB_E2p_EB_E } from "../../src/index.js";
+import {
+  arbitrary3dRotationMatrix,
+  arbitrary3dUnitVector,
+} from "../arbitrary.js";
+import {
+  NvectorTestClient,
+  createNvectorTestClient,
+} from "../nvector-test-api.js";
+
+const TEST_DURATION = 5000;
+
+describe("n_EB_E2p_EB_E()", () => {
+  let nvectorTestClient: NvectorTestClient;
+
+  beforeAll(async () => {
+    nvectorTestClient = await createNvectorTestClient();
+  });
+
+  afterAll(() => {
+    nvectorTestClient?.close();
+  });
+
+  it.prop(
+    [
+      arbitrary3dUnitVector(),
+      fc
+        .oneof(
+          fc.constant(WGS_84),
+          fc.constant(WGS_84_SPHERE),
+          fc.constant(WGS_72),
+          fc.constant(GRS_80),
+        )
+        .chain(({ a, f }) =>
+          fc.tuple(
+            // center of spheroid to 2X max radius
+            fc.option(fc.double({ min: -a, max: a, noNaN: true }), {
+              nil: undefined,
+            }),
+            fc.option(fc.constant(a), { nil: undefined }),
+            fc.option(fc.constant(f), { nil: undefined }),
+          ),
+        ),
+      fc.option(arbitrary3dRotationMatrix(), { nil: undefined }),
+    ],
+    { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity },
+  )(
+    "matches the Python implementation",
+    async (n_EB_E, [depth, a, f], R_Ee) => {
+      const expected = await nvectorTestClient.n_EB_E2p_EB_E(
+        n_EB_E,
+        depth,
+        a,
+        f,
+        R_Ee,
+      );
+
+      expect(expected).toMatchObject([
+        expect.any(Number),
+        expect.any(Number),
+        expect.any(Number),
+      ]);
+
+      const actual = n_EB_E2p_EB_E(n_EB_E, depth, a, f, R_Ee);
+
+      expect(actual).toMatchObject([
+        expect.any(Number),
+        expect.any(Number),
+        expect.any(Number),
+      ]);
+      expect(actual[0]).toBeCloseTo(expected[0], 8);
+      expect(actual[1]).toBeCloseTo(expected[1], 8);
+      expect(actual[2]).toBeCloseTo(expected[2], 8);
+    },
+    TEST_DURATION + 1000,
+  );
+});