diff --git a/src/ellipsoid.ts b/src/ellipsoid.ts new file mode 100644 index 0000000..716eca5 --- /dev/null +++ b/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; diff --git a/src/index.ts b/src/index.ts index a02e87d..1413cc3 100644 --- a/src/index.ts +++ b/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"; diff --git a/src/n_EB_E2p_EB_E.ts b/src/n_EB_E2p_EB_E.ts new file mode 100644 index 0000000..2f6bcc0 --- /dev/null +++ b/src/n_EB_E2p_EB_E.ts @@ -0,0 +1,57 @@ +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 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] = rotate(R_Ee, n_EB_E); + + // semi-minor axis + const b = a * (1 - f); + + // 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((x / sx) ** 2 + (y / sy) ** 2 + (z / 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 * (x / sx ** 2); + const p_EL_e_y = p_EL_e_ratio * (y / sy ** 2); + const p_EL_e_z = p_EL_e_ratio * (z / sz ** 2); + + // p_EL_e (vector) - n_EB_e (vector) * depth (scalar) + // guessing a name of p_EB_e (lowercase) is appropriate + // this is an unnamed expression in the original code + const p_EB_e: Vector3 = [ + p_EL_e_x - x * depth, + p_EL_e_y - y * depth, + p_EL_e_z - z * depth, + ]; + + return unrotate(R_Ee, p_EB_e); +} diff --git a/test/nvector-test-api.ts b/test/nvector-test-api.ts index 4aa2300..199b2bc 100644 --- a/test/nvector-test-api.ts +++ b/test/nvector-test-api.ts @@ -11,6 +11,14 @@ export type NvectorTestClient = { n_E2lat_lon: (n_E: Vector3, R_Ee?: Matrix3x3) => Promise; + n_EB_E2p_EB_E: ( + n_EB_E: Vector3, + depth?: number, + a?: number, + f?: number, + R_Ee?: Matrix3x3, + ) => Promise; + close: () => void; }; @@ -43,6 +51,18 @@ export async function createNvectorTestClient(): Promise { ); }, + async n_EB_E2p_EB_E(n_EB_E, depth, a, f, R_Ee) { + return unwrapVector3( + await call("n_EB_E2p_EB_E", { + n_EB_E: wrapVector3(n_EB_E), + depth, + a, + f, + R_Ee, + }), + ); + }, + close: () => ws.close(), }; diff --git a/test/vitest/n_EB_E2p_EB_E.spec.ts b/test/vitest/n_EB_E2p_EB_E.spec.ts new file mode 100644 index 0000000..a98b770 --- /dev/null +++ b/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, + ); +});