From dba0eda86c0856938ca339adedbae5a3edc79b54 Mon Sep 17 00:00:00 2001 From: Erin Millard Date: Sun, 28 Apr 2024 15:04:49 +1000 Subject: [PATCH] Add n_EA_E_and_p_AB_E2n_EB_E function --- src/index.ts | 1 + src/n_EA_E_and_p_AB_E2n_EB_E.ts | 43 ++++++ test/nvector-test-api.ts | 18 +++ test/vitest/n_EA_E_and_p_AB_E2n_EB_E.spec.ts | 135 +++++++++++++++++++ test/vitest/n_EB_E2p_EB_E.spec.ts | 2 +- test/vitest/p_EB_E2n_EB_E.spec.ts | 6 +- 6 files changed, 201 insertions(+), 4 deletions(-) create mode 100644 src/n_EA_E_and_p_AB_E2n_EB_E.ts create mode 100644 test/vitest/n_EA_E_and_p_AB_E2n_EB_E.spec.ts diff --git a/src/index.ts b/src/index.ts index 199657b..dde3d12 100644 --- a/src/index.ts +++ b/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"; diff --git a/src/n_EA_E_and_p_AB_E2n_EB_E.ts b/src/n_EA_E_and_p_AB_E2n_EB_E.ts new file mode 100644 index 0000000..87d7ec5 --- /dev/null +++ b/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); +} diff --git a/test/nvector-test-api.ts b/test/nvector-test-api.ts index 03b7c0f..21dae65 100644 --- a/test/nvector-test-api.ts +++ b/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; n_E2lat_lon: Async; n_EA_E_and_n_EB_E2p_AB_E: Async; + n_EA_E_and_p_AB_E2n_EB_E: Async; n_EB_E2p_EB_E: Async; p_EB_E2n_EB_E: Async; @@ -64,6 +66,22 @@ export async function createNvectorTestClient(): Promise { ); }, + 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("n_EB_E2p_EB_E", { diff --git a/test/vitest/n_EA_E_and_p_AB_E2n_EB_E.spec.ts b/test/vitest/n_EA_E_and_p_AB_E2n_EB_E.spec.ts new file mode 100644 index 0000000..f10c4c2 --- /dev/null +++ b/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, + ); +}); diff --git a/test/vitest/n_EB_E2p_EB_E.spec.ts b/test/vitest/n_EB_E2p_EB_E.spec.ts index ac8968d..93d64e3 100644 --- a/test/vitest/n_EB_E2p_EB_E.spec.ts +++ b/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 }), ); diff --git a/test/vitest/p_EB_E2n_EB_E.spec.ts b/test/vitest/p_EB_E2n_EB_E.spec.ts index aabc9b3..e46161e 100644 --- a/test/vitest/p_EB_E2n_EB_E.spec.ts +++ b/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,