From e1fc6178fe420b825d97d60d5c43d8b93d62aab8 Mon Sep 17 00:00:00 2001 From: Erin Millard Date: Sun, 28 Apr 2024 16:59:14 +1000 Subject: [PATCH] Add n_E2R_EN function --- src/index.ts | 1 + src/matrix.ts | 23 +++++++++++ src/n_E2R_EN.ts | 49 ++++++++++++++++++++++ test/nvector-test-api.ts | 10 +++++ test/vitest/n_E2R_EN.spec.ts | 79 ++++++++++++++++++++++++++++++++++++ 5 files changed, 162 insertions(+) create mode 100644 src/n_E2R_EN.ts create mode 100644 test/vitest/n_E2R_EN.spec.ts diff --git a/src/index.ts b/src/index.ts index dde3d12..423736c 100644 --- a/src/index.ts +++ b/src/index.ts @@ -1,4 +1,5 @@ export { lat_lon2n_E } from "./lat_lon2n_E.js"; +export { n_E2R_EN } from "./n_E2R_EN.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"; diff --git a/src/matrix.ts b/src/matrix.ts index 42df31a..c3d45e9 100644 --- a/src/matrix.ts +++ b/src/matrix.ts @@ -3,3 +3,26 @@ export type Matrix3x3 = [ [n10: number, n11: number, n12: number], [n20: number, n21: number, n22: number], ]; + +export function multiplyTransposed(a: Matrix3x3, b: Matrix3x3): Matrix3x3 { + const [[a00, a01, a02], [a10, a11, a12], [a20, a21, a22]] = a; + const [[b00, b01, b02], [b10, b11, b12], [b20, b21, b22]] = b; + + return [ + [ + a00 * b00 + a10 * b10 + a20 * b20, + a00 * b01 + a10 * b11 + a20 * b21, + a00 * b02 + a10 * b12 + a20 * b22, + ], + [ + a01 * b00 + a11 * b10 + a21 * b20, + a01 * b01 + a11 * b11 + a21 * b21, + a01 * b02 + a11 * b12 + a21 * b22, + ], + [ + a02 * b00 + a12 * b10 + a22 * b20, + a02 * b01 + a12 * b11 + a22 * b21, + a02 * b02 + a12 * b12 + a22 * b22, + ], + ]; +} diff --git a/src/n_E2R_EN.ts b/src/n_E2R_EN.ts new file mode 100644 index 0000000..a968e75 --- /dev/null +++ b/src/n_E2R_EN.ts @@ -0,0 +1,49 @@ +import { multiplyTransposed, type Matrix3x3 } from "./matrix.js"; +import { ROTATION_MATRIX_e, rotate } from "./rotation.js"; +import type { Vector3 } from "./vector.js"; + +/** + * Calculates the rotation matrix R_EN from an n-vector. + * + * @param n_E - An n-vector decomposed in E. + * @param R_Ee - A rotation matrix defining the axes of the coordinate frame E. + * + * @returns The resulting rotation matrix. + */ +export function n_E2R_EN( + n_E: Vector3, + R_Ee: Matrix3x3 = ROTATION_MATRIX_e, +): Matrix3x3 { + // Based on https://github.com/pbrod/nvector/blob/b8afd89a860a4958d499789607aacb4168dcef87/src/nvector/rotation.py#L478 + const [n_e_x, n_e_y, n_e_z] = rotate(R_Ee, n_E); + + // The z-axis of N (down) points opposite to n-vector + const Nz_e_x = -n_e_x; + const Nz_e_y = -n_e_y; + const Nz_e_z = -n_e_z; + + // Find y-axis of N (East) + // Remember that N is singular at poles + // Ny points perpendicular to the plane formed by n-vector and Earth's spin + // axis + const Ny_e_direction_y = -n_e_z; + const Ny_e_direction_z = n_e_y; + const Ny_e_direction_norm = Math.hypot(Ny_e_direction_y, Ny_e_direction_z); + const on_poles = Math.hypot(Ny_e_direction_y, Ny_e_direction_z) === 0; + // Ny_e_x is always 0, so it's factored out in the following equations + const Ny_e_y = on_poles ? 1 : Ny_e_direction_y / Ny_e_direction_norm; + const Ny_e_z = on_poles ? 0 : Ny_e_direction_z / Ny_e_direction_norm; + + // Find x-axis of N (North) + const Nx_e_x = Ny_e_y * Nz_e_z - Ny_e_z * Nz_e_y; + const Nx_e_y = Ny_e_z * Nz_e_x; + const Nx_e_z = -Ny_e_y * Nz_e_x; + + // Use each component as a column vector, then multiply by the transpose of + // R_Ee to get the rotation matrix R_EN + return multiplyTransposed(R_Ee, [ + [Nx_e_x, 0, Nz_e_x], + [Nx_e_y, Ny_e_y, Nz_e_y], + [Nx_e_z, Ny_e_z, Nz_e_z], + ]); +} diff --git a/test/nvector-test-api.ts b/test/nvector-test-api.ts index 21dae65..917066a 100644 --- a/test/nvector-test-api.ts +++ b/test/nvector-test-api.ts @@ -1,17 +1,20 @@ import { WebSocket } from "ws"; import type { lat_lon2n_E, + n_E2R_EN, 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"; +import type { Matrix3x3 } from "../src/matrix.js"; import type { Vector3 } from "../src/vector.js"; export type NvectorTestClient = { lat_lon2n_E: Async; n_E2lat_lon: Async; + n_E2R_EN: 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; @@ -52,6 +55,13 @@ export async function createNvectorTestClient(): Promise { return [latitude, longitude]; }, + async n_E2R_EN(n_E, R_Ee) { + return await call("n_E2R_EN", { + n_E: wrapVector3(n_E), + R_Ee, + }); + }, + async n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, z_EA, z_EB, a, f, R_Ee) { return unwrapVector3( await call("n_EA_E_and_n_EB_E2p_AB_E", { diff --git a/test/vitest/n_E2R_EN.spec.ts b/test/vitest/n_E2R_EN.spec.ts new file mode 100644 index 0000000..bf2d766 --- /dev/null +++ b/test/vitest/n_E2R_EN.spec.ts @@ -0,0 +1,79 @@ +import { fc, it } from "@fast-check/vitest"; +import { afterAll, beforeAll, describe, expect } from "vitest"; +import { n_E2R_EN } from "../../src/index.js"; +import { ROTATION_MATRIX_e, rotate } from "../../src/rotation.js"; +import { + arbitrary3dRotationMatrix, + arbitrary3dUnitVector, +} from "../arbitrary.js"; +import { + NvectorTestClient, + createNvectorTestClient, +} from "../nvector-test-api.js"; + +const TEST_DURATION = 5000; + +describe("n_E2R_EN()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop( + [ + fc + .tuple( + arbitrary3dUnitVector(), + fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), + ) + .filter(([n_E, R_Ee = ROTATION_MATRIX_e]) => { + // Avoid situations where very close to poles + // Python implementation rounds to zero in these cases, which causes + // the Y axis to be [0, 1, 0] instead of the calculated value, + // producing very different results. + const [, n_e_y, n_e_z] = rotate(R_Ee, n_E); + const Ny_e_direction_norm = Math.hypot(-n_e_z, n_e_y); + if (Ny_e_direction_norm > 0 && Ny_e_direction_norm <= 1e-100) { + return false; + } + + return true; + }), + ], + { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, + )( + "matches the Python implementation", + async ([n_E, R_Ee]) => { + const expected = await nvectorTestClient.n_E2R_EN(n_E, R_Ee); + + expect(expected).toMatchObject([ + [expect.any(Number), expect.any(Number), expect.any(Number)], + [expect.any(Number), expect.any(Number), expect.any(Number)], + [expect.any(Number), expect.any(Number), expect.any(Number)], + ]); + + const actual = n_E2R_EN(n_E, R_Ee); + + expect(actual).toMatchObject([ + [expect.any(Number), expect.any(Number), expect.any(Number)], + [expect.any(Number), expect.any(Number), expect.any(Number)], + [expect.any(Number), expect.any(Number), expect.any(Number)], + ]); + expect(actual[0][0]).toBeCloseTo(expected[0][0], 14); + expect(actual[0][1]).toBeCloseTo(expected[0][1], 14); + expect(actual[0][2]).toBeCloseTo(expected[0][2], 14); + expect(actual[1][0]).toBeCloseTo(expected[1][0], 14); + expect(actual[1][1]).toBeCloseTo(expected[1][1], 14); + expect(actual[1][2]).toBeCloseTo(expected[1][2], 14); + expect(actual[2][0]).toBeCloseTo(expected[2][0], 14); + expect(actual[2][1]).toBeCloseTo(expected[2][1], 14); + expect(actual[2][2]).toBeCloseTo(expected[2][2], 14); + }, + TEST_DURATION + 1000, + ); +});