From 28b572ec8096c56db364a94f655c5efd983307d4 Mon Sep 17 00:00:00 2001 From: Erin Millard Date: Tue, 28 May 2024 17:55:32 +1000 Subject: [PATCH] Reorganize the entire library - Improve all function and argument names. - Improve examples. --- README.md | 879 +++++++++++------- src/R2xyz.ts | 63 -- src/R2zyx.ts | 66 -- src/R_EL2n_E.ts | 16 - src/R_EN2n_E.ts | 16 - src/angle.ts | 25 + src/{rotation.ts => coord-frame.ts} | 26 +- src/coords.ts | 63 ++ src/deg.ts | 12 - src/delta.ts | 63 ++ src/ecef.ts | 109 +++ src/ellipsoid.ts | 27 +- src/euler.ts | 157 ++++ src/index.ts | 42 +- src/lat_long2n_E.ts | 31 - src/matrix.ts | 24 +- src/n_E2R_EN.ts | 60 -- src/n_E2lat_long.ts | 34 - src/n_EA_E_and_n_EB_E2p_AB_E.ts | 55 -- src/n_EA_E_and_p_AB_E2n_EB_E.ts | 52 -- src/n_EB_E2p_EB_E.ts | 60 -- src/n_E_and_wa2R_EL.ts | 36 - src/p_EB_E2n_EB_E.ts | 82 -- src/rad.ts | 12 - src/rotation-matrix.ts | 108 +++ src/unit.ts | 21 - src/vector.ts | 58 +- src/xyz2R.ts | 42 - src/zyx2R.ts | 44 - test/arbitrary.ts | 32 +- test/nvector-test-api.ts | 172 ++-- test/vitest/R2xyz.spec.ts | 59 -- test/vitest/R2zyx.spec.ts | 59 -- test/vitest/R_EL2n_E.spec.ts | 48 - test/vitest/R_EN2n_E.spec.ts | 48 - test/vitest/angle.spec.ts | 15 + test/vitest/coords.spec.ts | 99 ++ test/vitest/delta.spec.ts | 185 ++++ test/vitest/ecef.spec.ts | 154 +++ test/vitest/ellipsoid.spec.ts | 32 + test/vitest/euler.spec.ts | 216 +++++ test/vitest/examples/example-01.spec.ts | 108 ++- test/vitest/examples/example-02.spec.ts | 100 +- test/vitest/examples/example-03.spec.ts | 42 +- test/vitest/examples/example-04.spec.ts | 28 +- test/vitest/examples/example-05.spec.ts | 74 +- test/vitest/examples/example-06.spec.ts | 76 +- test/vitest/examples/example-07.spec.ts | 62 +- test/vitest/examples/example-08.spec.ts | 125 +-- test/vitest/examples/example-09.spec.ts | 92 +- test/vitest/examples/example-10.spec.ts | 88 +- test/vitest/lat_long2n_E.spec.ts | 55 -- test/vitest/n_E2R_EN.spec.ts | 85 -- test/vitest/n_E2lat_long.spec.ts | 45 - test/vitest/n_EA_E_and_n_EB_E2p_AB_E.spec.ts | 85 -- test/vitest/n_EA_E_and_p_AB_E2n_EB_E.spec.ts | 136 --- test/vitest/n_EB_E2p_EB_E.spec.ts | 69 -- test/vitest/n_E_and_wa2R_EL.spec.ts | 92 -- test/vitest/p_EB_E2n_EB_E.spec.ts | 92 -- test/vitest/rotation-matrix.spec.ts | 220 +++++ test/vitest/unit.spec.ts | 9 - test/vitest/{apply.spec.ts => vector.spec.ts} | 13 +- test/vitest/xyz2R.spec.ts | 54 -- test/vitest/zyx2R.spec.ts | 54 -- 64 files changed, 2581 insertions(+), 2625 deletions(-) delete mode 100644 src/R2xyz.ts delete mode 100644 src/R2zyx.ts delete mode 100644 src/R_EL2n_E.ts delete mode 100644 src/R_EN2n_E.ts create mode 100644 src/angle.ts rename src/{rotation.ts => coord-frame.ts} (65%) create mode 100644 src/coords.ts delete mode 100644 src/deg.ts create mode 100644 src/delta.ts create mode 100644 src/ecef.ts create mode 100644 src/euler.ts delete mode 100644 src/lat_long2n_E.ts delete mode 100644 src/n_E2R_EN.ts delete mode 100644 src/n_E2lat_long.ts delete mode 100644 src/n_EA_E_and_n_EB_E2p_AB_E.ts delete mode 100644 src/n_EA_E_and_p_AB_E2n_EB_E.ts delete mode 100644 src/n_EB_E2p_EB_E.ts delete mode 100644 src/n_E_and_wa2R_EL.ts delete mode 100644 src/p_EB_E2n_EB_E.ts delete mode 100644 src/rad.ts create mode 100644 src/rotation-matrix.ts delete mode 100644 src/unit.ts delete mode 100644 src/xyz2R.ts delete mode 100644 src/zyx2R.ts delete mode 100644 test/vitest/R2xyz.spec.ts delete mode 100644 test/vitest/R2zyx.spec.ts delete mode 100644 test/vitest/R_EL2n_E.spec.ts delete mode 100644 test/vitest/R_EN2n_E.spec.ts create mode 100644 test/vitest/angle.spec.ts create mode 100644 test/vitest/coords.spec.ts create mode 100644 test/vitest/delta.spec.ts create mode 100644 test/vitest/ecef.spec.ts create mode 100644 test/vitest/ellipsoid.spec.ts create mode 100644 test/vitest/euler.spec.ts delete mode 100644 test/vitest/lat_long2n_E.spec.ts delete mode 100644 test/vitest/n_E2R_EN.spec.ts delete mode 100644 test/vitest/n_E2lat_long.spec.ts delete mode 100644 test/vitest/n_EA_E_and_n_EB_E2p_AB_E.spec.ts delete mode 100644 test/vitest/n_EA_E_and_p_AB_E2n_EB_E.spec.ts delete mode 100644 test/vitest/n_EB_E2p_EB_E.spec.ts delete mode 100644 test/vitest/n_E_and_wa2R_EL.spec.ts delete mode 100644 test/vitest/p_EB_E2n_EB_E.spec.ts create mode 100644 test/vitest/rotation-matrix.spec.ts delete mode 100644 test/vitest/unit.spec.ts rename test/vitest/{apply.spec.ts => vector.spec.ts} (67%) delete mode 100644 test/vitest/xyz2R.spec.ts delete mode 100644 test/vitest/zyx2R.spec.ts diff --git a/README.md b/README.md index bf23ec7..455f27c 100644 --- a/README.md +++ b/README.md @@ -18,9 +18,11 @@ _Functions for performing geographical position calculations using n-vectors_ [badge-version-link]: https://npmjs.com/package/nvector-geodesy This library is a lightweight (<2kB), dependency-free port of the [Matlab -n-vector library] by [Kenneth Gade]. All original functions are included, plus -some extras for vector and matrix operations needed to solve the [10 examples -from the n-vector page]. +n-vector library] by [Kenneth Gade]. All original functions are included, +although the names of the functions and arguments have been changed in an +attempt to clarify their purpose. In addition, this library includes some extra +functions for vector and matrix operations needed to solve the [10 examples from +the n-vector page]. [matlab n-vector library]: https://github.com/FFI-no/n-vector [kenneth gade]: https://github.com/KennethGade @@ -54,52 +56,92 @@ using this library. ```ts import { - lat_long2n_E, - n_E2R_EN, - n_EA_E_and_n_EB_E2p_AB_E, - rad, - rotate, + delta, + fromGeodeticCoordinates, + radians, + toRotationMatrix, + transform, transpose, } from "nvector-geodesy"; - -// Positions A and B are given in (decimal) degrees and depths: - -// Position A: -const lat_EA_deg = 1; -const long_EA_deg = 2; -const z_EA = 3; - -// Position B: -const lat_EB_deg = 4; -const long_EB_deg = 5; -const z_EB = 6; - -// Find the exact vector between the two positions, given in meters north, -// east, and down, i.e. find p_AB_N. - -// SOLUTION: - -// Step1: Convert to n-vectors (rad() converts to radians): -const n_EA_E = lat_long2n_E(rad(lat_EA_deg), rad(long_EA_deg)); -const n_EB_E = lat_long2n_E(rad(lat_EB_deg), rad(long_EB_deg)); - -// Step2: Find p_AB_E (delta decomposed in E). WGS-84 ellipsoid is default: -const p_AB_E = n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, z_EA, z_EB); - -// Step3: Find R_EN for position A: -const R_EN = n_E2R_EN(n_EA_E); - -// Step4: Find p_AB_N -const p_AB_N = rotate(transpose(R_EN), p_AB_E); -// (Note the transpose of R_EN: The "closest-rule" says that when decomposing, -// the frame in the subscript of the rotation matrix that is closest to the -// vector, should equal the frame where the vector is decomposed. Thus the -// calculation R_NE*p_AB_E is correct, since the vector is decomposed in E, -// and E is closest to the vector. In the above example we only had R_EN, and -// thus we must transpose it: R_EN' = R_NE) - -// Step5: Also find the direction (azimuth) to B, relative to north: -const azimuth = Math.atan2(p_AB_N[1], p_AB_N[0]); +import { expect, test } from "vitest"; + +/** + * Example 1: A and B to delta + * + * Given two positions A and B. Find the exact vector from A to B in meters + * north, east and down, and find the direction (azimuth/bearing) to B, relative + * to north. Use WGS-84 ellipsoid. + * + * @see https://www.ffi.no/en/research/n-vector/#example_1 + */ +test("Example 1", () => { + // PROBLEM: + + // Given two positions, A and B as latitudes, longitudes and depths (relative + // to Earth, E): + const aLat = 1, + aLon = 2, + aDepth = 3; + const bLat = 4, + bLon = 5, + bDepth = 6; + + // Find the exact vector between the two positions, given in meters north, + // east, and down, and find the direction (azimuth) to B, relative to north. + // + // Details: + // + // - Assume WGS-84 ellipsoid. The given depths are from the ellipsoid surface. + // - Use position A to define north, east, and down directions. (Due to the + // curvature of Earth and different directions to the North Pole, the north, + // east, and down directions will change (relative to Earth) for different + // places. Position A must be outside the poles for the north and east + // directions to be defined. + + // SOLUTION: + + // Step 1 + // + // First, the given latitudes and longitudes are converted to n-vectors: + const a = fromGeodeticCoordinates(radians(aLat), radians(aLon)); + const b = fromGeodeticCoordinates(radians(bLat), radians(bLon)); + + // Step 2 + // + // When the positions are given as n-vectors (and depths), it is easy to find + // the delta vector decomposed in E. No ellipsoid is specified when calling + // the function, thus WGS-84 (default) is used: + const abE = delta(a, b, aDepth, bDepth); + + // Step 3 + // + // We now have the delta vector from A to B, but the three coordinates of the + // vector are along the Earth coordinate frame E, while we need the + // coordinates to be north, east and down. To get this, we define a + // North-East-Down coordinate frame called N, and then we need the rotation + // matrix (direction cosine matrix) rEN to go between E and N. We have a + // simple function that calculates rEN from an n-vector, and we use this + // function (using the n-vector at position A): + const rEN = toRotationMatrix(a); + + // Step 4 + // + // Now the delta vector is easily decomposed in N. Since the vector is + // decomposed in E, we must use rNE (rNE is the transpose of rEN): + const abN = transform(transpose(rEN), abE); + + // Step 5 + // + // The three components of abN are the north, east and down displacements from + // A to B in meters. The azimuth is simply found from element 1 and 2 of the + // vector (the north and east components): + const azimuth = Math.atan2(abN[1], abN[0]); + + expect(abN[0]).toBeCloseTo(331730.2347808944, 8); + expect(abN[1]).toBeCloseTo(332997.8749892695, 8); + expect(abN[2]).toBeCloseTo(17404.27136193635, 8); + expect(azimuth).toBeCloseTo(radians(45.10926323826139), 15); +}); ``` ### Example 2: B and delta to C @@ -114,58 +156,85 @@ const azimuth = Math.atan2(p_AB_N[1], p_AB_N[0]); ```ts import { WGS_72, - deg, + degrees, + destination, + eulerZYXToRotationMatrix, multiply, - n_E2R_EN, - n_E2lat_long, - n_EA_E_and_p_AB_E2n_EB_E, - rad, - rotate, - unit, - zyx2R, - type Vector3, + normalize, + radians, + toGeodeticCoordinates, + toRotationMatrix, + transform, + type Vector, } from "nvector-geodesy"; - -// delta vector from B to C, decomposed in B is given: -const p_BC_B: Vector3 = [3000, 2000, 100]; - -// Position and orientation of B is given: -// unit to get unit length of vector -const n_EB_E = unit([1, 2, 3]); -const z_EB = -400; -// the three angles are yaw, pitch, and roll -const R_NB = zyx2R(rad(10), rad(20), rad(30)); - -// A custom reference ellipsoid is given (replacing WGS-84): -// (WGS-72) -const a = WGS_72.a; -const f = WGS_72.f; - -// Find the position of C. - -// SOLUTION: - -// Step1: Find R_EN: -const R_EN = n_E2R_EN(n_EB_E); - -// Step2: Find R_EB, from R_EN and R_NB: -// Note: closest frames cancel -const R_EB = multiply(R_EN, R_NB); - -// Step3: Decompose the delta vector in E: -// no transpose of R_EB, since the vector is in B -const p_BC_E = rotate(R_EB, p_BC_B); - -// Step4: Find the position of C, using the functions that goes from one -// position and a delta, to a new position: -const [n_EC_E, z_EC] = n_EA_E_and_p_AB_E2n_EB_E(n_EB_E, p_BC_E, z_EB, a, f); - -// When displaying the resulting position for humans, it is more convenient -// to see lat, long: -const [lat_EC, long_EC] = n_E2lat_long(n_EC_E); - -// Here we also assume that the user wants the output to be height (= -depth): -const height = -z_EC; +import { expect, test } from "vitest"; + +/** + * Example 2: B and delta to C + * + * Given the position of vehicle B and a bearing and distance to an object C. + * Find the exact position of C. Use WGS-72 ellipsoid. + * + * @see https://www.ffi.no/en/research/n-vector/#example_2 + */ +test("Example 2", () => { + // PROBLEM: + + // A radar or sonar attached to a vehicle B (Body coordinate frame) measures + // the distance and direction to an object C. We assume that the distance and + // two angles measured by the sensor (typically bearing and elevation relative + // to B) are already converted (by converting from spherical to Cartesian + // coordinates) to the vector bcB (i.e. the vector from B to C, decomposed in + // B): + const bcB: Vector = [3000, 2000, 100]; + + // The position of B is given as an n-vector and a depth: + const b = normalize([1, 2, 3]); + const bDepth = -400; + + // The orientation (attitude) of B is given as rNB, specified as yaw, pitch, + // roll: + const rNB = eulerZYXToRotationMatrix(radians(10), radians(20), radians(30)); + + // Use the WGS-72 ellipsoid: + const e = WGS_72; + + // Find the exact position of object C as an n-vector and a depth. + + // SOLUTION: + + // Step 1 + // + // The delta vector is given in B. It should be decomposed in E before using + // it, and thus we need rEB. This matrix is found from the matrices rEN and + // rNB, and we need to find rEN, as in Example 1: + const rEN = toRotationMatrix(b); + + // Step 2 + // + // Now, we can find rEB y using that the closest frames cancel when + // multiplying two rotation matrices (i.e. N is cancelled here): + const rEB = multiply(rEN, rNB); + + // Step 3 + // + // The delta vector is now decomposed in E: + const bcE = transform(rEB, bcB); + + // Step 4 + // + // It is now easy to find the position of C using destination (with custom + // ellipsoid overriding the default WGS-84): + const [c, cDepth] = destination(b, bcE, bDepth, e); + + // Use human-friendly outputs: + const [lat, lon] = toGeodeticCoordinates(c); + const height = -cDepth; + + expect(degrees(lat)).toBeCloseTo(53.32637826433107, 13); + expect(degrees(lon)).toBeCloseTo(63.46812343514746, 13); + expect(height).toBeCloseTo(406.0071960700098, 15); +}); ``` ### Example 3: ECEF-vector to geodetic latitude @@ -180,25 +249,48 @@ const height = -z_EC; ```ts import { apply, - deg, - n_E2lat_long, - p_EB_E2n_EB_E, - type Vector3, + degrees, + fromECEF, + toGeodeticCoordinates, + type Vector, } from "nvector-geodesy"; - -// Position B is given as p_EB_E ("ECEF-vector") -const p_EB_E: Vector3 = apply((n) => n * 6371e3, [0.71, -0.72, 0.1]); // m - -// Find position B as geodetic latitude, longitude and height - -// SOLUTION: - -// Find n-vector from the p-vector: -const [n_EB_E, z_EB] = p_EB_E2n_EB_E(p_EB_E); - -// Convert to lat, long and height: -const [lat_EB, long_EB] = n_E2lat_long(n_EB_E); -const h_EB = -z_EB; +import { expect, test } from "vitest"; + +/** + * Example 3: ECEF-vector to geodetic latitude + * + * Given an ECEF-vector of a position. Find geodetic latitude, longitude and + * height (using WGS-84 ellipsoid). + * + * @see https://www.ffi.no/en/research/n-vector/#example_3 + */ +test("Example 3", () => { + // PROBLEM: + + // Position B is given as an “ECEF-vector” pb (i.e. a vector from E, the + // center of the Earth, to B, decomposed in E): + const pb: Vector = apply((n) => n * 6371e3, [0.71, -0.72, 0.1]); + + // Find the geodetic latitude, longitude and height, assuming WGS-84 + // ellipsoid. + + // SOLUTION: + + // Step 1 + // + // We have a function that converts ECEF-vectors to n-vectors: + const [b, bDepth] = fromECEF(pb); + + // Step 2 + // + // Find latitude, longitude and height: + const [lat, lon] = toGeodeticCoordinates(b); + const height = -bDepth; + + expect(degrees(lat)).toBeCloseTo(5.685075734513181, 14); + expect(degrees(lon)).toBeCloseTo(-45.40066325579215, 14); + expect(height).toBeCloseTo(95772.10761821801, 15); +}); ``` ### Example 4: Geodetic latitude to ECEF-vector @@ -211,22 +303,39 @@ const h_EB = -z_EB; > https://www.ffi.no/en/research/n-vector/#example_4 ```ts -import { lat_long2n_E, n_EB_E2p_EB_E, rad } from "nvector-geodesy"; - -// Position B is given with lat, long and height: -const lat_EB_deg = 1; -const long_EB_deg = 2; -const h_EB = 3; - -// Find the vector p_EB_E ("ECEF-vector") - -// SOLUTION: - -// Step1: Convert to n-vector: -const n_EB_E = lat_long2n_E(rad(lat_EB_deg), rad(long_EB_deg)); - -// Step2: Find the ECEF-vector p_EB_E: -const p_EB_E = n_EB_E2p_EB_E(n_EB_E, -h_EB); +import { fromGeodeticCoordinates, radians, toECEF } from "nvector-geodesy"; +import { expect, test } from "vitest"; + +/** + * Example 4: Geodetic latitude to ECEF-vector + * + * Given geodetic latitude, longitude and height. Find the ECEF-vector (using + * WGS-84 ellipsoid). + * + * @see https://www.ffi.no/en/research/n-vector/#example_4 + */ +test("Example 4", () => { + // PROBLEM: + + // Geodetic latitude, longitude and height are given for position B: + const bLat = 1; + const bLon = 2; + const bHeight = 3; + + // Find the ECEF-vector for this position. + + // SOLUTION: + + // Step 1: First, the given latitude and longitude are converted to n-vector: + const b = fromGeodeticCoordinates(radians(bLat), radians(bLon)); + + // Step 2: Convert to an ECEF-vector: + const pb = toECEF(b, -bHeight); + + expect(pb[0]).toBeCloseTo(6373290.277218279, 8); + expect(pb[1]).toBeCloseTo(222560.2006747365, 8); + expect(pb[2]).toBeCloseTo(110568.8271817859, 8); +}); ``` ### Example 5: Surface distance @@ -243,35 +352,47 @@ import { apply, cross, dot, - lat_long2n_E, + fromGeodeticCoordinates, norm, - rad, - unit, - type Vector3, + radians, } from "nvector-geodesy"; - -// Position A and B are given as n_EA_E and n_EB_E: -// Enter elements directly: -// const n_EA_E = unit([1, 0, -2]); -// const n_EB_E = unit([-1, -2, 0]); - -// or input as lat/long in deg: -const n_EA_E = lat_long2n_E(rad(88), rad(0)); -const n_EB_E = lat_long2n_E(rad(89), rad(-170)); - -// m, mean Earth radius -const r_Earth = 6371e3; - -// SOLUTION: - -// The great circle distance is given by equation (16) in Gade (2010): -// Well conditioned for all angles: -const s_AB = - Math.atan2(norm(cross(n_EA_E, n_EB_E)), dot(n_EA_E, n_EB_E)) * r_Earth; - -// The Euclidean distance is given by: -const d_AB = - norm(apply((n_EB_E, n_EA_E) => n_EB_E - n_EA_E, n_EB_E, n_EA_E)) * r_Earth; +import { expect, test } from "vitest"; + +/** + * Example 5: Surface distance + * + * Given position A and B. Find the surface distance (i.e. great circle + * distance) and the Euclidean distance. + * + * @see https://www.ffi.no/en/research/n-vector/#example_5 + */ +test("Example 5", () => { + // PROBLEM: + + // Given two positions A and B as n-vectors: + const a = fromGeodeticCoordinates(radians(88), radians(0)); + const b = fromGeodeticCoordinates(radians(89), radians(-170)); + + // Find the surface distance (i.e. great circle distance). The heights of A + // and B are not relevant (i.e. if they do not have zero height, we seek the + // distance between the points that are at the surface of the Earth, directly + // above/below A and B). The Euclidean distance (chord length) should also be + // found. + + // Use Earth radius r: + const r = 6371e3; + + // SOLUTION: + + // Find the great circle distance: + const gcd = Math.atan2(norm(cross(a, b)), dot(a, b)) * r; + + // Find the Euclidean distance: + const ed = norm(apply((b, a) => b - a, b, a)) * r; + + expect(gcd).toBeCloseTo(332456.4441053448, 9); + expect(ed).toBeCloseTo(332418.7248568097, 9); +}); ``` ### Example 6: Interpolated position @@ -286,45 +407,48 @@ const d_AB = ```ts import { apply, - deg, - lat_long2n_E, - n_E2lat_long, - rad, - unit, - type Vector3, + degrees, + fromGeodeticCoordinates, + normalize, + radians, + toGeodeticCoordinates, } from "nvector-geodesy"; - -// Position B is given at time t0 as n_EB_E_t0 and at time t1 as n_EB_E_t1: -// Enter elements directly: -// const n_EB_E_t0 = unit([1, 0, -2]); -// const n_EB_E_t1 = unit([-1, -2, 0]); - -// or input as lat/long in deg: -const n_EB_E_t0 = lat_long2n_E(rad(89.9), rad(-150)); -const n_EB_E_t1 = lat_long2n_E(rad(89.9), rad(150)); - -// The times are given as: -const t0 = 10; -const t1 = 20; -const ti = 16; // time of interpolation - -// Find the interpolated position at time ti, n_EB_E_ti - -// SOLUTION: - -// Using standard interpolation: -const n_EB_E_ti = unit( - apply( - (n_EB_E_t0, n_EB_E_t1) => - n_EB_E_t0 + ((ti - t0) * (n_EB_E_t1 - n_EB_E_t0)) / (t1 - t0), - n_EB_E_t0, - n_EB_E_t1, - ), -); - -// When displaying the resulting position for humans, it is more convenient -// to see lat, long: -const [lat_EB_ti, long_EB_ti] = n_E2lat_long(n_EB_E_ti); +import { expect, test } from "vitest"; + +/** + * Example 6: Interpolated position + * + * Given the position of B at time t(0) and t(1). Find an interpolated position + * at time t(i). + * + * @see https://www.ffi.no/en/research/n-vector/#example_6 + */ +test("Example 6", () => { + // PROBLEM: + + // Given the position of B at time t0 and t1, pt0 and pt1: + const t0 = 10; + const t1 = 20; + const ti = 16; + const pt0 = fromGeodeticCoordinates(radians(89.9), radians(-150)); + const pt1 = fromGeodeticCoordinates(radians(89.9), radians(150)); + + // Find an interpolated position at time ti, pti. All positions are given as + // n-vectors. + + // SOLUTION: + + // Standard interpolation can be used directly with n-vector: + const pti = normalize( + apply((pt0, pt1) => pt0 + ((ti - t0) * (pt1 - pt0)) / (t1 - t0), pt0, pt1), + ); + + // Use human-friendly outputs: + const [lat, lon] = toGeodeticCoordinates(pti); + + expect(degrees(lat)).toBeCloseTo(89.91282199988446, 12); + expect(degrees(lon)).toBeCloseTo(173.4132244463705, 12); +}); ``` ### Example 7: Mean position/center @@ -337,30 +461,41 @@ const [lat_EB_ti, long_EB_ti] = n_E2lat_long(n_EB_E_ti); > https://www.ffi.no/en/research/n-vector/#example_7 ```ts -import { apply, lat_long2n_E, rad, unit, type Vector3 } from "nvector-geodesy"; - -// Three positions A, B and C are given: -// Enter elements directly: -// const n_EA_E = unit([1, 0, -2]); -// const n_EB_E = unit([-1, -2, 0]); -// const n_EC_E = unit([0, -2, 3]); - -// or input as lat/long in degrees: -const n_EA_E = lat_long2n_E(rad(90), rad(0)); -const n_EB_E = lat_long2n_E(rad(60), rad(10)); -const n_EC_E = lat_long2n_E(rad(50), rad(-20)); - -// SOLUTION: - -// Find the horizontal mean position, M: -const n_EM_E = unit( - apply( - (n_EA_E, n_EB_E, n_EC_E) => n_EA_E + n_EB_E + n_EC_E, - n_EA_E, - n_EB_E, - n_EC_E, - ), -); +import { + apply, + fromGeodeticCoordinates, + normalize, + radians, +} from "nvector-geodesy"; +import { expect, test } from "vitest"; + +/** + * Example 7: Mean position/center + * + * Given three positions A, B, and C. Find the mean position (center/midpoint). + * + * @see https://www.ffi.no/en/research/n-vector/#example_7 + */ +test("Example 7", () => { + // PROBLEM: + + // Three positions A, B, and C are given as n-vectors: + const a = fromGeodeticCoordinates(radians(90), radians(0)); + const b = fromGeodeticCoordinates(radians(60), radians(10)); + const c = fromGeodeticCoordinates(radians(50), radians(-20)); + + // Find the mean position, M. Note that the calculation is independent of the + // heights/depths of the positions. + + // SOLUTION: + + // The mean position is simply given by the mean n-vector: + const m = normalize(apply((a, b, c) => a + b + c, a, b, c)); + + expect(m[0]).toBeCloseTo(0.3841171702926, 16); + expect(m[1]).toBeCloseTo(-0.04660240548568945, 16); + expect(m[2]).toBeCloseTo(0.9221074857571395, 16); +}); ``` ### Example 8: A and azimuth/distance to B @@ -374,61 +509,89 @@ const n_EM_E = unit( ```ts import { - R_Ee_NP_Z, + Z_AXIS_NORTH, apply, cross, - deg, - lat_long2n_E, - n_E2lat_long, - rad, - rotate, + degrees, + fromGeodeticCoordinates, + normalize, + radians, + toGeodeticCoordinates, + transform, transpose, - unit, - type Vector3, } from "nvector-geodesy"; - -// Position A is given as n_EA_E: -// Enter elements directly: -// const n_EA_E = unit([1, 0, -2]); - -// or input as lat/long in deg: -const n_EA_E = lat_long2n_E(rad(80), rad(-90)); - -// The initial azimuth and great circle distance (s_AB), and Earth radius -// (r_Earth) are also given: -const azimuth = rad(200); -const s_AB = 1000; // m -const r_Earth = 6371e3; // m, mean Earth radius - -// Find the destination point B, as n_EB_E ("The direct/first geodetic -// problem" for a sphere) - -// SOLUTION: - -// Step1: Find unit vectors for north and east (see equations (9) and (10) -// in Gade (2010): -const k_east_E = unit(cross(rotate(transpose(R_Ee_NP_Z), [1, 0, 0]), n_EA_E)); -const k_north_E = cross(n_EA_E, k_east_E); - -// Step2: Find the initial direction vector d_E: -const d_E = apply( - (k_north_E, k_east_E) => - k_north_E * Math.cos(azimuth) + k_east_E * Math.sin(azimuth), - k_north_E, - k_east_E, -); - -// Step3: Find n_EB_E: -const n_EB_E = apply( - (n_EA_E, d_E) => - n_EA_E * Math.cos(s_AB / r_Earth) + d_E * Math.sin(s_AB / r_Earth), - n_EA_E, - d_E, -); - -// When displaying the resulting position for humans, it is more convenient -// to see lat, long: -const [lat_EB, long_EB] = n_E2lat_long(n_EB_E); +import { expect, test } from "vitest"; + +/** + * Example 8: A and azimuth/distance to B + * + * Given position A and an azimuth/bearing and a (great circle) distance. Find + * the destination point B. + * + * @see https://www.ffi.no/en/research/n-vector/#example_8 + */ +test("Example 8", () => { + // PROBLEM: + + // Position A is given as n-vector: + const a = fromGeodeticCoordinates(radians(80), radians(-90)); + + // We also have an initial direction of travel given as an azimuth (bearing) + // relative to north (clockwise), and finally the distance to travel along a + // great circle is given: + const azimuth = radians(200); + const gcd = 1000; + + // Use Earth radius r: + const r = 6371e3; + + // Find the destination point B. + // + // In geodesy, this is known as "The first geodetic problem" or "The direct + // geodetic problem" for a sphere, and we see that this is similar to Example + // 2, but now the delta is given as an azimuth and a great circle distance. + // "The second/inverse geodetic problem" for a sphere is already solved in + // Examples 1 and 5. + + // SOLUTION: + + // The azimuth (relative to north) is a singular quantity (undefined at the + // Poles), but from this angle we can find a (non-singular) quantity that is + // more convenient when working with vector algebra: a vector d that points in + // the initial direction. We find this from azimuth by first finding the north + // and east vectors at the start point, with unit lengths. + // + // Here we have assumed that our coordinate frame E has its z-axis along the + // rotational axis of the Earth, pointing towards the North Pole. Hence, this + // axis is given by [1, 0, 0]: + const e = normalize(cross(transform(transpose(Z_AXIS_NORTH), [1, 0, 0]), a)); + const n = cross(a, e); + + // The two vectors n and e are horizontal, orthogonal, and span the tangent + // plane at the initial position. A unit vector d in the direction of the + // azimuth is now given by: + const d = apply( + (n, e) => n * Math.cos(azimuth) + e * Math.sin(azimuth), + n, + e, + ); + + // With the initial direction given as d instead of azimuth, it is now quite + // simple to find b. We know that d and a are orthogonal, and they will span + // the plane where b will lie. Thus, we can use sin and cos in the same manner + // as above, with the angle traveled given by gcd / r: + const b = apply( + (a, d) => a * Math.cos(gcd / r) + d * Math.sin(gcd / r), + a, + d, + ); + + // Use human-friendly outputs: + const [lat, lon] = toGeodeticCoordinates(b); + + expect(degrees(lat)).toBeCloseTo(79.99154867339445, 13); + expect(degrees(lon)).toBeCloseTo(-90.01769837291397, 13); +}); ``` ### Example 9: Intersection of two paths @@ -445,46 +608,63 @@ const [lat_EB, long_EB] = n_E2lat_long(n_EB_E); import { apply, cross, - deg, + degrees, dot, - lat_long2n_E, - n_E2lat_long, - rad, - unit, - type Vector3, + fromGeodeticCoordinates, + normalize, + radians, + toGeodeticCoordinates, } from "nvector-geodesy"; - -// Two paths A and B are given by two pairs of positions: -// Enter elements directly: -// const n_EA1_E = unit([0, 0, 1]); -// const n_EA2_E = unit([-1, 0, 1]); -// const n_EB1_E = unit([-2, -2, 4]); -// const n_EB2_E = unit([-2, 2, 2]); - -// or input as lat/long in deg: -const n_EA1_E = lat_long2n_E(rad(50), rad(180)); -const n_EA2_E = lat_long2n_E(rad(90), rad(180)); -const n_EB1_E = lat_long2n_E(rad(60), rad(160)); -const n_EB2_E = lat_long2n_E(rad(80), rad(-140)); - -// SOLUTION: - -// Find the intersection between the two paths, n_EC_E: -const n_EC_E_tmp = unit( - cross(cross(n_EA1_E, n_EA2_E), cross(n_EB1_E, n_EB2_E)), -); - -// n_EC_E_tmp is one of two solutions, the other is -n_EC_E_tmp. Select the -// one that is closest to n_EA1_E, by selecting sign from the dot product -// between n_EC_E_tmp and n_EA1_E: -const n_EC_E = apply( - (n) => Math.sign(dot(n_EC_E_tmp, n_EA1_E)) * n, - n_EC_E_tmp, -); - -// When displaying the resulting position for humans, it is more convenient -// to see lat, long: -const [lat_EC, long_EC] = n_E2lat_long(n_EC_E); +import { expect, test } from "vitest"; + +/** + * Example 9: Intersection of two paths + * + * Given path A going through A(1) and A(2), and path B going through B(1) and + * B(2). Find the intersection of the two paths. + * + * @see https://www.ffi.no/en/research/n-vector/#example_9 + */ +test("Example 9", () => { + // PROBLEM: + + // Define a path from two given positions (at the surface of a spherical + // Earth), as the great circle that goes through the two points (assuming that + // the two positions are not antipodal). + + // Path A is given by a1 and a2: + const a1 = fromGeodeticCoordinates(radians(50), radians(180)); + const a2 = fromGeodeticCoordinates(radians(90), radians(180)); + + // While path B is given by b1 and b2: + const b1 = fromGeodeticCoordinates(radians(60), radians(160)); + const b2 = fromGeodeticCoordinates(radians(80), radians(-140)); + + // Find the position C where the two paths intersect. + + // SOLUTION: + + // A convenient way to represent a great circle is by its normal vector (i.e. + // the normal vector to the plane containing the great circle). This normal + // vector is simply found by taking the cross product of the two n-vectors + // defining the great circle (path). Having the normal vectors to both paths, + // the intersection is now simply found by taking the cross product of the two + // normal vectors: + const cTmp = normalize(cross(cross(a1, a2), cross(b1, b2))); + + // Note that there will be two places where the great circles intersect, and + // thus two solutions are found. Selecting the solution that is closest to + // e.g. a1 can be achieved by selecting the solution that has a positive dot + // product with a1 (or the mean position from Example 7 could be used instead + // of a1): + const c = apply((n) => Math.sign(dot(cTmp, a1)) * n, cTmp); + + // Use human-friendly outputs: + const [lat, lon] = toGeodeticCoordinates(c); + + expect(degrees(lat)).toBeCloseTo(74.16344802135536, 16); + expect(degrees(lon)).toBeCloseTo(180, 16); +}); ``` ### Example 10: Cross track distance (cross track error) @@ -501,37 +681,58 @@ const [lat_EC, long_EC] = n_E2lat_long(n_EC_E); import { cross, dot, - lat_long2n_E, - rad, - unit, - type Vector3, + fromGeodeticCoordinates, + normalize, + radians, } from "nvector-geodesy"; - -// Position A1 and A2 and B are given as n_EA1_E, n_EA2_E, and n_EB_E: -// Enter elements directly: -// const n_EA1_E = unit([1, 0, -2]); -// const n_EA2_E = unit([-1, -2, 0]); -// const n_EB_E = unit([0, -2, 3]); - -// or input as lat/long in deg: -const n_EA1_E = lat_long2n_E(rad(0), rad(0)); -const n_EA2_E = lat_long2n_E(rad(10), rad(0)); -const n_EB_E = lat_long2n_E(rad(1), rad(0.1)); - -const r_Earth = 6371e3; // m, mean Earth radius - -// Find the cross track distance from path A to position B. - -// SOLUTION: - -// Find the unit normal to the great circle between n_EA1_E and n_EA2_E: -const c_E = unit(cross(n_EA1_E, n_EA2_E)); - -// Find the great circle cross track distance: (acos(x) - pi/2 = -asin(x)) -const s_xt = -Math.asin(dot(c_E, n_EB_E)) * r_Earth; - -// Find the Euclidean cross track distance: -const d_xt = -dot(c_E, n_EB_E) * r_Earth; +import { expect, test } from "vitest"; + +/** + * Example 10: Cross track distance (cross track error) + * + * Given path A going through A(1) and A(2), and a point B. Find the cross track + * distance/cross track error between B and the path. + * + * @see https://www.ffi.no/en/research/n-vector/#example_10 + */ +test("Example 10", () => { + // PROBLEM: + + // Path A is given by the two n-vectors a1 and a2 (as in the previous + // example): + const a1 = fromGeodeticCoordinates(radians(0), radians(0)); + const a2 = fromGeodeticCoordinates(radians(10), radians(0)); + + // And a position B is given by b: + const b = fromGeodeticCoordinates(radians(1), radians(0.1)); + + // Find the cross track distance between the path A (i.e. the great circle + // through a1 and a2) and the position B (i.e. the shortest distance at the + // surface, between the great circle and B). Also, find the Euclidean distance + // between B and the plane defined by the great circle. + + // Use Earth radius r: + const r = 6371e3; + + // SOLUTION: + + // First, find the normal to the great circle, with direction given by the + // right hand rule and the direction of travel: + const c = normalize(cross(a1, a2)); + + // Find the great circle cross track distance: + const gcd = -Math.asin(dot(c, b)) * r; + + // Finding the Euclidean distance is even simpler, since it is the projection + // of b onto c, thus simply the dot product: + const ed = -dot(c, b) * r; + + // For both gcd and ed, positive answers means that B is to the right of the + // track. + + expect(gcd).toBeCloseTo(11117.79911014538, 9); + expect(ed).toBeCloseTo(11117.79346740667, 9); +}); ``` ## Methodology diff --git a/src/R2xyz.ts b/src/R2xyz.ts deleted file mode 100644 index 882e158..0000000 --- a/src/R2xyz.ts +++ /dev/null @@ -1,63 +0,0 @@ -import type { Matrix3x3 } from "./matrix.js"; -import type { Vector3 } from "./vector.js"; - -/** - * 3 angles about new axes in the xyz order are found from a rotation matrix. - * - * 3 angles x,y,z about new axes (intrinsic) in the order x-y-z are found from - * the rotation matrix R_AB. The angles (called Euler angles or Tait–Bryan - * angles) are defined by the following procedure of successive rotations: - * - * Given two arbitrary coordinate frames A and B. Consider a temporary frame T - * that initially coincides with A. In order to make T align with B, we first - * rotate T an angle x about its x-axis (common axis for both A and T). - * Secondly, T is rotated an angle y about the NEW y-axis of T. Finally, T is - * rotated an angle z about its NEWEST z-axis. The final orientation of T now - * coincides with the orientation of B. - * - * The signs of the angles are given by the directions of the axes and the right - * hand rule. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/R2xyz.m - * - * @param R_AB - 3x3 rotation matrix (direction cosine matrix) such that the - * relation between a vector v decomposed in A and B is given by: v_A = R_AB * - * v_B. - * - * @returns Angles of rotation about new axes. - */ -export function R2xyz(R_AB: Matrix3x3): Vector3 { - const [[r11, r12, r13], [r21, r22, r23], [, , r33]] = R_AB; - - // cos_y is based on as many elements as possible, to average out numerical - // errors. It is selected as the positive square root since y: [-pi/2 pi/2] - const cos_y = Math.sqrt((r11 ** 2 + r12 ** 2 + r23 ** 2 + r33 ** 2) / 2); - - const n_of_eps_to_define_singularity = 10; - let x, y, z; - - // Check if (close to) Euler angle singularity: - if (cos_y > n_of_eps_to_define_singularity * Number.EPSILON) { - // Outside singularity: - // atan2: [-pi pi] - z = Math.atan2(-r12, r11); - x = Math.atan2(-r23, r33); - - const sin_y = r13; - - y = Math.atan2(sin_y, cos_y); - } else { - // In singularity (or close to), i.e. y = +pi/2 or -pi/2: - // Selecting y = +-pi/2, with correct sign - y = (Math.sign(r13) * Math.PI) / 2; - - // Only the sum/difference of x and z is now given, choosing x = 0: - x = 0; - - // Lower left 2x2 elements of R_AB now only consists of sin_z and cos_z. - // Using the two whose signs are the same for both singularities: - z = Math.atan2(r21, r22); - } - - return [x, y, z]; -} diff --git a/src/R2zyx.ts b/src/R2zyx.ts deleted file mode 100644 index c91e445..0000000 --- a/src/R2zyx.ts +++ /dev/null @@ -1,66 +0,0 @@ -import type { Matrix3x3 } from "./matrix.js"; -import type { Vector3 } from "./vector.js"; - -/** - * 3 angles about new axes in the zyx order are found from a rotation matrix. - * - * 3 angles z,y,x about new axes (intrinsic) in the order z-y-x are found from - * the rotation matrix R_AB. The angles (called Euler angles or Tait–Bryan - * angles) are defined by the following procedure of successive rotations: - * - * Given two arbitrary coordinate frames A and B. Consider a temporary frame T - * that initially coincides with A. In order to make T align with B, we first - * rotate T an angle z about its z-axis (common axis for both A and T). - * Secondly, T is rotated an angle y about the NEW y-axis of T. Finally, T is - * rotated an angle x about its NEWEST x-axis. The final orientation of T now - * coincides with the orientation of B. - * - * The signs of the angles are given by the directions of the axes and the right - * hand rule. - * - * Note that if A is a north-east-down frame and B is a body frame, we have that - * z = yaw, y = pitch and x = roll. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/R2zyx.m - * - * @param R_AB - 3x3 rotation matrix (direction cosine matrix) such that the - * relation between a vector v decomposed in A and B is given by: v_A = R_AB * - * v_B. - * - * @returns Angles of rotation about new axes. - */ -export function R2zyx(R_AB: Matrix3x3): Vector3 { - const [[r11, r12], [r21, r22], [r31, r32, r33]] = R_AB; - - // cos_y is based on as many elements as possible, to average out numerical - // errors. It is selected as the positive square root since y: [-pi/2 pi/2] - const cos_y = Math.sqrt((r11 ** 2 + r21 ** 2 + r32 ** 2 + r33 ** 2) / 2); - - const n_of_eps_to_define_singularity = 10; - let z, y, x; - - // Check if (close to) Euler angle singularity: - if (cos_y > n_of_eps_to_define_singularity * Number.EPSILON) { - // Outside singularity: - // atan2: [-pi pi] - z = Math.atan2(r21, r11); - x = Math.atan2(r32, r33); - - const sin_y = -r31; - - y = Math.atan2(sin_y, cos_y); - } else { - // In singularity (or close to), i.e. y = +pi/2 or -pi/2: - // Selecting y = +-pi/2, with correct sign - y = (Math.sign(r31) * Math.PI) / 2; - - // Only the sum/difference of x and z is now given, choosing x = 0: - x = 0; - - // Upper right 2x2 elements of R_AB now only consists of sin_z and cos_z. - // Using the two whose signs are the same for both singularities: - z = Math.atan2(-r12, r22); - } - - return [z, y, x]; -} diff --git a/src/R_EL2n_E.ts b/src/R_EL2n_E.ts deleted file mode 100644 index 8d8761d..0000000 --- a/src/R_EL2n_E.ts +++ /dev/null @@ -1,16 +0,0 @@ -import type { Matrix3x3 } from "./matrix.js"; -import { rotate } from "./rotation.js"; -import type { Vector3 } from "./vector.js"; - -/** - * Finds n-vector from R_EL. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/R_EL2n_E.m - * - * @param R_EL - Rotation matrix (direction cosine matrix). - * - * @returns An n-vector decomposed in E. - */ -export function R_EL2n_E(R_EL: Matrix3x3): Vector3 { - return rotate(R_EL, [0, 0, -1]); -} diff --git a/src/R_EN2n_E.ts b/src/R_EN2n_E.ts deleted file mode 100644 index 1a8c3ea..0000000 --- a/src/R_EN2n_E.ts +++ /dev/null @@ -1,16 +0,0 @@ -import type { Matrix3x3 } from "./matrix.js"; -import { rotate } from "./rotation.js"; -import type { Vector3 } from "./vector.js"; - -/** - * Finds n-vector from R_EN. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/R_EN2n_E.m - * - * @param R_EN - Rotation matrix (direction cosine matrix). - * - * @returns An n-vector decomposed in E. - */ -export function R_EN2n_E(R_EN: Matrix3x3): Vector3 { - return rotate(R_EN, [0, 0, -1]); -} diff --git a/src/angle.ts b/src/angle.ts new file mode 100644 index 0000000..f1c2917 --- /dev/null +++ b/src/angle.ts @@ -0,0 +1,25 @@ +/** + * Converts angle in radians to degrees. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/deg.m + * + * @param radians - Angle in radians. + * + * @returns Angle in degrees. + */ +export function degrees(radians: number): number { + return (radians * 180) / Math.PI; +} + +/** + * Converts angle in degrees to radians. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/rad.m + * + * @param degrees - Angle in degrees. + * + * @returns Angle in radians. + */ +export function radians(degrees: number): number { + return (degrees * Math.PI) / 180; +} diff --git a/src/rotation.ts b/src/coord-frame.ts similarity index 65% rename from src/rotation.ts rename to src/coord-frame.ts index 9e35cbb..39dce9d 100644 --- a/src/rotation.ts +++ b/src/coord-frame.ts @@ -1,5 +1,4 @@ -import type { Matrix3x3 } from "./matrix.js"; -import type { Vector3 } from "./vector.js"; +import type { Matrix } from "./matrix.js"; /** * Axes of the coordinate frame E (Earth-Centred, Earth-Fixed, ECEF) when the @@ -10,7 +9,7 @@ import type { Vector3 } from "./vector.js"; * * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/R_Ee.m#L48 */ -export const R_Ee_NP_Z: Matrix3x3 = [ +export const Z_AXIS_NORTH: Matrix = [ [0, 0, 1], [0, 1, 0], [-1, 0, 0], @@ -29,27 +28,8 @@ export const R_Ee_NP_Z: Matrix3x3 = [ * * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/R_Ee.m#L55 */ -export const R_Ee_NP_X: Matrix3x3 = [ +export const X_AXIS_NORTH: Matrix = [ [1, 0, 0], [0, 1, 0], [0, 0, 1], ]; - -/** - * Rotates a vector by a rotation matrix. - * - * @param r - A rotation matrix. - * @param v - A vector to rotate. - * - * @returns The rotated vector. - */ -export function rotate(r: Matrix3x3, v: Vector3): Vector3 { - const [[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]] = r; - const [x, y, z] = v; - - return [ - r11 * x + r12 * y + r13 * z, - r21 * x + r22 * y + r23 * z, - r31 * x + r32 * y + r33 * z, - ]; -} diff --git a/src/coords.ts b/src/coords.ts new file mode 100644 index 0000000..54e7d79 --- /dev/null +++ b/src/coords.ts @@ -0,0 +1,63 @@ +import { Z_AXIS_NORTH } from "./coord-frame.js"; +import type { Matrix } from "./matrix.js"; +import { transpose } from "./matrix.js"; +import type { Vector } from "./vector.js"; +import { transform } from "./vector.js"; + +/** + * Converts geodetic coordinates to an n-vector. + * + * @see https://github.com/FFI-no/n-vector/blob/82d749a67cc9f332f48c51aa969cdc277b4199f2/nvector/lat_long2n_E.m + * + * @param latitude - Geodetic latitude in radians. + * @param longitude - Geodetic longitude in radians. + * @param frame - Coordinate frame in which the n-vector is decomposed. + * + * @returns An n-vector. + */ +export function fromGeodeticCoordinates( + latitude: number, + longitude: number, + frame: Matrix = Z_AXIS_NORTH, +): Vector { + // Equation (3) from Gade (2010): + const cosLat = Math.cos(latitude); + + // frame selects correct E-axes + return transform(transpose(frame), [ + Math.sin(latitude), + Math.sin(longitude) * cosLat, + -Math.cos(longitude) * cosLat, + ]); +} + +/** + * Converts an n-vector to geodetic coordinates. + * + * @see https://github.com/FFI-no/n-vector/blob/82d749a67cc9f332f48c51aa969cdc277b4199f2/nvector/n_E2lat_long.m + * + * @param vector - An n-vector. + * @param frame - Coordinate frame in which the n-vector is decomposed. + * + * @returns Geodetic latitude and longitude in radians. + */ +export function toGeodeticCoordinates( + vector: Vector, + frame: Matrix = Z_AXIS_NORTH, +): [latitude: number, longitude: number] { + // Equation (5) in Gade (2010): + const [x, y, z] = transform(frame, vector); + const longitude = Math.atan2(y, -z); + + // Equation (6) in Gade (2010) (Robust numerical solution) + // vector component in the equatorial plane + const ec = Math.hypot(y, z); + // atan() could also be used since latitude is within [-pi/2,pi/2] + const latitude = Math.atan2(x, ec); + + // latitude = asin(x) is a theoretical solution, but close to the Poles it is + // ill-conditioned which may lead to numerical inaccuracies (and it will give + // imaginary results for norm(vector)>1) + + return [latitude, longitude]; +} diff --git a/src/deg.ts b/src/deg.ts deleted file mode 100644 index bfd08e8..0000000 --- a/src/deg.ts +++ /dev/null @@ -1,12 +0,0 @@ -/** - * Converts angle in radians to degrees. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/deg.m - * - * @param rad_angle - Angle in radians. - * - * @returns Angle in degrees. - */ -export function deg(rad_angle: number): number { - return (rad_angle * 180) / Math.PI; -} diff --git a/src/delta.ts b/src/delta.ts new file mode 100644 index 0000000..5e23966 --- /dev/null +++ b/src/delta.ts @@ -0,0 +1,63 @@ +import { Z_AXIS_NORTH } from "./coord-frame.js"; +import { fromECEF, toECEF } from "./ecef.js"; +import { WGS_84, type Ellipsoid } from "./ellipsoid.js"; +import type { Matrix } from "./matrix.js"; +import type { Vector } from "./vector.js"; + +/** + * Delta finds a delta ECEF position vector from a reference n-vector position, + * and a target n-vector position. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/n_EA_E_and_n_EB_E2p_AB_E.m + * + * @param from - An n-vector of position A. + * @param to - An n-vector of position B. + * @param fromDepth - Depth of position A in meters, relative to the ellipsoid. + * @param toDepth - Depth of position B in meters, relative to the ellipsoid. + * @param ellipsoid - A reference ellipsoid. + * @param frame - Coordinate frame in which the vectors are decomposed. + * + * @returns Position vector in meters from A to B, decomposed in E. + */ +export function delta( + from: Vector, + to: Vector, + fromDepth: number = 0, + toDepth: number = 0, + ellipsoid: Ellipsoid = WGS_84, + frame: Matrix = Z_AXIS_NORTH, +): Vector { + // Function 1. in Section 5.4 in Gade (2010): + const [ax, ay, az] = toECEF(from, fromDepth, ellipsoid, frame); + const [bx, by, bz] = toECEF(to, toDepth, ellipsoid, frame); + + return [bx - ax, by - ay, bz - az]; +} + +/** + * From position A and delta, finds position B. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/n_EA_E_and_p_AB_E2n_EB_E.m + * + * @param from - An n-vector of position A. + * @param delta - ECEF position vector in meters from A to B. + * @param fromDepth - Depth of position A in meters, relative to the ellipsoid. + * @param ellipsoid - A reference ellipsoid. + * @param frame - Coordinate frame in which the vectors are decomposed. + * + * @returns An n-vector of position B, and depth of position B in meters, + * relative to the ellipsoid. + */ +export function destination( + from: Vector, + [dx, dy, dz]: Vector, + fromDepth: number = 0, + ellipsoid: Ellipsoid = WGS_84, + frame: Matrix = Z_AXIS_NORTH, +): [to: Vector, toDepth: number] { + // Function 2. in Section 5.4 in Gade (2010): + const [ax, ay, az] = toECEF(from, fromDepth, ellipsoid, frame); + const b: Vector = [ax + dx, ay + dy, az + dz]; + + return fromECEF(b, ellipsoid, frame); +} diff --git a/src/ecef.ts b/src/ecef.ts new file mode 100644 index 0000000..45a7f9e --- /dev/null +++ b/src/ecef.ts @@ -0,0 +1,109 @@ +import { Z_AXIS_NORTH } from "./coord-frame.js"; +import { WGS_84, type Ellipsoid } from "./ellipsoid.js"; +import type { Matrix } from "./matrix.js"; +import { transpose } from "./matrix.js"; +import type { Vector } from "./vector.js"; +import { normalize, transform } from "./vector.js"; + +/** + * Converts an ECEF position vector to an n-vector and depth. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/p_EB_E2n_EB_E.m + * + * @param vector - An ECEF position vector. + * @param ellipsoid - A reference ellipsoid. + * @param frame - Coordinate frame in which the vectors are decomposed. + * + * @returns Representation of position B, decomposed in E, and depth in meters + * of system B relative to the ellipsoid. + */ +export function fromECEF( + vector: Vector, + { a, f }: Ellipsoid = WGS_84, + frame: Matrix = Z_AXIS_NORTH, +): [vector: Vector, depth: number] { + // frame selects correct E-axes + const [x, y, z] = transform(frame, vector); + + // e2 = eccentricity^2 + const e2 = 2 * f - f ** 2; + + // The following code implements equation (23) from Gade (2010): + + const R2 = y ** 2 + z ** 2; + // R = component of vector in the equatorial plane + const R = Math.sqrt(R2); + + const x2 = x ** 2; + + const p = R2 / a ** 2; + const q = ((1 - e2) / a ** 2) * x2; + const r = (p + q - e2 ** 2) / 6; + + const s = (e2 ** 2 * p * q) / (4 * r ** 3); + const t = Math.cbrt(1 + s + Math.sqrt(s * (2 + s))); + const u = r * (1 + t + 1 / t); + const v = Math.sqrt(u ** 2 + e2 ** 2 * q); + + const w = (e2 * (u + v - q)) / (2 * v); + const k = Math.sqrt(u + v + w ** 2) - w; + const d = (k * R) / (k + e2); + + // Calculate height: + const hf = Math.sqrt(d ** 2 + x2); + const h = ((k + e2 - 1) / k) * hf; + + const temp = 1 / hf; + + return [ + // Ensure unit length: + normalize( + transform(transpose(frame), [ + temp * x, + ((temp * k) / (k + e2)) * y, + ((temp * k) / (k + e2)) * z, + ]), + ), + -h, + ]; +} + +/** + * Converts an n-vector and depth to an ECEF position vector. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/n_EB_E2p_EB_E.m + * + * @param vector - An n-vector. + * @param depth - Depth of the position in meters, relative to the ellipsoid. + * @param ellipsoid - A reference ellipsoid. + * @param frame - Coordinate frame in which the n-vector is decomposed. + * + * @returns An ECEF position vector. + */ +export function toECEF( + vector: Vector, + depth: number = 0, + { b, f }: Ellipsoid = WGS_84, + frame: Matrix = Z_AXIS_NORTH, +): Vector { + // frame selects correct E-axes + const [x, y, z] = transform(frame, vector); + + // The following code implements equation (22) in Gade (2010): + + const denom = Math.hypot(x, y / (1 - f), z / (1 - f)); + + // We first calculate the position at the origin of coordinate system L, which + // has the same n-vector as B (bEL = bEB), but lies at the surface of the + // Earth (depth = 0). + + const ex = (b / denom) * x; + const ey = (b / denom) * (y / (1 - f) ** 2); + const ez = (b / denom) * (z / (1 - f) ** 2); + + return transform(transpose(frame), [ + ex - x * depth, + ey - y * depth, + ez - z * depth, + ]); +} diff --git a/src/ellipsoid.ts b/src/ellipsoid.ts index 95a5a5a..4ca701e 100644 --- a/src/ellipsoid.ts +++ b/src/ellipsoid.ts @@ -5,6 +5,7 @@ */ export const GRS_80: Ellipsoid = { a: 6378137, + b: 6356752.314140356, f: 1 / 298.257222101, } as const; @@ -15,6 +16,7 @@ export const GRS_80: Ellipsoid = { */ export const WGS_72: Ellipsoid = { a: 6378135, + b: 6356750.520016094, f: 1 / 298.26, } as const; @@ -25,28 +27,39 @@ export const WGS_72: Ellipsoid = { */ export const WGS_84: Ellipsoid = { a: 6378137, + b: 6356752.314245179, f: 1 / 298.257223563, } as const; /** - * A sphere with the same semi-major axis as the WGS-84 ellipsoid. + * Create a spherical ellipsoid with the given radius. * - * @see https://github.com/chrisveness/geodesy/blob/761587cd748bd9f7c9825195eba4a9fc5891b859/latlon-ellipsoidal-datum.js#L39 + * @param radius - A radius in meters. + * + * @returns A spherical ellipsoid. */ -export const WGS_84_SPHERE: Ellipsoid = { - a: WGS_84.a, - f: 0, -} as const; +export function sphere(radius: number): Ellipsoid { + return { + a: radius, + b: radius, + f: 0, + }; +} /** * An ellipsoid. */ export type Ellipsoid = { /** - * The semi-major axis of the ellipsoid. + * The semi-major axis of the ellipsoid in meters. */ a: number; + /** + * The semi-minor axis of the ellipsoid in meters. + */ + b: number; + /** * The flattening of the ellipsoid. */ diff --git a/src/euler.ts b/src/euler.ts new file mode 100644 index 0000000..e834ac7 --- /dev/null +++ b/src/euler.ts @@ -0,0 +1,157 @@ +import type { Matrix } from "./matrix.js"; +import type { Vector } from "./vector.js"; + +// A small number used to avoid Euler angle singularities. +const eulerThreshold = Number.EPSILON * 10; + +/** + * Converts Euler angles in XYZ order to a rotation matrix. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/xyz2R.m + * + * @param x - Rotation around the x-axis in radians. + * @param y - Rotation around the y-axis in radians. + * @param z - Rotation around the z-axis in radians. + * + * @returns A rotation matrix. + */ +export function eulerXYZToRotationMatrix( + x: number, + y: number, + z: number, +): Matrix { + const cz = Math.cos(z); + const sz = Math.sin(z); + const cy = Math.cos(y); + const sy = Math.sin(y); + const cx = Math.cos(x); + const sx = Math.sin(x); + + return [ + [cy * cz, -cy * sz, sy], + [sy * sx * cz + cx * sz, -sy * sx * sz + cx * cz, -cy * sx], + [-sy * cx * cz + sx * sz, sy * cx * sz + sx * cz, cy * cx], + ]; +} + +/** + * Converts Euler angles in ZYX order to a rotation matrix. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/zyx2R.m + * + * @param z - Rotation around the z-axis in radians. + * @param y - Rotation around the y-axis in radians. + * @param x - Rotation around the x-axis in radians. + * + * @returns A rotation matrix. + */ +export function eulerZYXToRotationMatrix( + z: number, + y: number, + x: number, +): Matrix { + const cz = Math.cos(z); + const sz = Math.sin(z); + const cy = Math.cos(y); + const sy = Math.sin(y); + const cx = Math.cos(x); + const sx = Math.sin(x); + + return [ + [cz * cy, -sz * cx + cz * sy * sx, sz * sx + cz * sy * cx], + [sz * cy, cz * cx + sz * sy * sx, -cz * sx + sz * sy * cx], + [-sy, cy * sx, cy * cx], + ]; +} + +/** + * Converts a rotation matrix to Euler angles in XYZ order. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/R2xyz.m + * + * @param rotation - A rotation matrix. + * + * @returns The Euler angles in XYZ order. + */ +export function rotationMatrixToEulerXYZ([ + [r11, r12, r13], + [r21, r22, r23], + [, , r33], +]: Matrix): Vector { + // cy is based on as many elements as possible, to average out numerical + // errors. It is selected as the positive square root since y: [-pi/2 pi/2] + const cy = Math.sqrt((r11 ** 2 + r12 ** 2 + r23 ** 2 + r33 ** 2) / 2); + + let x, y, z; + + // Check if (close to) Euler angle singularity: + if (cy > eulerThreshold) { + // Outside singularity: + // atan2: [-pi pi] + z = Math.atan2(-r12, r11); + x = Math.atan2(-r23, r33); + + const sy = r13; + + y = Math.atan2(sy, cy); + } else { + // In singularity (or close to), i.e. y = +pi/2 or -pi/2: + // Selecting y = +-pi/2, with correct sign + y = (Math.sign(r13) * Math.PI) / 2; + + // Only the sum/difference of x and z is now given, choosing x = 0: + x = 0; + + // Lower left 2x2 elements of rotation now only consists of sin z and cos z. + // Using the two whose signs are the same for both singularities: + z = Math.atan2(r21, r22); + } + + return [x, y, z]; +} + +/** + * Converts a rotation matrix to Euler angles in ZYX order. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/R2zyx.m + * + * @param rotation - A rotation matrix. + * + * @returns The Euler angles in ZYX order. + */ +export function rotationMatrixToEulerZYX([ + [r11, r12], + [r21, r22], + [r31, r32, r33], +]: Matrix): Vector { + // cy is based on as many elements as possible, to average out numerical + // errors. It is selected as the positive square root since y: [-pi/2 pi/2] + const cy = Math.sqrt((r11 ** 2 + r21 ** 2 + r32 ** 2 + r33 ** 2) / 2); + + let z, y, x; + + // Check if (close to) Euler angle singularity: + if (cy > eulerThreshold) { + // Outside singularity: + // atan2: [-pi pi] + z = Math.atan2(r21, r11); + x = Math.atan2(r32, r33); + + const sy = -r31; + + y = Math.atan2(sy, cy); + } else { + // In singularity (or close to), i.e. y = +pi/2 or -pi/2: + // Selecting y = +-pi/2, with correct sign + y = (Math.sign(r31) * Math.PI) / 2; + + // Only the sum/difference of x and z is now given, choosing x = 0: + x = 0; + + // Upper right 2x2 elements of rotation now only consists of sin z and cos + // z. Using the two whose signs are the same for both singularities: + z = Math.atan2(-r12, r22); + } + + return [z, y, x]; +} diff --git a/src/index.ts b/src/index.ts index 27ed210..849ecf4 100644 --- a/src/index.ts +++ b/src/index.ts @@ -1,24 +1,22 @@ -export { R2xyz } from "./R2xyz.js"; -export { R2zyx } from "./R2zyx.js"; -export { R_EL2n_E } from "./R_EL2n_E.js"; -export { R_EN2n_E } from "./R_EN2n_E.js"; -export { deg } from "./deg.js"; -export { GRS_80, WGS_72, WGS_84, WGS_84_SPHERE } from "./ellipsoid.js"; +export { degrees, radians } from "./angle.js"; +export { X_AXIS_NORTH, Z_AXIS_NORTH } from "./coord-frame.js"; +export { fromGeodeticCoordinates, toGeodeticCoordinates } from "./coords.js"; +export { delta, destination } from "./delta.js"; +export { fromECEF, toECEF } from "./ecef.js"; +export { GRS_80, WGS_72, WGS_84, sphere } from "./ellipsoid.js"; export type { Ellipsoid } from "./ellipsoid.js"; -export { lat_long2n_E } from "./lat_long2n_E.js"; +export { + eulerXYZToRotationMatrix, + eulerZYXToRotationMatrix, + rotationMatrixToEulerXYZ, + rotationMatrixToEulerZYX, +} from "./euler.js"; export { multiply, transpose } from "./matrix.js"; -export type { Matrix3x3 } from "./matrix.js"; -export { n_E2R_EN } from "./n_E2R_EN.js"; -export { n_E2lat_long } from "./n_E2lat_long.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 { n_E_and_wa2R_EL } from "./n_E_and_wa2R_EL.js"; -export { p_EB_E2n_EB_E } from "./p_EB_E2n_EB_E.js"; -export { rad } from "./rad.js"; -export { R_Ee_NP_X, R_Ee_NP_Z, rotate } from "./rotation.js"; -export { unit } from "./unit.js"; -export { apply, cross, dot, norm } from "./vector.js"; -export type { Vector3 } from "./vector.js"; -export { xyz2R } from "./xyz2R.js"; -export { zyx2R } from "./zyx2R.js"; +export type { Matrix } from "./matrix.js"; +export { + fromRotationMatrix, + toRotationMatrix, + toRotationMatrixUsingWanderAzimuth, +} from "./rotation-matrix.js"; +export { apply, cross, dot, norm, normalize, transform } from "./vector.js"; +export type { Vector } from "./vector.js"; diff --git a/src/lat_long2n_E.ts b/src/lat_long2n_E.ts deleted file mode 100644 index 2417ed4..0000000 --- a/src/lat_long2n_E.ts +++ /dev/null @@ -1,31 +0,0 @@ -import type { Matrix3x3 } from "./matrix.js"; -import { transpose } from "./matrix.js"; -import { R_Ee_NP_Z, rotate } from "./rotation.js"; -import type { Vector3 } from "./vector.js"; - -/** - * Converts latitude and longitude to n-vector. - * - * @see https://github.com/FFI-no/n-vector/blob/82d749a67cc9f332f48c51aa969cdc277b4199f2/nvector/lat_long2n_E.m - * - * @param latitude - Geodetic latitude in radians. - * @param longitude - Geodetic longitude in radians. - * @param R_Ee - Defines the axes of the coordinate frame E. - * - * @returns An n-vector decomposed in E. - */ -export function lat_long2n_E( - latitude: number, - longitude: number, - R_Ee: Matrix3x3 = R_Ee_NP_Z, -): Vector3 { - // Equation (3) from Gade (2010): - const cosLat = Math.cos(latitude); - - // R_Ee selects correct E-axes - return rotate(transpose(R_Ee), [ - Math.sin(latitude), - Math.sin(longitude) * cosLat, - -Math.cos(longitude) * cosLat, - ]); -} diff --git a/src/matrix.ts b/src/matrix.ts index aac0c72..6bb4d2b 100644 --- a/src/matrix.ts +++ b/src/matrix.ts @@ -1,24 +1,24 @@ /** * A 3x3 matrix. */ -export type Matrix3x3 = [ +export type Matrix = [ [n11: number, n12: number, n13: number], [n21: number, n22: number, n23: number], [n31: number, n32: number, n33: number], ]; /** - * Multiplies two 3x3 matrices. + * Multiplies two matrices. * * @param a - The first matrix. * @param b - The second matrix. * * @returns The resulting matrix. */ -export function multiply(a: Matrix3x3, b: Matrix3x3): Matrix3x3 { - const [[a11, a12, a13], [a21, a22, a23], [a31, a32, a33]] = a; - const [[b11, b12, b13], [b21, b22, b23], [b31, b32, b33]] = b; - +export function multiply( + [[a11, a12, a13], [a21, a22, a23], [a31, a32, a33]]: Matrix, + [[b11, b12, b13], [b21, b22, b23], [b31, b32, b33]]: Matrix, +): Matrix { return [ [ a11 * b11 + a12 * b21 + a13 * b31, @@ -39,15 +39,17 @@ export function multiply(a: Matrix3x3, b: Matrix3x3): Matrix3x3 { } /** - * Transposes a 3x3 matrix. + * Transposes a matrix. * - * @param a - The matrix to transpose. + * @param m - A matrix. * * @returns The transposed matrix. */ -export function transpose(a: Matrix3x3): Matrix3x3 { - const [[a11, a12, a13], [a21, a22, a23], [a31, a32, a33]] = a; - +export function transpose([ + [a11, a12, a13], + [a21, a22, a23], + [a31, a32, a33], +]: Matrix): Matrix { return [ [a11, a21, a31], [a12, a22, a32], diff --git a/src/n_E2R_EN.ts b/src/n_E2R_EN.ts deleted file mode 100644 index 609b4e5..0000000 --- a/src/n_E2R_EN.ts +++ /dev/null @@ -1,60 +0,0 @@ -import type { Matrix3x3 } from "./matrix.js"; -import { multiply, transpose } from "./matrix.js"; -import { R_Ee_NP_Z, rotate } from "./rotation.js"; -import type { Vector3 } from "./vector.js"; - -/** - * Finds the rotation matrix R_EN from n-vector. - * - * @see https://github.com/FFI-no/n-vector/blob/82d749a67cc9f332f48c51aa969cdc277b4199f2/nvector/n_E2R_EN.m - * - * @param n_E - An n-vector decomposed in E. - * @param R_Ee - Defines the axes of the coordinate frame E. - * - * @returns The resulting rotation matrix (direction cosine matrix). - */ -export function n_E2R_EN(n_E: Vector3, R_Ee: Matrix3x3 = R_Ee_NP_Z): Matrix3x3 { - // R_Ee selects correct E-axes - const [n_E_x, n_E_y, n_E_z] = rotate(R_Ee, n_E); - - // N coordinate frame (North-East-Down) is defined in Table 2 in Gade (2010) - - // R_EN is constructed by the following three column vectors: The x, y and z - // basis vectors (axes) of N, each decomposed in E. - - // Find z-axis of N (Nz): - // 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) - // Equation (9) in Gade (2010): - // 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 // Pole position: selected y-axis direction - : Ny_E_direction_y / Ny_E_direction_norm; // outside Poles: - const Ny_E_z = on_poles - ? 0 // Pole position: selected y-axis direction - : Ny_E_direction_z / Ny_E_direction_norm; // outside Poles: - - // Find x-axis of N (North): - // Final axis found by right hand rule - 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; - - // Form R_EN from the unit vectors: - // R_Ee selects correct E-axes - return multiply(transpose(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/src/n_E2lat_long.ts b/src/n_E2lat_long.ts deleted file mode 100644 index cc4a4d5..0000000 --- a/src/n_E2lat_long.ts +++ /dev/null @@ -1,34 +0,0 @@ -import type { Matrix3x3 } from "./matrix.js"; -import { R_Ee_NP_Z, rotate } from "./rotation.js"; -import type { Vector3 } from "./vector.js"; - -/** - * Converts n-vector to latitude and longitude. - * - * @see https://github.com/FFI-no/n-vector/blob/82d749a67cc9f332f48c51aa969cdc277b4199f2/nvector/n_E2lat_long.m - * - * @param n_E - An n-vector decomposed in E. - * @param R_Ee - Defines the axes of the coordinate frame E. - * - * @returns Geodetic latitude and longitude in radians. - */ -export function n_E2lat_long( - n_E: Vector3, - R_Ee: Matrix3x3 = R_Ee_NP_Z, -): [latitude: number, longitude: number] { - // Equation (5) in Gade (2010): - const [x, y, z] = rotate(R_Ee, n_E); - const longitude = Math.atan2(y, -z); - - // Equation (6) in Gade (2010) (Robust numerical solution) - // vector component in the equatorial plane - const equatorial_component = Math.hypot(y, z); - // atan() could also be used since latitude is within [-pi/2,pi/2] - const latitude = Math.atan2(x, equatorial_component); - - // latitude = asin(n_E(1)) is a theoretical solution, but close to the Poles - // it is ill-conditioned which may lead to numerical inaccuracies (and it will - // give imaginary results for norm(n_E)>1) - - return [latitude, longitude]; -} diff --git a/src/n_EA_E_and_n_EB_E2p_AB_E.ts b/src/n_EA_E_and_n_EB_E2p_AB_E.ts deleted file mode 100644 index f8ba98b..0000000 --- a/src/n_EA_E_and_n_EB_E2p_AB_E.ts +++ /dev/null @@ -1,55 +0,0 @@ -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 { R_Ee_NP_Z } from "./rotation.js"; -import type { Vector3 } from "./vector.js"; - -/** - * From two positions A and B, finds the delta position. - * - * The calculation is exact, taking the ellipsity of the Earth into account. It - * is also nonsingular as both n-vector and p-vector are nonsingular (except for - * the center of the Earth). The default ellipsoid model used is WGS-84, but - * other ellipsoids (or spheres) might be specified. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/n_EA_E_and_n_EB_E2p_AB_E.m - * - * @param n_EA_E - An n-vector of position A, decomposed in E. - * @param n_EB_E - An n-vector of position B, decomposed in E. - * @param z_EA - Depth of system A in meters, relative to the ellipsoid (z_EA = - * -height). - * @param z_EB - Depth of system B in meters, relative to the ellipsoid (z_EB = - * -height). - * @param a - Semi-major axis of the Earth ellipsoid in meters. - * @param f - Flattening of the Earth ellipsoid. - * @param R_Ee - Defines the axes of the coordinate frame E. - * - * @returns Position vector in meters from A to B, decomposed in E. - */ -export function n_EA_E_and_n_EB_E2p_AB_E( - n_EA_E: Vector3, - n_EB_E: Vector3, - z_EA: number = 0, - z_EB: number = 0, - a: number = WGS_84.a, - f: number = WGS_84.f, - R_Ee: Matrix3x3 = R_Ee_NP_Z, -): Vector3 { - // Function 1. in Section 5.4 in Gade (2010): - 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_x, p_EB_E_y, p_EB_E_z] = n_EB_E2p_EB_E( - n_EB_E, - z_EB, - a, - f, - R_Ee, - ); - - return [p_EB_E_x - p_EA_E_x, p_EB_E_y - p_EA_E_y, p_EB_E_z - p_EA_E_z]; -} 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 deleted file mode 100644 index 6dae41d..0000000 --- a/src/n_EA_E_and_p_AB_E2n_EB_E.ts +++ /dev/null @@ -1,52 +0,0 @@ -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 { R_Ee_NP_Z } from "./rotation.js"; -import type { Vector3 } from "./vector.js"; - -/** - * From position A and delta, finds position B. - * - * The calculation is exact, taking the ellipsity of the Earth into account. It - * is also nonsingular as both n-vector and p-vector are nonsingular (except for - * the center of the Earth). The default ellipsoid model used is WGS-84, but - * other ellipsoids (or spheres) might be specified. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/n_EA_E_and_p_AB_E2n_EB_E.m - * - * @param n_EA_E - An n-vector of position A, decomposed in E. - * @param p_AB_E - Position vector in meters from A to B, decomposed in E. - * @param z_EA - Depth of system A in meters, relative to the ellipsoid (z_EA = - * -height). - * @param a - Semi-major axis of the Earth ellipsoid in meters. - * @param f - Flattening of the Earth ellipsoid. - * @param R_Ee - Defines the axes of the coordinate frame E. - * - * @returns An n-vector of position B, decomposed in E, and depth in meters of - * system B, relative to the ellipsoid (-height). - */ -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 = R_Ee_NP_Z, -): [n_EB_E: Vector3, z_EB: number] { - // Function 2. in Section 5.4 in Gade (2010): - 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/src/n_EB_E2p_EB_E.ts b/src/n_EB_E2p_EB_E.ts deleted file mode 100644 index 17a56ee..0000000 --- a/src/n_EB_E2p_EB_E.ts +++ /dev/null @@ -1,60 +0,0 @@ -import { WGS_84 } from "./ellipsoid.js"; -import type { Matrix3x3 } from "./matrix.js"; -import { transpose } from "./matrix.js"; -import { R_Ee_NP_Z, rotate } from "./rotation.js"; -import type { Vector3 } from "./vector.js"; - -/** - * Converts n-vector to Cartesian position vector in meters. - * - * The position of B (typically body) relative to E (typically Earth) is given - * into this function as n-vector, n_EB_E. The function converts to cartesian - * position vector ("ECEF-vector"), p_EB_E, in meters. - * - * The calculation is exact, taking the ellipsity of the Earth into account. It - * is also nonsingular as both n-vector and p-vector are nonsingular (except for - * the center of the Earth). The default ellipsoid model used is WGS-84, but - * other ellipsoids (or spheres) might be specified. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/n_EB_E2p_EB_E.m - * - * @param n_EB_E - An n-vector of position B, decomposed in E. - * @param z_EB - Depth of system B in meters, relative to the ellipsoid (z_EB = - * -height). - * @param a - Semi-major axis of the Earth ellipsoid in meters. - * @param f - Flattening of the Earth ellipsoid. - * @param R_Ee - Defines the axes of the coordinate frame E. - * - * @returns Cartesian position vector in meters from E to B, decomposed in E. - */ -export function n_EB_E2p_EB_E( - n_EB_E: Vector3, - z_EB: number = 0, - a: number = WGS_84.a, - f: number = WGS_84.f, - R_Ee: Matrix3x3 = R_Ee_NP_Z, -): Vector3 { - // R_Ee selects correct E-axes - const [x, y, z] = rotate(R_Ee, n_EB_E); - - // semi-minor axis: - const b = a * (1 - f); - - // The following code implements equation (22) in Gade (2010): - - const denominator = Math.hypot(x, y / (1 - f), z / (1 - f)); - - // We first calculate the position at the origin of coordinate system L, which - // has the same n-vector as B (n_EL_E = n_EB_E), but lies at the surface of - // the Earth (z_EL = 0). - - const p_EL_E_x = (b / denominator) * x; - const p_EL_E_y = (b / denominator) * (y / (1 - f) ** 2); - const p_EL_E_z = (b / denominator) * (z / (1 - f) ** 2); - - return rotate(transpose(R_Ee), [ - p_EL_E_x - x * z_EB, - p_EL_E_y - y * z_EB, - p_EL_E_z - z * z_EB, - ]); -} diff --git a/src/n_E_and_wa2R_EL.ts b/src/n_E_and_wa2R_EL.ts deleted file mode 100644 index 84cdb78..0000000 --- a/src/n_E_and_wa2R_EL.ts +++ /dev/null @@ -1,36 +0,0 @@ -import type { Matrix3x3 } from "./matrix.js"; -import { multiply, transpose } from "./matrix.js"; -import { n_E2lat_long } from "./n_E2lat_long.js"; -import { R_Ee_NP_Z } from "./rotation.js"; -import type { Vector3 } from "./vector.js"; -import { xyz2R } from "./xyz2R.js"; - -/** - * Finds R_EL from n-vector and wander azimuth angle. - * - * When wander_azimuth = 0, we have that N = L (See Table 2 in Gade (2010) for - * details) - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/n_E_and_wa2R_EL.m - * - * @param n_E - An n-vector decomposed in E. - * @param wander_azimuth - The angle in radians between L's x-axis and north, - * pos about L's z-axis - * @param R_Ee - Defines the axes of the coordinate frame E. - * - * @returns The resulting rotation matrix. - */ -export function n_E_and_wa2R_EL( - n_E: Vector3, - wander_azimuth: number, - R_Ee: Matrix3x3 = R_Ee_NP_Z, -): Matrix3x3 { - const [latitude, longitude] = n_E2lat_long(n_E, R_Ee); - - // Longitude, -latitude, and wander azimuth are the x-y-z Euler angles (about - // new axes) for R_EL. See also the second paragraph of Section 5.2 in Gade - // (2010): - - // R_Ee selects correct E-axes - return multiply(transpose(R_Ee), xyz2R(longitude, -latitude, wander_azimuth)); -} diff --git a/src/p_EB_E2n_EB_E.ts b/src/p_EB_E2n_EB_E.ts deleted file mode 100644 index 4d33d66..0000000 --- a/src/p_EB_E2n_EB_E.ts +++ /dev/null @@ -1,82 +0,0 @@ -import { WGS_84 } from "./ellipsoid.js"; -import type { Matrix3x3 } from "./matrix.js"; -import { transpose } from "./matrix.js"; -import { R_Ee_NP_Z, rotate } from "./rotation.js"; -import { unit } from "./unit.js"; -import type { Vector3 } from "./vector.js"; - -/** - * Converts Cartesian position vector in meters to n-vector. - * - * The position of B (typically body) relative to E (typically Earth) is given - * into this function as cartesian position vector p_EB_E, in meters - * ("ECEF-vector"). The function converts to n-vector, n_EB_E and its depth, - * z_EB. - * - * The calculation is exact, taking the ellipsity of the Earth into account. It - * is also nonsingular as both n-vector and p-vector are nonsingular (except for - * the center of the Earth). The default ellipsoid model used is WGS-84, but - * other ellipsoids (or spheres) might be specified. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/p_EB_E2n_EB_E.m - * - * @param p_EB_E - Cartesian position vector in meters from E to B, decomposed - * in E. - * @param a - Semi-major axis of the Earth ellipsoid in meters. - * @param f - Flattening of the Earth ellipsoid. - * @param R_Ee - Defines the axes of the coordinate frame E. - * - * @returns Representation of position B, decomposed in E, and depth in meters - * of system B relative to the ellipsoid. - */ -export function p_EB_E2n_EB_E( - p_EB_E: Vector3, - a: number = WGS_84.a, - f: number = WGS_84.f, - R_Ee: Matrix3x3 = R_Ee_NP_Z, -): [n_EB_E: Vector3, depth: number] { - // R_Ee selects correct E-axes - const [p_EB_E_x, p_EB_E_y, p_EB_E_z] = rotate(R_Ee, p_EB_E); - - // e_2 = eccentricity^2 - const e_2 = 2 * f - f ** 2; - - // The following code implements equation (23) from Gade (2010): - - const R_2 = p_EB_E_y ** 2 + p_EB_E_z ** 2; - // R = component of p_EB_E in the equatorial plane - const R = Math.sqrt(R_2); - - const p_EB_E_x_2 = p_EB_E_x ** 2; - - const p = R_2 / a ** 2; - const q = ((1 - e_2) / a ** 2) * p_EB_E_x_2; - const r = (p + q - e_2 ** 2) / 6; - - const s = (e_2 ** 2 * p * q) / (4 * r ** 3); - const t = Math.cbrt(1 + s + Math.sqrt(s * (2 + s))); - const u = r * (1 + t + 1 / t); - const v = Math.sqrt(u ** 2 + e_2 ** 2 * q); - - const w = (e_2 * (u + v - q)) / (2 * v); - const k = Math.sqrt(u + v + w ** 2) - w; - const d = (k * R) / (k + e_2); - - // Calculate height: - const height_factor = Math.sqrt(d ** 2 + p_EB_E_x_2); - const height = ((k + e_2 - 1) / k) * height_factor; - - const temp = 1 / height_factor; - - const n_EB_E_x = temp * p_EB_E_x; - const n_EB_E_y = ((temp * k) / (k + e_2)) * p_EB_E_y; - const n_EB_E_z = ((temp * k) / (k + e_2)) * p_EB_E_z; - - const n_EB_E: Vector3 = [n_EB_E_x, n_EB_E_y, n_EB_E_z]; - - return [ - // Ensure unit length: - unit(rotate(transpose(R_Ee), n_EB_E)), - -height, - ]; -} diff --git a/src/rad.ts b/src/rad.ts deleted file mode 100644 index 4f781fc..0000000 --- a/src/rad.ts +++ /dev/null @@ -1,12 +0,0 @@ -/** - * Converts angle in degrees to radians. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/rad.m - * - * @param deg_angle - Angle in degrees. - * - * @returns Angle in radians. - */ -export function rad(deg_angle: number): number { - return (deg_angle * Math.PI) / 180; -} diff --git a/src/rotation-matrix.ts b/src/rotation-matrix.ts new file mode 100644 index 0000000..2e45e6f --- /dev/null +++ b/src/rotation-matrix.ts @@ -0,0 +1,108 @@ +import { Z_AXIS_NORTH } from "./coord-frame.js"; +import { toGeodeticCoordinates } from "./coords.js"; +import { eulerXYZToRotationMatrix } from "./euler.js"; +import { multiply, transpose, type Matrix } from "./matrix.js"; +import type { Vector } from "./vector.js"; +import { transform } from "./vector.js"; + +/** + * Converts a rotation matrix to an n-vector. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/R_EL2n_E.m + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/R_EN2n_E.m + * + * @param rotation - A rotation matrix. + * + * @returns An n-vector. + */ +export function fromRotationMatrix(rotation: Matrix): Vector { + return transform(rotation, [0, 0, -1]); +} + +/** + * Converts n-vector to a rotation matrix. + * + * @see https://github.com/FFI-no/n-vector/blob/82d749a67cc9f332f48c51aa969cdc277b4199f2/nvector/n_E2R_EN.m + * + * @param vector - An n-vector. + * @param frame - Coordinate frame in which the n-vector is decomposed. + * + * @returns A rotation matrix. + */ +export function toRotationMatrix( + vector: Vector, + frame: Matrix = Z_AXIS_NORTH, +): Matrix { + // frame selects correct E-axes + const [x, y, z] = transform(frame, vector); + + // N coordinate frame (North-East-Down) is defined in Table 2 in Gade (2010) + + // rEN is constructed by the following three column vectors: The x, y and z + // basis vectors (axes) of N, each decomposed in E. + + // Find z-axis of N (Nz): + // z-axis of N (down) points opposite to n-vector + const zx = -x; + const zy = -y; + const zz = -z; + + // Find y-axis of N (East)(remember that N is singular at Poles) + // Equation (9) in Gade (2010): + // Ny points perpendicular to the plane formed by n-vector and Earth's spin + // axis + const yyDir = -z; + const yzDir = y; + const yDirNorm = Math.hypot(yyDir, yzDir); + const onPoles = Math.hypot(yyDir, yzDir) === 0; + // yx is always 0, so it's factored out in the following equations + const yy = onPoles + ? 1 // Pole position: selected y-axis direction + : yyDir / yDirNorm; // outside Poles: + const yz = onPoles + ? 0 // Pole position: selected y-axis direction + : yzDir / yDirNorm; // outside Poles: + + // Find x-axis of N (North): + // Final axis found by right hand rule + const xx = yy * zz - yz * zy; + const xy = yz * zx; + const xz = -yy * zx; + + // Form rotation from the unit vectors: + // frame selects correct E-axes + return multiply(transpose(frame), [ + [xx, 0, zx], + [xy, yy, zy], + [xz, yz, zz], + ]); +} + +/** + * Converts an n-vector and a wander azimuth angle to a rotation matrix. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/n_E_and_wa2R_EL.m + * + * @param vector - An n-vector. + * @param wanderAzimuth - A wander azimuth angle in radians. + * @param frame - Coordinate frame in which the n-vector is decomposed. + * + * @returns A rotation matrix. + */ +export function toRotationMatrixUsingWanderAzimuth( + vector: Vector, + wanderAzimuth: number, + frame: Matrix = Z_AXIS_NORTH, +): Matrix { + const [latitude, longitude] = toGeodeticCoordinates(vector, frame); + + // Longitude, -latitude, and wander azimuth are the x-y-z Euler angles (about + // new axes) for rotation. See also the second paragraph of Section 5.2 in + // Gade (2010): + + // frame selects correct E-axes + return multiply( + transpose(frame), + eulerXYZToRotationMatrix(longitude, -latitude, wanderAzimuth), + ); +} diff --git a/src/unit.ts b/src/unit.ts deleted file mode 100644 index 2687f3d..0000000 --- a/src/unit.ts +++ /dev/null @@ -1,21 +0,0 @@ -import type { Vector3 } from "./vector.js"; - -/** - * Makes input vector unit length, i.e. norm == 1. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/unit.m - * - * @param v - A vector. - * - * @returns A unit vector. - */ -export function unit(v: Vector3): Vector3 { - const [x, y, z] = v; - const current_norm = Math.hypot(x, y, z); - - // If the vector has norm == 0, i.e. all elements in the vector are zero, the - // unit vector [1 0 0]' is returned. - if (current_norm === 0) return [1, 0, 0]; - - return [x / current_norm, y / current_norm, z / current_norm]; -} diff --git a/src/vector.ts b/src/vector.ts index 4221ea1..03ed4fb 100644 --- a/src/vector.ts +++ b/src/vector.ts @@ -1,7 +1,9 @@ +import type { Matrix } from "./matrix.js"; + /** * A 3D vector. */ -export type Vector3 = [x: number, y: number, z: number]; +export type Vector = [x: number, y: number, z: number]; /** * Creates a new vector by applying a function component-wise to the components @@ -29,9 +31,9 @@ export type Vector3 = [x: number, y: number, z: number]; */ export function apply( fn: (...n: N) => number, - ...v: { [Property in keyof N]: Vector3 } -): Vector3 { - const result: Vector3 = [0, 0, 0]; + ...v: { [Property in keyof N]: Vector } +): Vector { + const result: Vector = [0, 0, 0]; for (let i = 0; i < 3; ++i) { const n: number[] = []; @@ -51,10 +53,7 @@ export function apply( * * @returns The resulting vector. */ -export function cross(a: Vector3, b: Vector3): Vector3 { - const [a1, a2, a3] = a; - const [b1, b2, b3] = b; - +export function cross([a1, a2, a3]: Vector, [b1, b2, b3]: Vector): Vector { return [a2 * b3 - a3 * b2, a3 * b1 - a1 * b3, a1 * b2 - a2 * b1]; } @@ -66,10 +65,7 @@ export function cross(a: Vector3, b: Vector3): Vector3 { * * @returns The resulting scalar. */ -export function dot(a: Vector3, b: Vector3): number { - const [a1, a2, a3] = a; - const [b1, b2, b3] = b; - +export function dot([a1, a2, a3]: Vector, [b1, b2, b3]: Vector): number { return a1 * b1 + a2 * b2 + a3 * b3; } @@ -80,6 +76,42 @@ export function dot(a: Vector3, b: Vector3): number { * * @returns The Euclidean norm. */ -export function norm(v: Vector3): number { +export function norm(v: Vector): number { return Math.hypot(...v); } + +/** + * Finds a vector in the same direction as v but with norm 1. + * + * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/unit.m + * + * @param v - A vector. + * + * @returns A normalized vector. + */ +export function normalize([x, y, z]: Vector): Vector { + const norm = Math.hypot(x, y, z); + + // If the vector has norm == 0, i.e. all elements in the vector are zero, the + // unit vector [1 0 0]' is returned. + return norm === 0 ? [1, 0, 0] : [x / norm, y / norm, z / norm]; +} + +/** + * Transforms a vector by a matrix. + * + * @param r - A transformation matrix. + * @param v - A vector. + * + * @returns The transformed vector. + */ +export function transform( + [[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]]: Matrix, + [x, y, z]: Vector, +): Vector { + return [ + r11 * x + r12 * y + r13 * z, + r21 * x + r22 * y + r23 * z, + r31 * x + r32 * y + r33 * z, + ]; +} diff --git a/src/xyz2R.ts b/src/xyz2R.ts deleted file mode 100644 index b8d860e..0000000 --- a/src/xyz2R.ts +++ /dev/null @@ -1,42 +0,0 @@ -import type { Matrix3x3 } from "./matrix.js"; - -/** - * Creates a rotation matrix from 3 angles about new axes in the xyz order. - * - * The rotation matrix R_AB is created based on 3 angles x,y,z about new axes - * (intrinsic) in the order x-y-z. The angles (called Euler angles or Tait-Bryan - * angles) are defined by the following procedure of successive rotations: - * - * Given two arbitrary coordinate frames A and B. Consider a temporary frame T - * that initially coincides with A. In order to make T align with B, we first - * rotate T an angle x about its x-axis (common axis for both A and T). - * Secondly, T is rotated an angle y about the NEW y-axis of T. Finally, T is - * rotated an angle z about its NEWEST z-axis. The final orientation of T now - * coincides with the orientation of B. - * - * The signs of the angles are given by the directions of the axes and the right - * hand rule. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/xyz2R.m - * - * @param x - Angle of rotation about the new x-axis in radians. - * @param y - Angle of rotation about the new y-axis in radians. - * @param z - Angle of rotation about the new z-axis in radians. - * - * @returns 3x3 rotation matrix (direction cosine matrix) such that the relation - * between a vector v decomposed in A and B is given by: v_A = R_AB * v_B. - */ -export function xyz2R(x: number, y: number, z: number): Matrix3x3 { - const cz = Math.cos(z); - const sz = Math.sin(z); - const cy = Math.cos(y); - const sy = Math.sin(y); - const cx = Math.cos(x); - const sx = Math.sin(x); - - return [ - [cy * cz, -cy * sz, sy], - [sy * sx * cz + cx * sz, -sy * sx * sz + cx * cz, -cy * sx], - [-sy * cx * cz + sx * sz, sy * cx * sz + sx * cz, cy * cx], - ]; -} diff --git a/src/zyx2R.ts b/src/zyx2R.ts deleted file mode 100644 index 1ff33b4..0000000 --- a/src/zyx2R.ts +++ /dev/null @@ -1,44 +0,0 @@ -import type { Matrix3x3 } from "./matrix.js"; - -/** - * Creates a rotation matrix from 3 angles about new axes in the zyx order. - * - * The rotation matrix R_AB is created based on 3 angles z,y,x about new axes - * (intrinsic) in the order z-y-x. The angles (called Euler angles or Tait-Bryan - * angles) are defined by the following procedure of successive rotations: - * - * Given two arbitrary coordinate frames A and B. Consider a temporary frame T - * that initially coincides with A. In order to make T align with B, we first - * rotate T an angle z about its z-axis (common axis for both A and T). - * Secondly, T is rotated an angle y about the NEW y-axis of T. Finally, T is - * rotated an angle x about its NEWEST x-axis. The final orientation of T now - * coincides with the orientation of B. - * - * The signs of the angles are given by the directions of the axes and the right - * hand rule. - * - * Note that if A is a north-east-down frame and B is a body frame, we have that - * z = yaw, y = pitch and x = roll. - * - * @see https://github.com/FFI-no/n-vector/blob/f77f43d18ddb6b8ea4e1a8bb23a53700af965abb/nvector/zyx2R.m - * - * @param z - Angle of rotation about the new z-axis in radians. - * @param y - Angle of rotation about the new y-axis in radians. - * @param x - Angle of rotation about the new x-axis in radians. - * - * @returns The rotation matrix. - */ -export function zyx2R(z: number, y: number, x: number): Matrix3x3 { - const cz = Math.cos(z); - const sz = Math.sin(z); - const cy = Math.cos(y); - const sy = Math.sin(y); - const cx = Math.cos(x); - const sx = Math.sin(x); - - return [ - [cz * cy, -sz * cx + cz * sy * sx, sz * sx + cz * sy * cx], - [sz * cy, cz * cx + sz * sy * sx, -cz * sx + sz * sy * cx], - [-sy, cy * sx, cy * cx], - ]; -} diff --git a/test/arbitrary.ts b/test/arbitrary.ts index 25a8321..2c300d8 100644 --- a/test/arbitrary.ts +++ b/test/arbitrary.ts @@ -1,16 +1,10 @@ import { fc } from "@fast-check/vitest"; -import { - GRS_80, - WGS_72, - WGS_84, - WGS_84_SPHERE, - type Ellipsoid, -} from "../src/ellipsoid.js"; -import type { Matrix3x3, Vector3 } from "../src/index.js"; +import type { Ellipsoid, Matrix, Vector } from "nvector-geodesy"; +import { GRS_80, WGS_72, WGS_84 } from "nvector-geodesy"; const RADIAN = Math.PI / 180; -export function arbitrary3dRotationMatrix(): fc.Arbitrary { +export function arbitrary3dRotationMatrix(): fc.Arbitrary { return arbitraryQuaternion().map(([x, y, z, w]) => { // based on https://github.com/rawify/Quaternion.js/blob/c3834673b502e64e1866dbbf13568c0be93e52cc/quaternion.js#L791 const wx = w * x; @@ -33,13 +27,13 @@ export function arbitrary3dRotationMatrix(): fc.Arbitrary { export function arbitrary3dVector( magnitudeConstraints: fc.DoubleConstraints, -): fc.Arbitrary { +): fc.Arbitrary { return fc .tuple(arbitrary3dUnitVector(), fc.double(magnitudeConstraints)) .map(([[x, y, z], m]) => [x * m, y * m, z * m]); } -export function arbitrary3dUnitVector(): fc.Arbitrary { +export function arbitrary3dUnitVector(): fc.Arbitrary { // based on https://github.com/mrdoob/three.js/blob/a2e9ee8204b67f9dca79f48cf620a34a05aa8126/src/math/Vector3.js#L695 // https://mathworld.wolfram.com/SpherePointPicking.html @@ -58,29 +52,21 @@ export function arbitrary3dUnitVector(): fc.Arbitrary { export function arbitraryEllipsoid(): fc.Arbitrary { return fc.oneof( fc.constant(WGS_84), - fc.constant(WGS_84_SPHERE), - fc.constant(WGS_72), fc.constant(GRS_80), + fc.constant(WGS_72), ); } export function arbitraryEllipsoidDepth({ - a, - f, + b, }: Ellipsoid): fc.Arbitrary { - // semi-minor axis - const b = a * (1 - f); - return fc.double({ min: -b, max: b, noNaN: true }); } export function arbitraryEllipsoidECEFVector({ a, - f, -}: Ellipsoid): fc.Arbitrary { - // semi-minor axis - const b = a * (1 - f); - + b, +}: Ellipsoid): fc.Arbitrary { return arbitrary3dVector({ min: a - b, max: a + b, noNaN: true }); } diff --git a/test/nvector-test-api.ts b/test/nvector-test-api.ts index fbe773d..15736f9 100644 --- a/test/nvector-test-api.ts +++ b/test/nvector-test-api.ts @@ -1,38 +1,38 @@ -import { WebSocket } from "ws"; import type { - Matrix3x3, - R2xyz, - R2zyx, - R_EL2n_E, - R_EN2n_E, - Vector3, - lat_long2n_E, - n_E2R_EN, - n_E2lat_long, - n_EA_E_and_n_EB_E2p_AB_E, - n_EA_E_and_p_AB_E2n_EB_E, - n_EB_E2p_EB_E, - n_E_and_wa2R_EL, - p_EB_E2n_EB_E, - xyz2R, - zyx2R, -} from "../src/index.js"; + Matrix, + Vector, + delta, + destination, + eulerXYZToRotationMatrix, + eulerZYXToRotationMatrix, + fromECEF, + fromGeodeticCoordinates, + fromRotationMatrix, + rotationMatrixToEulerXYZ, + rotationMatrixToEulerZYX, + toECEF, + toGeodeticCoordinates, + toRotationMatrix, + toRotationMatrixUsingWanderAzimuth, +} from "nvector-geodesy"; +import { WebSocket } from "ws"; export type NvectorTestClient = { - lat_long2n_E: Async; - n_E_and_wa2R_EL: Async; - n_E2lat_long: 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; - p_EB_E2n_EB_E: Async; - R_EL2n_E: Async; - R_EN2n_E: Async; - R2xyz: Async; - R2zyx: Async; - xyz2R: Async; - zyx2R: Async; + fromGeodeticCoordinates: Async; + toRotationMatrixUsingWanderAzimuth: Async< + typeof toRotationMatrixUsingWanderAzimuth + >; + toGeodeticCoordinates: Async; + toRotationMatrix: Async; + delta: Async; + destination: Async; + toECEF: Async; + fromECEF: Async; + fromRotationMatrix: Async; + rotationMatrixToEulerXYZ: Async; + rotationMatrixToEulerZYX: Async; + eulerXYZToRotationMatrix: Async; + eulerZYXToRotationMatrix: Async; close: () => void; }; @@ -47,131 +47,129 @@ export async function createNvectorTestClient(): Promise { }); return { - async lat_long2n_E(latitude, longitude, R_Ee) { + async fromGeodeticCoordinates(latitude, longitude, frame) { return unwrapVector3( await call("lat_lon2n_E", { latitude, longitude, - R_Ee, + R_Ee: frame, }), ); }, - async n_E_and_wa2R_EL(n_E, wander_azimuth, R_Ee) { - return await call("n_E_and_wa2R_EL", { - n_E: wrapVector3(n_E), - wander_azimuth, - R_Ee, + async toRotationMatrixUsingWanderAzimuth(vector, wanderAzimuth, frame) { + return await call("n_E_and_wa2R_EL", { + n_E: wrapVector3(vector), + wander_azimuth: wanderAzimuth, + R_Ee: frame, }); }, - async n_E2lat_long(n_E, R_Ee) { + async toGeodeticCoordinates(vector, frame) { const { latitude, longitude } = await call<{ latitude: number; longitude: number; }>("n_E2lat_lon", { - n_E: wrapVector3(n_E), - R_Ee, + n_E: wrapVector3(vector), + R_Ee: frame, }); 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 toRotationMatrix(vector, frame) { + return await call("n_E2R_EN", { + n_E: wrapVector3(vector), + R_Ee: frame, }); }, - async n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, z_EA, z_EB, a, f, R_Ee) { + async delta(from, to, fromDepth, toDepth, ellipsoid, frame) { return unwrapVector3( await call("n_EA_E_and_n_EB_E2p_AB_E", { - n_EA_E: wrapVector3(n_EA_E), - n_EB_E: wrapVector3(n_EB_E), - z_EA, - z_EB, - a, - f, - R_Ee, + n_EA_E: wrapVector3(from), + n_EB_E: wrapVector3(to), + z_EA: fromDepth, + z_EB: toDepth, + a: ellipsoid?.a, + f: ellipsoid?.f, + R_Ee: frame, }), ); }, - async n_EA_E_and_p_AB_E2n_EB_E(n_EA_E, p_AB_E, z_EA, a, f, R_Ee) { + async destination(from, delta, fromDepth, ellipsoid, frame) { 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, + n_EA_E: wrapVector3(from), + p_AB_E: wrapVector3(delta), + z_EA: fromDepth, + a: ellipsoid?.a, + f: ellipsoid?.f, + R_Ee: frame, }); return [unwrapVector3(n_EB_E), z_EB]; }, - async n_EB_E2p_EB_E(n_EB_E, depth, a, f, R_Ee) { + async toECEF(vector, depth, ellipsoid, frame) { return unwrapVector3( await call("n_EB_E2p_EB_E", { - n_EB_E: wrapVector3(n_EB_E), + n_EB_E: wrapVector3(vector), depth, - a, - f, - R_Ee, + a: ellipsoid?.a, + f: ellipsoid?.f, + R_Ee: frame, }), ); }, - async p_EB_E2n_EB_E(p_EB_E, a, f, R_Ee) { + async fromECEF(vector, ellipsoid, frame) { const { n_EB_E, depth } = await call<{ n_EB_E: WrappedVector3; depth: number; }>("p_EB_E2n_EB_E", { - p_EB_E: wrapVector3(p_EB_E), - a, - f, - R_Ee, + p_EB_E: wrapVector3(vector), + a: ellipsoid?.a, + f: ellipsoid?.f, + R_Ee: frame, }); return [unwrapVector3(n_EB_E), depth]; }, - async R_EL2n_E(R_EL) { - return unwrapVector3(await call("R_EL2n_E", { R_EL })); - }, - - async R_EN2n_E(R_EN) { - return unwrapVector3(await call("R_EN2n_E", { R_EN })); + async fromRotationMatrix(rotation) { + return unwrapVector3( + await call("R_EN2n_E", { R_EN: rotation }), + ); }, - async R2xyz(R_AB) { + async rotationMatrixToEulerXYZ(rotation) { const { x, y, z } = await call<{ x: number; y: number; z: number }>( "R2xyz", - { R_AB }, + { R_AB: rotation }, ); return [x, y, z]; }, - async R2zyx(R_AB) { + async rotationMatrixToEulerZYX(rotation) { const { z, y, x } = await call<{ z: number; y: number; x: number }>( "R2zyx", - { R_AB }, + { R_AB: rotation }, ); return [z, y, x]; }, - async xyz2R(x, y, z) { - return await call("xyz2R", { x, y, z }); + async eulerXYZToRotationMatrix(x, y, z) { + return await call("xyz2R", { x, y, z }); }, - async zyx2R(z, y, x) { - return await call("zyx2R", { z, y, x }); + async eulerZYXToRotationMatrix(z, y, x) { + return await call("zyx2R", { z, y, x }); }, close: () => ws.close(), @@ -217,11 +215,11 @@ type ErrorMessage = { type WrappedVector3 = [[x: number], [y: number], [z: number]]; -function wrapVector3([x, y, z]: Vector3): WrappedVector3 { +function wrapVector3([x, y, z]: Vector): WrappedVector3 { return [[x], [y], [z]]; } -function unwrapVector3([[x], [y], [z]]: WrappedVector3): Vector3 { +function unwrapVector3([[x], [y], [z]]: WrappedVector3): Vector { return [x, y, z]; } diff --git a/test/vitest/R2xyz.spec.ts b/test/vitest/R2xyz.spec.ts deleted file mode 100644 index 837b35f..0000000 --- a/test/vitest/R2xyz.spec.ts +++ /dev/null @@ -1,59 +0,0 @@ -import { it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { R2xyz } from "../../src/index.js"; -import { arbitrary3dRotationMatrix } from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { createNvectorTestClient } from "../nvector-test-api.js"; -import { angleDelta } from "../util.js"; - -const TEST_DURATION = 5000; - -describe("R2xyz()", () => { - let nvectorTestClient: NvectorTestClient; - - beforeAll(async () => { - nvectorTestClient = await createNvectorTestClient(); - }); - - afterAll(() => { - nvectorTestClient?.close(); - }); - - it.prop([arbitrary3dRotationMatrix()], { - interruptAfterTimeLimit: TEST_DURATION, - numRuns: Infinity, - })( - "matches the reference implementation", - async (R_AB) => { - const expected = await nvectorTestClient.R2xyz(R_AB); - - expect(expected).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - - const actual = R2xyz(R_AB); - - expect(actual).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - expect(angleDelta(actual[0], expected[0])).toBeCloseTo(0, 15); - expect(angleDelta(actual[1], expected[1])).toBeCloseTo(0, 15); - expect(angleDelta(actual[2], expected[2])).toBeCloseTo(0, 15); - }, - TEST_DURATION + 1000, - ); - - it("handles Euler angle singularity", () => { - expect( - R2xyz([ - [0, 0, 1], - [0, 1, 0], - [1, 0, 0], - ]), - ).toMatchObject([0, Math.PI / 2, 0]); - }); -}); diff --git a/test/vitest/R2zyx.spec.ts b/test/vitest/R2zyx.spec.ts deleted file mode 100644 index dd57a39..0000000 --- a/test/vitest/R2zyx.spec.ts +++ /dev/null @@ -1,59 +0,0 @@ -import { it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { R2zyx } from "../../src/index.js"; -import { arbitrary3dRotationMatrix } from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { createNvectorTestClient } from "../nvector-test-api.js"; -import { angleDelta } from "../util.js"; - -const TEST_DURATION = 5000; - -describe("R2zyx()", () => { - let nvectorTestClient: NvectorTestClient; - - beforeAll(async () => { - nvectorTestClient = await createNvectorTestClient(); - }); - - afterAll(() => { - nvectorTestClient?.close(); - }); - - it.prop([arbitrary3dRotationMatrix()], { - interruptAfterTimeLimit: TEST_DURATION, - numRuns: Infinity, - })( - "matches the reference implementation", - async (R_AB) => { - const expected = await nvectorTestClient.R2zyx(R_AB); - - expect(expected).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - - const actual = R2zyx(R_AB); - - expect(actual).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - expect(angleDelta(actual[0], expected[0])).toBeCloseTo(0, 15); - expect(angleDelta(actual[1], expected[1])).toBeCloseTo(0, 15); - expect(angleDelta(actual[2], expected[2])).toBeCloseTo(0, 15); - }, - TEST_DURATION + 1000, - ); - - it("handles Euler angle singularity", () => { - expect( - R2zyx([ - [0, 0, 1], - [0, 1, 0], - [1, 0, 0], - ]), - ).toMatchObject([-0, Math.PI / 2, 0]); - }); -}); diff --git a/test/vitest/R_EL2n_E.spec.ts b/test/vitest/R_EL2n_E.spec.ts deleted file mode 100644 index 448fcbd..0000000 --- a/test/vitest/R_EL2n_E.spec.ts +++ /dev/null @@ -1,48 +0,0 @@ -import { it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { R_EL2n_E } from "../../src/index.js"; -import { arbitrary3dRotationMatrix } from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { createNvectorTestClient } from "../nvector-test-api.js"; - -const TEST_DURATION = 5000; - -describe("R_EL2n_E()", () => { - let nvectorTestClient: NvectorTestClient; - - beforeAll(async () => { - nvectorTestClient = await createNvectorTestClient(); - }); - - afterAll(() => { - nvectorTestClient?.close(); - }); - - it.prop([arbitrary3dRotationMatrix()], { - interruptAfterTimeLimit: TEST_DURATION, - numRuns: Infinity, - })( - "matches the reference implementation", - async (R_EL) => { - const expected = await nvectorTestClient.R_EL2n_E(R_EL); - - expect(expected).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - - const actual = R_EL2n_E(R_EL); - - expect(actual).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - expect(actual[0]).toBeCloseTo(expected[0], 15); - expect(actual[1]).toBeCloseTo(expected[1], 15); - expect(actual[2]).toBeCloseTo(expected[2], 15); - }, - TEST_DURATION + 1000, - ); -}); diff --git a/test/vitest/R_EN2n_E.spec.ts b/test/vitest/R_EN2n_E.spec.ts deleted file mode 100644 index 80db5ca..0000000 --- a/test/vitest/R_EN2n_E.spec.ts +++ /dev/null @@ -1,48 +0,0 @@ -import { it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { R_EN2n_E } from "../../src/index.js"; -import { arbitrary3dRotationMatrix } from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { createNvectorTestClient } from "../nvector-test-api.js"; - -const TEST_DURATION = 5000; - -describe("R_EN2n_E()", () => { - let nvectorTestClient: NvectorTestClient; - - beforeAll(async () => { - nvectorTestClient = await createNvectorTestClient(); - }); - - afterAll(() => { - nvectorTestClient?.close(); - }); - - it.prop([arbitrary3dRotationMatrix()], { - interruptAfterTimeLimit: TEST_DURATION, - numRuns: Infinity, - })( - "matches the reference implementation", - async (R_EN) => { - const expected = await nvectorTestClient.R_EN2n_E(R_EN); - - expect(expected).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - - const actual = R_EN2n_E(R_EN); - - expect(actual).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - expect(actual[0]).toBeCloseTo(expected[0], 15); - expect(actual[1]).toBeCloseTo(expected[1], 15); - expect(actual[2]).toBeCloseTo(expected[2], 15); - }, - TEST_DURATION + 1000, - ); -}); diff --git a/test/vitest/angle.spec.ts b/test/vitest/angle.spec.ts new file mode 100644 index 0000000..be46d8e --- /dev/null +++ b/test/vitest/angle.spec.ts @@ -0,0 +1,15 @@ +import { it } from "@fast-check/vitest"; +import { degrees, radians } from "nvector-geodesy"; +import { describe, expect } from "vitest"; + +describe("degrees()", () => { + it("converts angle in radians to degrees", () => { + expect(degrees(1)).toBeCloseTo(57.29577951308232, 15); + }); +}); + +describe("radians()", () => { + it("converts angle in degrees to radians", () => { + expect(radians(57.29577951308232)).toBeCloseTo(1, 15); + }); +}); diff --git a/test/vitest/coords.spec.ts b/test/vitest/coords.spec.ts new file mode 100644 index 0000000..dbc1d4f --- /dev/null +++ b/test/vitest/coords.spec.ts @@ -0,0 +1,99 @@ +import { fc, it } from "@fast-check/vitest"; +import { + fromGeodeticCoordinates, + toGeodeticCoordinates, +} from "nvector-geodesy"; +import { afterAll, beforeAll, describe, expect } from "vitest"; +import { + arbitrary3dRotationMatrix, + arbitrary3dUnitVector, + arbitraryLatLon, +} from "../arbitrary.js"; +import type { NvectorTestClient } from "../nvector-test-api.js"; +import { createNvectorTestClient } from "../nvector-test-api.js"; + +const TEST_DURATION = 5000; + +describe("fromGeodeticCoordinates()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop( + [ + arbitraryLatLon(), + fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), + ], + { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, + )( + "matches the reference implementation", + async ([latitude, longitude], frame) => { + const expected = await nvectorTestClient.fromGeodeticCoordinates( + latitude, + longitude, + frame, + ); + + expect(expected).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + + const actual = fromGeodeticCoordinates(latitude, longitude, frame); + + expect(actual).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + expect(actual[0]).toBeCloseTo(expected[0], 15); + expect(actual[1]).toBeCloseTo(expected[1], 15); + expect(actual[2]).toBeCloseTo(expected[2], 15); + }, + TEST_DURATION + 1000, + ); +}); + +describe("toGeodeticCoordinates()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop( + [ + arbitrary3dUnitVector(), + fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), + ], + { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, + )( + "matches the reference implementation", + async (vector, frame) => { + const expected = await nvectorTestClient.toGeodeticCoordinates( + vector, + frame, + ); + + expect(expected).toMatchObject([expect.any(Number), expect.any(Number)]); + + const actual = toGeodeticCoordinates(vector, frame); + + expect(actual).toMatchObject([expect.any(Number), expect.any(Number)]); + expect(actual[0]).toBeCloseTo(expected[0], 15); + expect(actual[1]).toBeCloseTo(expected[1], 15); + }, + TEST_DURATION + 1000, + ); +}); diff --git a/test/vitest/delta.spec.ts b/test/vitest/delta.spec.ts new file mode 100644 index 0000000..5c37cd9 --- /dev/null +++ b/test/vitest/delta.spec.ts @@ -0,0 +1,185 @@ +import { fc, it } from "@fast-check/vitest"; +import { + WGS_84, + Z_AXIS_NORTH, + delta, + destination, + toECEF, + transform, + type Vector, +} from "nvector-geodesy"; +import { afterAll, beforeAll, describe, expect } from "vitest"; +import { + arbitrary3dRotationMatrix, + arbitrary3dUnitVector, + arbitraryEllipsoid, + arbitraryEllipsoidDepth, + arbitraryEllipsoidECEFVector, +} from "../arbitrary.js"; +import type { NvectorTestClient } from "../nvector-test-api.js"; +import { createNvectorTestClient } from "../nvector-test-api.js"; + +const TEST_DURATION = 5000; + +describe("delta()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop( + [ + arbitrary3dUnitVector(), + arbitrary3dUnitVector(), + arbitraryEllipsoid().chain((ellipsoid) => { + return fc.tuple( + fc.option(arbitraryEllipsoidDepth(ellipsoid), { + nil: undefined, + }), + fc.option(arbitraryEllipsoidDepth(ellipsoid), { + nil: undefined, + }), + fc.option(fc.constant(ellipsoid), { nil: undefined }), + ); + }), + fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), + ], + { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, + )( + "matches the reference implementation", + async (from, to, [fromDepth, toDepth, ellipsoid], frame) => { + const expected = await nvectorTestClient.delta( + from, + to, + fromDepth, + toDepth, + ellipsoid, + frame, + ); + + expect(expected).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + + const actual = delta(from, to, fromDepth, toDepth, ellipsoid, frame); + + expect(actual).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + expect(actual[0]).toBeCloseTo(expected[0], 7); + expect(actual[1]).toBeCloseTo(expected[1], 7); + expect(actual[2]).toBeCloseTo(expected[2], 7); + }, + TEST_DURATION + 1000, + ); +}); + +describe("destination()", () => { + 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), { nil: undefined }), + fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), + ); + }) + .filter( + ([ + from, + delta, + fromDepth, + ellipsoid = WGS_84, + frame = Z_AXIS_NORTH, + ]) => { + const { a, f } = ellipsoid; + const [ax, ay, az] = toECEF(from, fromDepth, ellipsoid, frame); + const b: Vector = [ax + delta[0], ay + delta[1], az + delta[2]]; + const [bx, by, bz] = transform(frame, b); + + // filter vectors where the x or yz components are zero after + // rotation + // this causes a division by zero in the Python implementation + if (bx === 0 || by + bz === 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 e2 = (2.0 - f) * f; + const R2 = b[1] ** 2 + b[2] ** 2; + const p = R2 / a ** 2; + const q = ((1 - e2) / a ** 2) * b[0] ** 2; + const r = (p + q - e2 ** 2) / 6; + + return (e2 ** 2 * p * q) / (4 * r ** 3); + })(); + if (Number.isNaN(s) || s <= 0) return false; + + return true; + }, + ), + ], + { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, + )( + "matches the reference implementation", + async ([from, delta, fromDepth, ellipsoid, frame]) => { + const [expectedVector, expectedDepth] = + await nvectorTestClient.destination( + from, + delta, + fromDepth, + ellipsoid, + frame, + ); + + expect(expectedVector).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + expect(expectedDepth).toEqual(expect.any(Number)); + + const [actualVector, actualDepth] = destination( + from, + delta, + fromDepth, + ellipsoid, + frame, + ); + + expect(actualVector).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + expect(actualVector[0]).toBeCloseTo(expectedVector[0], 12); + expect(actualVector[1]).toBeCloseTo(expectedVector[1], 12); + expect(actualVector[2]).toBeCloseTo(expectedVector[2], 12); + expect(actualDepth).toBeCloseTo(expectedDepth, 7); + }, + TEST_DURATION + 1000, + ); +}); diff --git a/test/vitest/ecef.spec.ts b/test/vitest/ecef.spec.ts new file mode 100644 index 0000000..478b3ce --- /dev/null +++ b/test/vitest/ecef.spec.ts @@ -0,0 +1,154 @@ +import { fc, it } from "@fast-check/vitest"; +import { + WGS_84, + Z_AXIS_NORTH, + fromECEF, + toECEF, + transform, +} from "nvector-geodesy"; +import { afterAll, beforeAll, describe, expect } from "vitest"; +import { + arbitrary3dRotationMatrix, + arbitrary3dUnitVector, + arbitraryEllipsoid, + arbitraryEllipsoidDepth, + arbitraryEllipsoidECEFVector, +} from "../arbitrary.js"; +import type { NvectorTestClient } from "../nvector-test-api.js"; +import { createNvectorTestClient } from "../nvector-test-api.js"; + +const TEST_DURATION = 5000; + +describe("fromECEF()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop( + [ + arbitraryEllipsoid() + .chain((ellipsoid) => { + return fc.tuple( + arbitraryEllipsoidECEFVector(ellipsoid), + fc.option(fc.constant(ellipsoid), { nil: undefined }), + fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), + ); + }) + .filter(([from, ellipsoid = WGS_84, frame = Z_AXIS_NORTH]) => { + const { a, f } = ellipsoid; + const [x, y, z] = transform(frame, from); + + // filter vectors where the x or yz components are zero after + // rotation + // this causes a division by zero in the Python implementation + if (x === 0 || y + z === 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 e2 = (2.0 - f) * f; + const R2 = from[1] ** 2 + from[2] ** 2; + const p = R2 / a ** 2; + const q = ((1 - e2) / a ** 2) * from[0] ** 2; + const r = (p + q - e2 ** 2) / 6; + + return (e2 ** 2 * p * q) / (4 * r ** 3); + })(); + if (Number.isNaN(s) || s <= 0) return false; + + return true; + }), + ], + { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, + )( + "matches the reference implementation", + async ([from, ellipsoid, frame]) => { + const [expectedVector, expectedDepth] = await nvectorTestClient.fromECEF( + from, + ellipsoid, + frame, + ); + + expect(expectedVector).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + expect(expectedDepth).toEqual(expect.any(Number)); + + const [actualVector, actualDepth] = fromECEF(from, ellipsoid, frame); + + expect(actualVector).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + 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, + ); +}); + +describe("toECEF()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop( + [ + arbitrary3dUnitVector(), + arbitraryEllipsoid().chain((ellipsoid) => { + return fc.tuple( + fc.option(arbitraryEllipsoidDepth(ellipsoid), { nil: undefined }), + fc.option(fc.constant(ellipsoid), { nil: undefined }), + ); + }), + fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), + ], + { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, + )( + "matches the reference implementation", + async (vector, [depth, ellipsoid], frame) => { + const expected = await nvectorTestClient.toECEF( + vector, + depth, + ellipsoid, + frame, + ); + + expect(expected).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + + const actual = toECEF(vector, depth, ellipsoid, frame); + + expect(actual).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + expect(actual[0]).toBeCloseTo(expected[0], 7); + expect(actual[1]).toBeCloseTo(expected[1], 7); + expect(actual[2]).toBeCloseTo(expected[2], 7); + }, + TEST_DURATION + 1000, + ); +}); diff --git a/test/vitest/ellipsoid.spec.ts b/test/vitest/ellipsoid.spec.ts new file mode 100644 index 0000000..52233b5 --- /dev/null +++ b/test/vitest/ellipsoid.spec.ts @@ -0,0 +1,32 @@ +import { it } from "@fast-check/vitest"; +import { GRS_80, WGS_72, WGS_84, sphere } from "nvector-geodesy"; +import { describe, expect } from "vitest"; + +describe("GRS_80", () => { + it("has the correct semi-minor axis", () => { + expect(GRS_80.b).toEqual(GRS_80.a * (1 - GRS_80.f)); + }); +}); + +describe("WGS_72", () => { + it("has the correct semi-minor axis", () => { + expect(WGS_72.b).toEqual(WGS_72.a * (1 - WGS_72.f)); + }); +}); + +describe("WGS_84", () => { + it("has the correct semi-minor axis", () => { + expect(WGS_84.b).toEqual(WGS_84.a * (1 - WGS_84.f)); + }); +}); + +describe("sphere", () => { + it("produces a sphere with the given radius", () => { + const radius = 6371e3; + const ellipsoid = sphere(radius); + + expect(ellipsoid.a).toEqual(radius); + expect(ellipsoid.b).toEqual(radius); + expect(ellipsoid.f).toEqual(0); + }); +}); diff --git a/test/vitest/euler.spec.ts b/test/vitest/euler.spec.ts new file mode 100644 index 0000000..6083505 --- /dev/null +++ b/test/vitest/euler.spec.ts @@ -0,0 +1,216 @@ +import { it } from "@fast-check/vitest"; +import { + eulerXYZToRotationMatrix, + eulerZYXToRotationMatrix, + rotationMatrixToEulerXYZ, + rotationMatrixToEulerZYX, +} from "nvector-geodesy"; +import { afterAll, beforeAll, describe, expect } from "vitest"; +import { arbitrary3dRotationMatrix, arbitraryRadians } from "../arbitrary.js"; +import type { NvectorTestClient } from "../nvector-test-api.js"; +import { createNvectorTestClient } from "../nvector-test-api.js"; +import { angleDelta } from "../util.js"; + +const TEST_DURATION = 5000; + +describe("eulerXYZToRotationMatrix()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop([arbitraryRadians(), arbitraryRadians(), arbitraryRadians()], { + interruptAfterTimeLimit: TEST_DURATION, + numRuns: Infinity, + })( + "matches the reference implementation", + async (x, y, z) => { + const expected = await nvectorTestClient.eulerXYZToRotationMatrix( + x, + y, + z, + ); + + 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 = eulerXYZToRotationMatrix(x, y, z); + + 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], 15); + expect(actual[0][1]).toBeCloseTo(expected[0][1], 15); + expect(actual[0][2]).toBeCloseTo(expected[0][2], 15); + expect(actual[1][0]).toBeCloseTo(expected[1][0], 15); + expect(actual[1][1]).toBeCloseTo(expected[1][1], 15); + expect(actual[1][2]).toBeCloseTo(expected[1][2], 15); + expect(actual[2][0]).toBeCloseTo(expected[2][0], 15); + expect(actual[2][1]).toBeCloseTo(expected[2][1], 15); + expect(actual[2][2]).toBeCloseTo(expected[2][2], 15); + }, + TEST_DURATION + 1000, + ); +}); + +describe("eulerZYXToRotationMatrix()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop([arbitraryRadians(), arbitraryRadians(), arbitraryRadians()], { + interruptAfterTimeLimit: TEST_DURATION, + numRuns: Infinity, + })( + "matches the reference implementation", + async (z, y, x) => { + const expected = await nvectorTestClient.eulerZYXToRotationMatrix( + z, + y, + x, + ); + + 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 = eulerZYXToRotationMatrix(z, y, x); + + 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], 15); + expect(actual[0][1]).toBeCloseTo(expected[0][1], 15); + expect(actual[0][2]).toBeCloseTo(expected[0][2], 15); + expect(actual[1][0]).toBeCloseTo(expected[1][0], 15); + expect(actual[1][1]).toBeCloseTo(expected[1][1], 15); + expect(actual[1][2]).toBeCloseTo(expected[1][2], 15); + expect(actual[2][0]).toBeCloseTo(expected[2][0], 15); + expect(actual[2][1]).toBeCloseTo(expected[2][1], 15); + expect(actual[2][2]).toBeCloseTo(expected[2][2], 15); + }, + TEST_DURATION + 1000, + ); +}); + +describe("rotationMatrixToEulerXYZ()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop([arbitrary3dRotationMatrix()], { + interruptAfterTimeLimit: TEST_DURATION, + numRuns: Infinity, + })( + "matches the reference implementation", + async (rotation) => { + const expected = + await nvectorTestClient.rotationMatrixToEulerXYZ(rotation); + + expect(expected).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + + const actual = rotationMatrixToEulerXYZ(rotation); + + expect(actual).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + expect(angleDelta(actual[0], expected[0])).toBeCloseTo(0, 15); + expect(angleDelta(actual[1], expected[1])).toBeCloseTo(0, 15); + expect(angleDelta(actual[2], expected[2])).toBeCloseTo(0, 15); + }, + TEST_DURATION + 1000, + ); + + it("handles Euler angle singularity", () => { + expect( + rotationMatrixToEulerXYZ([ + [0, 0, 1], + [0, 1, 0], + [1, 0, 0], + ]), + ).toMatchObject([0, Math.PI / 2, 0]); + }); +}); + +describe("rotationMatrixToEulerZYX()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop([arbitrary3dRotationMatrix()], { + interruptAfterTimeLimit: TEST_DURATION, + numRuns: Infinity, + })( + "matches the reference implementation", + async (rotation) => { + const expected = + await nvectorTestClient.rotationMatrixToEulerZYX(rotation); + + expect(expected).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + + const actual = rotationMatrixToEulerZYX(rotation); + + expect(actual).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + expect(angleDelta(actual[0], expected[0])).toBeCloseTo(0, 15); + expect(angleDelta(actual[1], expected[1])).toBeCloseTo(0, 15); + expect(angleDelta(actual[2], expected[2])).toBeCloseTo(0, 15); + }, + TEST_DURATION + 1000, + ); + + it("handles Euler angle singularity", () => { + expect( + rotationMatrixToEulerZYX([ + [0, 0, 1], + [0, 1, 0], + [1, 0, 0], + ]), + ).toMatchObject([-0, Math.PI / 2, 0]); + }); +}); diff --git a/test/vitest/examples/example-01.spec.ts b/test/vitest/examples/example-01.spec.ts index a72e4f2..a5a6dc4 100644 --- a/test/vitest/examples/example-01.spec.ts +++ b/test/vitest/examples/example-01.spec.ts @@ -1,12 +1,12 @@ -import { expect, test } from "vitest"; import { - lat_long2n_E, - n_E2R_EN, - n_EA_E_and_n_EB_E2p_AB_E, - rad, - rotate, + delta, + fromGeodeticCoordinates, + radians, + toRotationMatrix, + transform, transpose, -} from "../../../src/index.js"; +} from "nvector-geodesy"; +import { expect, test } from "vitest"; /** * Example 1: A and B to delta @@ -15,57 +15,73 @@ import { * north, east and down, and find the direction (azimuth/bearing) to B, relative * to north. Use WGS-84 ellipsoid. * - * - Assume WGS-84 ellipsoid. The given depths are from the ellipsoid surface. - * - Use position A to define north, east, and down directions. (Due to the - * curvature of Earth and different directions to the North Pole, the north, - * east, and down directions will change (relative to Earth) for different - * places. Position A must be outside the poles for the north and east - * directions to be defined. - * * @see https://www.ffi.no/en/research/n-vector/#example_1 */ test("Example 1", () => { - // Positions A and B are given in (decimal) degrees and depths: - - // Position A: - const lat_EA_deg = 1; - const long_EA_deg = 2; - const z_EA = 3; + // PROBLEM: - // Position B: - const lat_EB_deg = 4; - const long_EB_deg = 5; - const z_EB = 6; + // Given two positions, A and B as latitudes, longitudes and depths (relative + // to Earth, E): + const aLat = 1, + aLon = 2, + aDepth = 3; + const bLat = 4, + bLon = 5, + bDepth = 6; // Find the exact vector between the two positions, given in meters north, - // east, and down, i.e. find p_AB_N. + // east, and down, and find the direction (azimuth) to B, relative to north. + // + // Details: + // + // - Assume WGS-84 ellipsoid. The given depths are from the ellipsoid surface. + // - Use position A to define north, east, and down directions. (Due to the + // curvature of Earth and different directions to the North Pole, the north, + // east, and down directions will change (relative to Earth) for different + // places. Position A must be outside the poles for the north and east + // directions to be defined. // SOLUTION: - // Step1: Convert to n-vectors (rad() converts to radians): - const n_EA_E = lat_long2n_E(rad(lat_EA_deg), rad(long_EA_deg)); - const n_EB_E = lat_long2n_E(rad(lat_EB_deg), rad(long_EB_deg)); + // Step 1 + // + // First, the given latitudes and longitudes are converted to n-vectors: + const a = fromGeodeticCoordinates(radians(aLat), radians(aLon)); + const b = fromGeodeticCoordinates(radians(bLat), radians(bLon)); - // Step2: Find p_AB_E (delta decomposed in E). WGS-84 ellipsoid is default: - const p_AB_E = n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, z_EA, z_EB); + // Step 2 + // + // When the positions are given as n-vectors (and depths), it is easy to find + // the delta vector decomposed in E. No ellipsoid is specified when calling + // the function, thus WGS-84 (default) is used: + const abE = delta(a, b, aDepth, bDepth); - // Step3: Find R_EN for position A: - const R_EN = n_E2R_EN(n_EA_E); + // Step 3 + // + // We now have the delta vector from A to B, but the three coordinates of the + // vector are along the Earth coordinate frame E, while we need the + // coordinates to be north, east and down. To get this, we define a + // North-East-Down coordinate frame called N, and then we need the rotation + // matrix (direction cosine matrix) rEN to go between E and N. We have a + // simple function that calculates rEN from an n-vector, and we use this + // function (using the n-vector at position A): + const rEN = toRotationMatrix(a); - // Step4: Find p_AB_N - const p_AB_N = rotate(transpose(R_EN), p_AB_E); - // (Note the transpose of R_EN: The "closest-rule" says that when decomposing, - // the frame in the subscript of the rotation matrix that is closest to the - // vector, should equal the frame where the vector is decomposed. Thus the - // calculation R_NE*p_AB_E is correct, since the vector is decomposed in E, - // and E is closest to the vector. In the above example we only had R_EN, and - // thus we must transpose it: R_EN' = R_NE) + // Step 4 + // + // Now the delta vector is easily decomposed in N. Since the vector is + // decomposed in E, we must use rNE (rNE is the transpose of rEN): + const abN = transform(transpose(rEN), abE); - // Step5: Also find the direction (azimuth) to B, relative to north: - const azimuth = Math.atan2(p_AB_N[1], p_AB_N[0]); + // Step 5 + // + // The three components of abN are the north, east and down displacements from + // A to B in meters. The azimuth is simply found from element 1 and 2 of the + // vector (the north and east components): + const azimuth = Math.atan2(abN[1], abN[0]); - expect(p_AB_N[0]).toBeCloseTo(331730.2347808944, 8); // meters - expect(p_AB_N[1]).toBeCloseTo(332997.8749892695, 8); // meters - expect(p_AB_N[2]).toBeCloseTo(17404.27136193635, 8); // meters - expect(azimuth).toBeCloseTo(rad(45.10926323826139), 15); + expect(abN[0]).toBeCloseTo(331730.2347808944, 8); + expect(abN[1]).toBeCloseTo(332997.8749892695, 8); + expect(abN[2]).toBeCloseTo(17404.27136193635, 8); + expect(azimuth).toBeCloseTo(radians(45.10926323826139), 15); }); diff --git a/test/vitest/examples/example-02.spec.ts b/test/vitest/examples/example-02.spec.ts index ef09050..5b60055 100644 --- a/test/vitest/examples/example-02.spec.ts +++ b/test/vitest/examples/example-02.spec.ts @@ -1,17 +1,17 @@ -import { expect, test } from "vitest"; import { WGS_72, - deg, + degrees, + destination, + eulerZYXToRotationMatrix, multiply, - n_E2R_EN, - n_E2lat_long, - n_EA_E_and_p_AB_E2n_EB_E, - rad, - rotate, - unit, - zyx2R, - type Vector3, -} from "../../../src/index.js"; + normalize, + radians, + toGeodeticCoordinates, + toRotationMatrix, + transform, + type Vector, +} from "nvector-geodesy"; +import { expect, test } from "vitest"; /** * Example 2: B and delta to C @@ -22,48 +22,60 @@ import { * @see https://www.ffi.no/en/research/n-vector/#example_2 */ test("Example 2", () => { - // delta vector from B to C, decomposed in B is given: - const p_BC_B: Vector3 = [3000, 2000, 100]; + // PROBLEM: - // Position and orientation of B is given: - // unit to get unit length of vector - const n_EB_E = unit([1, 2, 3]); - const z_EB = -400; - // the three angles are yaw, pitch, and roll - const R_NB = zyx2R(rad(10), rad(20), rad(30)); + // A radar or sonar attached to a vehicle B (Body coordinate frame) measures + // the distance and direction to an object C. We assume that the distance and + // two angles measured by the sensor (typically bearing and elevation relative + // to B) are already converted (by converting from spherical to Cartesian + // coordinates) to the vector bcB (i.e. the vector from B to C, decomposed in + // B): + const bcB: Vector = [3000, 2000, 100]; - // A custom reference ellipsoid is given (replacing WGS-84): - // (WGS-72) - const a = WGS_72.a; - const f = WGS_72.f; + // The position of B is given as an n-vector and a depth: + const b = normalize([1, 2, 3]); + const bDepth = -400; - // Find the position of C. + // The orientation (attitude) of B is given as rNB, specified as yaw, pitch, + // roll: + const rNB = eulerZYXToRotationMatrix(radians(10), radians(20), radians(30)); - // SOLUTION: + // Use the WGS-72 ellipsoid: + const e = WGS_72; - // Step1: Find R_EN: - const R_EN = n_E2R_EN(n_EB_E); + // Find the exact position of object C as an n-vector and a depth. + + // SOLUTION: - // Step2: Find R_EB, from R_EN and R_NB: - // Note: closest frames cancel - const R_EB = multiply(R_EN, R_NB); + // Step 1 + // + // The delta vector is given in B. It should be decomposed in E before using + // it, and thus we need rEB. This matrix is found from the matrices rEN and + // rNB, and we need to find rEN, as in Example 1: + const rEN = toRotationMatrix(b); - // Step3: Decompose the delta vector in E: - // no transpose of R_EB, since the vector is in B - const p_BC_E = rotate(R_EB, p_BC_B); + // Step 2 + // + // Now, we can find rEB y using that the closest frames cancel when + // multiplying two rotation matrices (i.e. N is cancelled here): + const rEB = multiply(rEN, rNB); - // Step4: Find the position of C, using the functions that goes from one - // position and a delta, to a new position: - const [n_EC_E, z_EC] = n_EA_E_and_p_AB_E2n_EB_E(n_EB_E, p_BC_E, z_EB, a, f); + // Step 3 + // + // The delta vector is now decomposed in E: + const bcE = transform(rEB, bcB); - // When displaying the resulting position for humans, it is more convenient - // to see lat, long: - const [lat_EC, long_EC] = n_E2lat_long(n_EC_E); + // Step 4 + // + // It is now easy to find the position of C using destination (with custom + // ellipsoid overriding the default WGS-84): + const [c, cDepth] = destination(b, bcE, bDepth, e); - // Here we also assume that the user wants the output to be height (= -depth): - const height = -z_EC; + // Use human-friendly outputs: + const [lat, lon] = toGeodeticCoordinates(c); + const height = -cDepth; - expect(deg(lat_EC)).toBeCloseTo(53.32637826433107, 13); - expect(deg(long_EC)).toBeCloseTo(63.46812343514746, 13); - expect(height).toBeCloseTo(406.0071960700098, 15); // meters + expect(degrees(lat)).toBeCloseTo(53.32637826433107, 13); + expect(degrees(lon)).toBeCloseTo(63.46812343514746, 13); + expect(height).toBeCloseTo(406.0071960700098, 15); }); diff --git a/test/vitest/examples/example-03.spec.ts b/test/vitest/examples/example-03.spec.ts index 9796000..fd2791d 100644 --- a/test/vitest/examples/example-03.spec.ts +++ b/test/vitest/examples/example-03.spec.ts @@ -1,11 +1,11 @@ -import { expect, test } from "vitest"; import { apply, - deg, - n_E2lat_long, - p_EB_E2n_EB_E, - type Vector3, -} from "../../../src/index.js"; + degrees, + fromECEF, + toGeodeticCoordinates, + type Vector, +} from "nvector-geodesy"; +import { expect, test } from "vitest"; /** * Example 3: ECEF-vector to geodetic latitude @@ -16,21 +16,29 @@ import { * @see https://www.ffi.no/en/research/n-vector/#example_3 */ test("Example 3", () => { - // Position B is given as p_EB_E ("ECEF-vector") - const p_EB_E: Vector3 = apply((n) => n * 6371e3, [0.71, -0.72, 0.1]); // m + // PROBLEM: + + // Position B is given as an “ECEF-vector” pb (i.e. a vector from E, the + // center of the Earth, to B, decomposed in E): + const pb: Vector = apply((n) => n * 6371e3, [0.71, -0.72, 0.1]); - // Find position B as geodetic latitude, longitude and height + // Find the geodetic latitude, longitude and height, assuming WGS-84 + // ellipsoid. // SOLUTION: - // Find n-vector from the p-vector: - const [n_EB_E, z_EB] = p_EB_E2n_EB_E(p_EB_E); + // Step 1 + // + // We have a function that converts ECEF-vectors to n-vectors: + const [b, bDepth] = fromECEF(pb); - // Convert to lat, long and height: - const [lat_EB, long_EB] = n_E2lat_long(n_EB_E); - const h_EB = -z_EB; + // Step 2 + // + // Find latitude, longitude and height: + const [lat, lon] = toGeodeticCoordinates(b); + const height = -bDepth; - expect(deg(lat_EB)).toBeCloseTo(5.685075734513181, 14); - expect(deg(long_EB)).toBeCloseTo(-45.40066325579215, 14); - expect(h_EB).toBeCloseTo(95772.10761821801, 15); // meters + expect(degrees(lat)).toBeCloseTo(5.685075734513181, 14); + expect(degrees(lon)).toBeCloseTo(-45.40066325579215, 14); + expect(height).toBeCloseTo(95772.10761821801, 15); }); diff --git a/test/vitest/examples/example-04.spec.ts b/test/vitest/examples/example-04.spec.ts index cc78054..c86a8cc 100644 --- a/test/vitest/examples/example-04.spec.ts +++ b/test/vitest/examples/example-04.spec.ts @@ -1,5 +1,5 @@ +import { fromGeodeticCoordinates, radians, toECEF } from "nvector-geodesy"; import { expect, test } from "vitest"; -import { lat_long2n_E, n_EB_E2p_EB_E, rad } from "../../../src/index.js"; /** * Example 4: Geodetic latitude to ECEF-vector @@ -10,22 +10,24 @@ import { lat_long2n_E, n_EB_E2p_EB_E, rad } from "../../../src/index.js"; * @see https://www.ffi.no/en/research/n-vector/#example_4 */ test("Example 4", () => { - // Position B is given with lat, long and height: - const lat_EB_deg = 1; - const long_EB_deg = 2; - const h_EB = 3; + // PROBLEM: - // Find the vector p_EB_E ("ECEF-vector") + // Geodetic latitude, longitude and height are given for position B: + const bLat = 1; + const bLon = 2; + const bHeight = 3; + + // Find the ECEF-vector for this position. // SOLUTION: - // Step1: Convert to n-vector: - const n_EB_E = lat_long2n_E(rad(lat_EB_deg), rad(long_EB_deg)); + // Step 1: First, the given latitude and longitude are converted to n-vector: + const b = fromGeodeticCoordinates(radians(bLat), radians(bLon)); - // Step2: Find the ECEF-vector p_EB_E: - const p_EB_E = n_EB_E2p_EB_E(n_EB_E, -h_EB); + // Step 2: Convert to an ECEF-vector: + const pb = toECEF(b, -bHeight); - expect(p_EB_E[0]).toBeCloseTo(6373290.277218279, 8); // meters - expect(p_EB_E[1]).toBeCloseTo(222560.2006747365, 8); // meters - expect(p_EB_E[2]).toBeCloseTo(110568.8271817859, 8); // meters + expect(pb[0]).toBeCloseTo(6373290.277218279, 8); + expect(pb[1]).toBeCloseTo(222560.2006747365, 8); + expect(pb[2]).toBeCloseTo(110568.8271817859, 8); }); diff --git a/test/vitest/examples/example-05.spec.ts b/test/vitest/examples/example-05.spec.ts index e054f0e..2d85615 100644 --- a/test/vitest/examples/example-05.spec.ts +++ b/test/vitest/examples/example-05.spec.ts @@ -1,14 +1,12 @@ -import { expect, test } from "vitest"; import { apply, cross, dot, - lat_long2n_E, + fromGeodeticCoordinates, norm, - rad, - unit, - type Vector3, -} from "../../../src/index.js"; + radians, +} from "nvector-geodesy"; +import { expect, test } from "vitest"; /** * Example 5: Surface distance @@ -18,40 +16,30 @@ import { * * @see https://www.ffi.no/en/research/n-vector/#example_5 */ -test.each` - label | n_EA_E | n_EB_E | s_AB_expected | d_AB_expected - ${"Enter elements directly:"} | ${unit([1, 0, -2])} | ${unit([-1, -2, 0])} | ${11290.39471136548} | ${9869.91075947498} - ${"or input as lat/long in deg:"} | ${lat_long2n_E(rad(88), rad(0))} | ${lat_long2n_E(rad(89), rad(-170))} | ${332.4564441053448} | ${332.4187248568097} -`( - "Example 5 ($label)", - // Position A and B are given as n_EA_E and n_EB_E: - ({ - n_EA_E, - n_EB_E, - s_AB_expected, - d_AB_expected, - }: { - n_EA_E: Vector3; - n_EB_E: Vector3; - s_AB_expected: number; - d_AB_expected: number; - }) => { - // m, mean Earth radius - const r_Earth = 6371e3; - - // SOLUTION: - - // The great circle distance is given by equation (16) in Gade (2010): - // Well conditioned for all angles: - const s_AB = - Math.atan2(norm(cross(n_EA_E, n_EB_E)), dot(n_EA_E, n_EB_E)) * r_Earth; - - // The Euclidean distance is given by: - const d_AB = - norm(apply((n_EB_E, n_EA_E) => n_EB_E - n_EA_E, n_EB_E, n_EA_E)) * - r_Earth; - - expect(s_AB / 1000).toBeCloseTo(s_AB_expected, 10); // kilometers - expect(d_AB / 1000).toBeCloseTo(d_AB_expected, 10); // kilometers - }, -); +test("Example 5", () => { + // PROBLEM: + + // Given two positions A and B as n-vectors: + const a = fromGeodeticCoordinates(radians(88), radians(0)); + const b = fromGeodeticCoordinates(radians(89), radians(-170)); + + // Find the surface distance (i.e. great circle distance). The heights of A + // and B are not relevant (i.e. if they do not have zero height, we seek the + // distance between the points that are at the surface of the Earth, directly + // above/below A and B). The Euclidean distance (chord length) should also be + // found. + + // Use Earth radius r: + const r = 6371e3; + + // SOLUTION: + + // Find the great circle distance: + const gcd = Math.atan2(norm(cross(a, b)), dot(a, b)) * r; + + // Find the Euclidean distance: + const ed = norm(apply((b, a) => b - a, b, a)) * r; + + expect(gcd).toBeCloseTo(332456.4441053448, 9); + expect(ed).toBeCloseTo(332418.7248568097, 9); +}); diff --git a/test/vitest/examples/example-06.spec.ts b/test/vitest/examples/example-06.spec.ts index 850704c..ed1f3d4 100644 --- a/test/vitest/examples/example-06.spec.ts +++ b/test/vitest/examples/example-06.spec.ts @@ -1,13 +1,12 @@ -import { expect, test } from "vitest"; import { apply, - deg, - lat_long2n_E, - n_E2lat_long, - rad, - unit, - type Vector3, -} from "../../../src/index.js"; + degrees, + fromGeodeticCoordinates, + normalize, + radians, + toGeodeticCoordinates, +} from "nvector-geodesy"; +import { expect, test } from "vitest"; /** * Example 6: Interpolated position @@ -17,48 +16,29 @@ import { * * @see https://www.ffi.no/en/research/n-vector/#example_6 */ -test.each` - label | n_EB_E_t0 | n_EB_E_t1 | lat_EB_ti_expected | long_EB_ti_expected - ${"Enter elements directly:"} | ${unit([1, 0, -2])} | ${unit([-1, -2, 0])} | ${-33.3287577974902} | ${-99.46232220802563} - ${"or input as lat/long in deg:"} | ${lat_long2n_E(rad(89.9), rad(-150))} | ${lat_long2n_E(rad(89.9), rad(150))} | ${89.91282199988446} | ${173.4132244463705} -`( - "Example 6 ($label)", - // Position B is given at time t0 as n_EB_E_t0 and at time t1 as n_EB_E_t1: - ({ - n_EB_E_t0, - n_EB_E_t1, - lat_EB_ti_expected, - long_EB_ti_expected, - }: { - n_EB_E_t0: Vector3; - n_EB_E_t1: Vector3; - lat_EB_ti_expected: number; - long_EB_ti_expected: number; - }) => { - // The times are given as: - const t0 = 10; - const t1 = 20; - const ti = 16; // time of interpolation +test("Example 6", () => { + // PROBLEM: + + // Given the position of B at time t0 and t1, pt0 and pt1: + const t0 = 10; + const t1 = 20; + const ti = 16; + const pt0 = fromGeodeticCoordinates(radians(89.9), radians(-150)); + const pt1 = fromGeodeticCoordinates(radians(89.9), radians(150)); - // Find the interpolated position at time ti, n_EB_E_ti + // Find an interpolated position at time ti, pti. All positions are given as + // n-vectors. - // SOLUTION: + // SOLUTION: - // Using standard interpolation: - const n_EB_E_ti = unit( - apply( - (n_EB_E_t0, n_EB_E_t1) => - n_EB_E_t0 + ((ti - t0) * (n_EB_E_t1 - n_EB_E_t0)) / (t1 - t0), - n_EB_E_t0, - n_EB_E_t1, - ), - ); + // Standard interpolation can be used directly with n-vector: + const pti = normalize( + apply((pt0, pt1) => pt0 + ((ti - t0) * (pt1 - pt0)) / (t1 - t0), pt0, pt1), + ); - // When displaying the resulting position for humans, it is more convenient - // to see lat, long: - const [lat_EB_ti, long_EB_ti] = n_E2lat_long(n_EB_E_ti); + // Use human-friendly outputs: + const [lat, lon] = toGeodeticCoordinates(pti); - expect(deg(lat_EB_ti)).toBeCloseTo(lat_EB_ti_expected, 12); - expect(deg(long_EB_ti)).toBeCloseTo(long_EB_ti_expected, 12); - }, -); + expect(degrees(lat)).toBeCloseTo(89.91282199988446, 12); + expect(degrees(lon)).toBeCloseTo(173.4132244463705, 12); +}); diff --git a/test/vitest/examples/example-07.spec.ts b/test/vitest/examples/example-07.spec.ts index fb2174e..3282d20 100644 --- a/test/vitest/examples/example-07.spec.ts +++ b/test/vitest/examples/example-07.spec.ts @@ -1,11 +1,10 @@ -import { expect, test } from "vitest"; import { apply, - lat_long2n_E, - rad, - unit, - type Vector3, -} from "../../../src/index.js"; + fromGeodeticCoordinates, + normalize, + radians, +} from "nvector-geodesy"; +import { expect, test } from "vitest"; /** * Example 7: Mean position/center @@ -14,38 +13,23 @@ import { * * @see https://www.ffi.no/en/research/n-vector/#example_7 */ -test.each` - label | n_EA_E | n_EB_E | n_EC_E | n_EM_E_expected - ${"Enter elements directly:"} | ${unit([1, 0, -2])} | ${unit([-1, -2, 0])} | ${unit([0, -2, 3])} | ${[0, -0.9990748728803714, -0.04300463206527586]} - ${"or input as lat/long in deg:"} | ${lat_long2n_E(rad(90), rad(0))} | ${lat_long2n_E(rad(60), rad(10))} | ${lat_long2n_E(rad(50), rad(-20))} | ${[0.3841171702926, -0.04660240548568945, 0.9221074857571395]} -`( - "Example 7 ($label)", - // Three positions A, B and C are given: - ({ - n_EA_E, - n_EB_E, - n_EC_E, - n_EM_E_expected, - }: { - n_EA_E: Vector3; - n_EB_E: Vector3; - n_EC_E: Vector3; - n_EM_E_expected: Vector3; - }) => { - // SOLUTION: +test("Example 7", () => { + // PROBLEM: + + // Three positions A, B, and C are given as n-vectors: + const a = fromGeodeticCoordinates(radians(90), radians(0)); + const b = fromGeodeticCoordinates(radians(60), radians(10)); + const c = fromGeodeticCoordinates(radians(50), radians(-20)); + + // Find the mean position, M. Note that the calculation is independent of the + // heights/depths of the positions. + + // SOLUTION: - // Find the horizontal mean position, M: - const n_EM_E = unit( - apply( - (n_EA_E, n_EB_E, n_EC_E) => n_EA_E + n_EB_E + n_EC_E, - n_EA_E, - n_EB_E, - n_EC_E, - ), - ); + // The mean position is simply given by the mean n-vector: + const m = normalize(apply((a, b, c) => a + b + c, a, b, c)); - expect(n_EM_E[0]).toBeCloseTo(n_EM_E_expected[0], 16); - expect(n_EM_E[1]).toBeCloseTo(n_EM_E_expected[1], 16); - expect(n_EM_E[2]).toBeCloseTo(n_EM_E_expected[2], 16); - }, -); + expect(m[0]).toBeCloseTo(0.3841171702926, 16); + expect(m[1]).toBeCloseTo(-0.04660240548568945, 16); + expect(m[2]).toBeCloseTo(0.9221074857571395, 16); +}); diff --git a/test/vitest/examples/example-08.spec.ts b/test/vitest/examples/example-08.spec.ts index 3c92fd2..9d38661 100644 --- a/test/vitest/examples/example-08.spec.ts +++ b/test/vitest/examples/example-08.spec.ts @@ -1,17 +1,16 @@ -import { expect, test } from "vitest"; import { - R_Ee_NP_Z, + Z_AXIS_NORTH, apply, cross, - deg, - lat_long2n_E, - n_E2lat_long, - rad, - rotate, + degrees, + fromGeodeticCoordinates, + normalize, + radians, + toGeodeticCoordinates, + transform, transpose, - unit, - type Vector3, -} from "../../../src/index.js"; +} from "nvector-geodesy"; +import { expect, test } from "vitest"; /** * Example 8: A and azimuth/distance to B @@ -21,61 +20,65 @@ import { * * @see https://www.ffi.no/en/research/n-vector/#example_8 */ -test.each` - label | n_EA_E | lat_EB_expected | long_EB_expected - ${"Enter elements directly:"} | ${unit([1, 0, -2])} | ${-63.44339951651296} | ${-0.006879863905895194} - ${"or input as lat/long in deg:"} | ${lat_long2n_E(rad(80), rad(-90))} | ${79.99154867339445} | ${-90.01769837291397} -`( - "Example 8 ($label)", - // Position A is given as n_EA_E: - ({ - n_EA_E, - lat_EB_expected, - long_EB_expected, - }: { - n_EA_E: Vector3; - lat_EB_expected: number; - long_EB_expected: number; - }) => { - // The initial azimuth and great circle distance (s_AB), and Earth radius - // (r_Earth) are also given: - const azimuth = rad(200); - const s_AB = 1000; // m - const r_Earth = 6371e3; // m, mean Earth radius +test("Example 8", () => { + // PROBLEM: + + // Position A is given as n-vector: + const a = fromGeodeticCoordinates(radians(80), radians(-90)); + + // We also have an initial direction of travel given as an azimuth (bearing) + // relative to north (clockwise), and finally the distance to travel along a + // great circle is given: + const azimuth = radians(200); + const gcd = 1000; + + // Use Earth radius r: + const r = 6371e3; - // Find the destination point B, as n_EB_E ("The direct/first geodetic - // problem" for a sphere) + // Find the destination point B. + // + // In geodesy, this is known as "The first geodetic problem" or "The direct + // geodetic problem" for a sphere, and we see that this is similar to Example + // 2, but now the delta is given as an azimuth and a great circle distance. + // "The second/inverse geodetic problem" for a sphere is already solved in + // Examples 1 and 5. - // SOLUTION: + // SOLUTION: - // Step1: Find unit vectors for north and east (see equations (9) and (10) - // in Gade (2010): - const k_east_E = unit( - cross(rotate(transpose(R_Ee_NP_Z), [1, 0, 0]), n_EA_E), - ); - const k_north_E = cross(n_EA_E, k_east_E); + // The azimuth (relative to north) is a singular quantity (undefined at the + // Poles), but from this angle we can find a (non-singular) quantity that is + // more convenient when working with vector algebra: a vector d that points in + // the initial direction. We find this from azimuth by first finding the north + // and east vectors at the start point, with unit lengths. + // + // Here we have assumed that our coordinate frame E has its z-axis along the + // rotational axis of the Earth, pointing towards the North Pole. Hence, this + // axis is given by [1, 0, 0]: + const e = normalize(cross(transform(transpose(Z_AXIS_NORTH), [1, 0, 0]), a)); + const n = cross(a, e); - // Step2: Find the initial direction vector d_E: - const d_E = apply( - (k_north_E, k_east_E) => - k_north_E * Math.cos(azimuth) + k_east_E * Math.sin(azimuth), - k_north_E, - k_east_E, - ); + // The two vectors n and e are horizontal, orthogonal, and span the tangent + // plane at the initial position. A unit vector d in the direction of the + // azimuth is now given by: + const d = apply( + (n, e) => n * Math.cos(azimuth) + e * Math.sin(azimuth), + n, + e, + ); - // Step3: Find n_EB_E: - const n_EB_E = apply( - (n_EA_E, d_E) => - n_EA_E * Math.cos(s_AB / r_Earth) + d_E * Math.sin(s_AB / r_Earth), - n_EA_E, - d_E, - ); + // With the initial direction given as d instead of azimuth, it is now quite + // simple to find b. We know that d and a are orthogonal, and they will span + // the plane where b will lie. Thus, we can use sin and cos in the same manner + // as above, with the angle traveled given by gcd / r: + const b = apply( + (a, d) => a * Math.cos(gcd / r) + d * Math.sin(gcd / r), + a, + d, + ); - // When displaying the resulting position for humans, it is more convenient - // to see lat, long: - const [lat_EB, long_EB] = n_E2lat_long(n_EB_E); + // Use human-friendly outputs: + const [lat, lon] = toGeodeticCoordinates(b); - expect(deg(lat_EB)).toBeCloseTo(lat_EB_expected, 13); - expect(deg(long_EB)).toBeCloseTo(long_EB_expected, 13); - }, -); + expect(degrees(lat)).toBeCloseTo(79.99154867339445, 13); + expect(degrees(lon)).toBeCloseTo(-90.01769837291397, 13); +}); diff --git a/test/vitest/examples/example-09.spec.ts b/test/vitest/examples/example-09.spec.ts index e01f10f..f1467cf 100644 --- a/test/vitest/examples/example-09.spec.ts +++ b/test/vitest/examples/example-09.spec.ts @@ -1,15 +1,14 @@ -import { expect, test } from "vitest"; import { apply, cross, - deg, + degrees, dot, - lat_long2n_E, - n_E2lat_long, - rad, - unit, - type Vector3, -} from "../../../src/index.js"; + fromGeodeticCoordinates, + normalize, + radians, + toGeodeticCoordinates, +} from "nvector-geodesy"; +import { expect, test } from "vitest"; /** * Example 9: Intersection of two paths @@ -19,48 +18,43 @@ import { * * @see https://www.ffi.no/en/research/n-vector/#example_9 */ -test.each` - label | n_EA1_E | n_EA2_E | n_EB1_E | n_EB2_E | lat_EC_expected | long_EC_expected - ${"Enter elements directly:"} | ${unit([0, 0, 1])} | ${unit([-1, 0, 1])} | ${unit([-2, -2, 4])} | ${unit([-2, 2, 2])} | ${56.30993247402022} | ${180} - ${"or input as lat/long in deg:"} | ${lat_long2n_E(rad(50), rad(180))} | ${lat_long2n_E(rad(90), rad(180))} | ${lat_long2n_E(rad(60), rad(160))} | ${lat_long2n_E(rad(80), rad(-140))} | ${74.16344802135536} | ${180} -`( - "Example 9 ($label)", - // Two paths A and B are given by two pairs of positions: - ({ - n_EA1_E, - n_EA2_E, - n_EB1_E, - n_EB2_E, - lat_EC_expected, - long_EC_expected, - }: { - n_EA1_E: Vector3; - n_EA2_E: Vector3; - n_EB1_E: Vector3; - n_EB2_E: Vector3; - lat_EC_expected: number; - long_EC_expected: number; - }) => { - // SOLUTION: +test("Example 9", () => { + // PROBLEM: + + // Define a path from two given positions (at the surface of a spherical + // Earth), as the great circle that goes through the two points (assuming that + // the two positions are not antipodal). + + // Path A is given by a1 and a2: + const a1 = fromGeodeticCoordinates(radians(50), radians(180)); + const a2 = fromGeodeticCoordinates(radians(90), radians(180)); + + // While path B is given by b1 and b2: + const b1 = fromGeodeticCoordinates(radians(60), radians(160)); + const b2 = fromGeodeticCoordinates(radians(80), radians(-140)); + + // Find the position C where the two paths intersect. + + // SOLUTION: - // Find the intersection between the two paths, n_EC_E: - const n_EC_E_tmp = unit( - cross(cross(n_EA1_E, n_EA2_E), cross(n_EB1_E, n_EB2_E)), - ); + // A convenient way to represent a great circle is by its normal vector (i.e. + // the normal vector to the plane containing the great circle). This normal + // vector is simply found by taking the cross product of the two n-vectors + // defining the great circle (path). Having the normal vectors to both paths, + // the intersection is now simply found by taking the cross product of the two + // normal vectors: + const cTmp = normalize(cross(cross(a1, a2), cross(b1, b2))); - // n_EC_E_tmp is one of two solutions, the other is -n_EC_E_tmp. Select the - // one that is closest to n_EA1_E, by selecting sign from the dot product - // between n_EC_E_tmp and n_EA1_E: - const n_EC_E = apply( - (n) => Math.sign(dot(n_EC_E_tmp, n_EA1_E)) * n, - n_EC_E_tmp, - ); + // Note that there will be two places where the great circles intersect, and + // thus two solutions are found. Selecting the solution that is closest to + // e.g. a1 can be achieved by selecting the solution that has a positive dot + // product with a1 (or the mean position from Example 7 could be used instead + // of a1): + const c = apply((n) => Math.sign(dot(cTmp, a1)) * n, cTmp); - // When displaying the resulting position for humans, it is more convenient - // to see lat, long: - const [lat_EC, long_EC] = n_E2lat_long(n_EC_E); + // Use human-friendly outputs: + const [lat, lon] = toGeodeticCoordinates(c); - expect(deg(lat_EC)).toBeCloseTo(lat_EC_expected, 16); - expect(deg(long_EC)).toBeCloseTo(long_EC_expected, 16); - }, -); + expect(degrees(lat)).toBeCloseTo(74.16344802135536, 16); + expect(degrees(lon)).toBeCloseTo(180, 16); +}); diff --git a/test/vitest/examples/example-10.spec.ts b/test/vitest/examples/example-10.spec.ts index 08413f9..2627fc6 100644 --- a/test/vitest/examples/example-10.spec.ts +++ b/test/vitest/examples/example-10.spec.ts @@ -1,12 +1,11 @@ -import { expect, test } from "vitest"; import { cross, dot, - lat_long2n_E, - rad, - unit, - type Vector3, -} from "../../../src/index.js"; + fromGeodeticCoordinates, + normalize, + radians, +} from "nvector-geodesy"; +import { expect, test } from "vitest"; /** * Example 10: Cross track distance (cross track error) @@ -16,42 +15,41 @@ import { * * @see https://www.ffi.no/en/research/n-vector/#example_10 */ -test.each` - label | n_EA1_E | n_EA2_E | n_EB_E | s_xt_expected | d_xt_expected - ${"Enter elements directly:"} | ${unit([1, 0, -2])} | ${unit([-1, -2, 0])} | ${unit([0, -2, 3])} | ${3834155.561819959} | ${3606868.49226761} - ${"or input as lat/long in deg:"} | ${lat_long2n_E(rad(0), rad(0))} | ${lat_long2n_E(rad(10), rad(0))} | ${lat_long2n_E(rad(1), rad(0.1))} | ${11117.79911014538} | ${11117.79346740667} -`( - "Example 10 ($label)", - // Position A1 and A2 and B are given as n_EA1_E, n_EA2_E, and n_EB_E: - ({ - n_EA1_E, - n_EA2_E, - n_EB_E, - s_xt_expected, - d_xt_expected, - }: { - n_EA1_E: Vector3; - n_EA2_E: Vector3; - n_EB_E: Vector3; - s_xt_expected: number; - d_xt_expected: number; - }) => { - const r_Earth = 6371e3; // m, mean Earth radius - - // Find the cross track distance from path A to position B. - - // SOLUTION: - - // Find the unit normal to the great circle between n_EA1_E and n_EA2_E: - const c_E = unit(cross(n_EA1_E, n_EA2_E)); - - // Find the great circle cross track distance: (acos(x) - pi/2 = -asin(x)) - const s_xt = -Math.asin(dot(c_E, n_EB_E)) * r_Earth; - - // Find the Euclidean cross track distance: - const d_xt = -dot(c_E, n_EB_E) * r_Earth; - - expect(s_xt).toBeCloseTo(s_xt_expected, 9); // meters - expect(d_xt).toBeCloseTo(d_xt_expected, 9); // meters - }, -); +test("Example 10", () => { + // PROBLEM: + + // Path A is given by the two n-vectors a1 and a2 (as in the previous + // example): + const a1 = fromGeodeticCoordinates(radians(0), radians(0)); + const a2 = fromGeodeticCoordinates(radians(10), radians(0)); + + // And a position B is given by b: + const b = fromGeodeticCoordinates(radians(1), radians(0.1)); + + // Find the cross track distance between the path A (i.e. the great circle + // through a1 and a2) and the position B (i.e. the shortest distance at the + // surface, between the great circle and B). Also, find the Euclidean distance + // between B and the plane defined by the great circle. + + // Use Earth radius r: + const r = 6371e3; + + // SOLUTION: + + // First, find the normal to the great circle, with direction given by the + // right hand rule and the direction of travel: + const c = normalize(cross(a1, a2)); + + // Find the great circle cross track distance: + const gcd = -Math.asin(dot(c, b)) * r; + + // Finding the Euclidean distance is even simpler, since it is the projection + // of b onto c, thus simply the dot product: + const ed = -dot(c, b) * r; + + // For both gcd and ed, positive answers means that B is to the right of the + // track. + + expect(gcd).toBeCloseTo(11117.79911014538, 9); + expect(ed).toBeCloseTo(11117.79346740667, 9); +}); diff --git a/test/vitest/lat_long2n_E.spec.ts b/test/vitest/lat_long2n_E.spec.ts deleted file mode 100644 index d2bf151..0000000 --- a/test/vitest/lat_long2n_E.spec.ts +++ /dev/null @@ -1,55 +0,0 @@ -import { fc, it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { lat_long2n_E } from "../../src/index.js"; -import { arbitrary3dRotationMatrix, arbitraryLatLon } from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { createNvectorTestClient } from "../nvector-test-api.js"; - -const TEST_DURATION = 5000; - -describe("lat_long2n_E()", () => { - let nvectorTestClient: NvectorTestClient; - - beforeAll(async () => { - nvectorTestClient = await createNvectorTestClient(); - }); - - afterAll(() => { - nvectorTestClient?.close(); - }); - - it.prop( - [ - arbitraryLatLon(), - fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), - ], - { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, - )( - "matches the reference implementation", - async ([latitude, longitude], R_Ee) => { - const expected = await nvectorTestClient.lat_long2n_E( - latitude, - longitude, - R_Ee, - ); - - expect(expected).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - - const actual = lat_long2n_E(latitude, longitude, R_Ee); - - expect(actual).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - expect(actual[0]).toBeCloseTo(expected[0], 15); - expect(actual[1]).toBeCloseTo(expected[1], 15); - expect(actual[2]).toBeCloseTo(expected[2], 15); - }, - TEST_DURATION + 1000, - ); -}); diff --git a/test/vitest/n_E2R_EN.spec.ts b/test/vitest/n_E2R_EN.spec.ts deleted file mode 100644 index 9208ddb..0000000 --- a/test/vitest/n_E2R_EN.spec.ts +++ /dev/null @@ -1,85 +0,0 @@ -import { fc, it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { n_E2R_EN } from "../../src/index.js"; -import { R_Ee_NP_Z, rotate } from "../../src/rotation.js"; -import { - arbitrary3dRotationMatrix, - arbitrary3dUnitVector, -} from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { 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 = R_Ee_NP_Z]) => { - // 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 reference 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, - ); - - it("handles the poles", () => { - expect(n_E2R_EN([0, 0, 1])).toMatchObject([ - [-1, 0, 0], - [0, 1, -0], - [0, 0, -1], - ]); - }); -}); diff --git a/test/vitest/n_E2lat_long.spec.ts b/test/vitest/n_E2lat_long.spec.ts deleted file mode 100644 index 64989c3..0000000 --- a/test/vitest/n_E2lat_long.spec.ts +++ /dev/null @@ -1,45 +0,0 @@ -import { fc, it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { n_E2lat_long } from "../../src/index.js"; -import { - arbitrary3dRotationMatrix, - arbitrary3dUnitVector, -} from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { createNvectorTestClient } from "../nvector-test-api.js"; - -const TEST_DURATION = 5000; - -describe("n_E2lat_long()", () => { - let nvectorTestClient: NvectorTestClient; - - beforeAll(async () => { - nvectorTestClient = await createNvectorTestClient(); - }); - - afterAll(() => { - nvectorTestClient?.close(); - }); - - it.prop( - [ - arbitrary3dUnitVector(), - fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), - ], - { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, - )( - "matches the reference implementation", - async (n_E, R_Ee) => { - const expected = await nvectorTestClient.n_E2lat_long(n_E, R_Ee); - - expect(expected).toMatchObject([expect.any(Number), expect.any(Number)]); - - const actual = n_E2lat_long(n_E, R_Ee); - - expect(actual).toMatchObject([expect.any(Number), expect.any(Number)]); - expect(actual[0]).toBeCloseTo(expected[0], 15); - expect(actual[1]).toBeCloseTo(expected[1], 15); - }, - TEST_DURATION + 1000, - ); -}); diff --git a/test/vitest/n_EA_E_and_n_EB_E2p_AB_E.spec.ts b/test/vitest/n_EA_E_and_n_EB_E2p_AB_E.spec.ts deleted file mode 100644 index 5a4751c..0000000 --- a/test/vitest/n_EA_E_and_n_EB_E2p_AB_E.spec.ts +++ /dev/null @@ -1,85 +0,0 @@ -import { fc, it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { n_EA_E_and_n_EB_E2p_AB_E } from "../../src/index.js"; -import { - arbitrary3dRotationMatrix, - arbitrary3dUnitVector, - arbitraryEllipsoid, - arbitraryEllipsoidDepth, -} from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { createNvectorTestClient } from "../nvector-test-api.js"; - -const TEST_DURATION = 5000; - -describe("n_EA_E_and_n_EB_E2p_AB_E()", () => { - let nvectorTestClient: NvectorTestClient; - - beforeAll(async () => { - nvectorTestClient = await createNvectorTestClient(); - }); - - afterAll(() => { - nvectorTestClient?.close(); - }); - - it.prop( - [ - arbitrary3dUnitVector(), - arbitrary3dUnitVector(), - arbitraryEllipsoid().chain((ellipsoid) => { - return fc.tuple( - fc.option(arbitraryEllipsoidDepth(ellipsoid), { - nil: undefined, - }), - 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 }), - ], - { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, - )( - "matches the reference implementation", - async (n_EA_E, n_EB_E, [z_EA, z_EB, a, f], R_Ee) => { - const expected = await nvectorTestClient.n_EA_E_and_n_EB_E2p_AB_E( - n_EA_E, - n_EB_E, - z_EA, - z_EB, - a, - f, - R_Ee, - ); - - expect(expected).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - - const actual = n_EA_E_and_n_EB_E2p_AB_E( - n_EA_E, - n_EB_E, - z_EA, - z_EB, - a, - f, - R_Ee, - ); - - expect(actual).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - expect(actual[0]).toBeCloseTo(expected[0], 7); - expect(actual[1]).toBeCloseTo(expected[1], 7); - expect(actual[2]).toBeCloseTo(expected[2], 7); - }, - TEST_DURATION + 1000, - ); -}); 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 deleted file mode 100644 index f472d7a..0000000 --- a/test/vitest/n_EA_E_and_p_AB_E2n_EB_E.spec.ts +++ /dev/null @@ -1,136 +0,0 @@ -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, - type Vector3, -} from "../../src/index.js"; -import { R_Ee_NP_Z, rotate } from "../../src/rotation.js"; -import { - arbitrary3dRotationMatrix, - arbitrary3dUnitVector, - arbitraryEllipsoid, - arbitraryEllipsoidDepth, - arbitraryEllipsoidECEFVector, -} from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { 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 = R_Ee_NP_Z, - ]) => { - 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 reference 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], 12); - expect(actualVector[1]).toBeCloseTo(expectedVector[1], 12); - expect(actualVector[2]).toBeCloseTo(expectedVector[2], 12); - 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 deleted file mode 100644 index 7e5b648..0000000 --- a/test/vitest/n_EB_E2p_EB_E.spec.ts +++ /dev/null @@ -1,69 +0,0 @@ -import { fc, it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { n_EB_E2p_EB_E } from "../../src/index.js"; -import { - arbitrary3dRotationMatrix, - arbitrary3dUnitVector, - arbitraryEllipsoid, - arbitraryEllipsoidDepth, -} from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { 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(), - arbitraryEllipsoid().chain((ellipsoid) => { - return fc.tuple( - 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 }), - ], - { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, - )( - "matches the reference 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], 7); - expect(actual[1]).toBeCloseTo(expected[1], 7); - expect(actual[2]).toBeCloseTo(expected[2], 7); - }, - TEST_DURATION + 1000, - ); -}); diff --git a/test/vitest/n_E_and_wa2R_EL.spec.ts b/test/vitest/n_E_and_wa2R_EL.spec.ts deleted file mode 100644 index 83b2327..0000000 --- a/test/vitest/n_E_and_wa2R_EL.spec.ts +++ /dev/null @@ -1,92 +0,0 @@ -import { fc, it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { - R_Ee_NP_Z, - n_E2lat_long, - n_E_and_wa2R_EL, - xyz2R, -} from "../../src/index.js"; -import { - arbitrary3dRotationMatrix, - arbitrary3dUnitVector, - arbitraryRadians, -} from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { createNvectorTestClient } from "../nvector-test-api.js"; - -const TEST_DURATION = 5000; - -describe("n_E_and_wa2R_EL()", () => { - let nvectorTestClient: NvectorTestClient; - - beforeAll(async () => { - nvectorTestClient = await createNvectorTestClient(); - }); - - afterAll(() => { - nvectorTestClient?.close(); - }); - - it.prop( - [ - fc - .tuple( - arbitrary3dUnitVector(), - arbitraryRadians(), - fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), - ) - .filter(([n_E, wander_azimuth, R_Ee = R_Ee_NP_Z]) => { - // Avoid situations where components of the xyz2R matrix are close - // to zero. The Python implementation rounds to zero in these cases, - // which produces very different results. - const [latitude, longitude] = n_E2lat_long(n_E, R_Ee); - const R_AB = xyz2R(longitude, -latitude, wander_azimuth); - if ( - R_AB.some((row) => - row.some( - (value) => value !== 0 && value < 1e-15 && value > -1e-15, - ), - ) - ) { - return false; - } - - return true; - }), - ], - { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, - )( - "matches the reference implementation", - async ([n_E, wander_azimuth, R_Ee]) => { - const expected = await nvectorTestClient.n_E_and_wa2R_EL( - n_E, - wander_azimuth, - 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_E_and_wa2R_EL(n_E, wander_azimuth, 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, - ); -}); diff --git a/test/vitest/p_EB_E2n_EB_E.spec.ts b/test/vitest/p_EB_E2n_EB_E.spec.ts deleted file mode 100644 index c4c4cee..0000000 --- a/test/vitest/p_EB_E2n_EB_E.spec.ts +++ /dev/null @@ -1,92 +0,0 @@ -import { fc, it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { WGS_84 } from "../../src/ellipsoid.js"; -import { p_EB_E2n_EB_E } from "../../src/index.js"; -import { R_Ee_NP_Z, rotate } from "../../src/rotation.js"; -import { - arbitrary3dRotationMatrix, - arbitraryEllipsoid, - arbitraryEllipsoidECEFVector, -} from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { createNvectorTestClient } from "../nvector-test-api.js"; - -const TEST_DURATION = 5000; - -describe("p_EB_E2n_EB_E()", () => { - let nvectorTestClient: NvectorTestClient; - - beforeAll(async () => { - nvectorTestClient = await createNvectorTestClient(); - }); - - afterAll(() => { - nvectorTestClient?.close(); - }); - - it.prop( - [ - arbitraryEllipsoid() - .chain((ellipsoid) => { - return fc.tuple( - arbitraryEllipsoidECEFVector(ellipsoid), - fc.option(fc.constant(ellipsoid.a), { nil: undefined }), - fc.option(fc.constant(ellipsoid.f), { nil: undefined }), - fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), - ); - }) - .filter(([p_EB_E, a = WGS_84.a, f = WGS_84.f, R_Ee = R_Ee_NP_Z]) => { - 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 reference implementation", - async ([p_EB_E, a, f, R_Ee]) => { - const [expectedVector, expectedDepth] = - await nvectorTestClient.p_EB_E2n_EB_E(p_EB_E, 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] = p_EB_E2n_EB_E(p_EB_E, a, f, R_Ee); - - expect(actualVector).toMatchObject([ - expect.any(Number), - expect.any(Number), - expect.any(Number), - ]); - 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, - ); -}); diff --git a/test/vitest/rotation-matrix.spec.ts b/test/vitest/rotation-matrix.spec.ts new file mode 100644 index 0000000..2100c85 --- /dev/null +++ b/test/vitest/rotation-matrix.spec.ts @@ -0,0 +1,220 @@ +import { fc, it } from "@fast-check/vitest"; +import { + Z_AXIS_NORTH, + eulerXYZToRotationMatrix, + fromRotationMatrix, + toGeodeticCoordinates, + toRotationMatrix, + toRotationMatrixUsingWanderAzimuth, + transform, +} from "nvector-geodesy"; +import { afterAll, beforeAll, describe, expect } from "vitest"; +import { + arbitrary3dRotationMatrix, + arbitrary3dUnitVector, + arbitraryRadians, +} from "../arbitrary.js"; +import type { NvectorTestClient } from "../nvector-test-api.js"; +import { createNvectorTestClient } from "../nvector-test-api.js"; + +const TEST_DURATION = 5000; + +describe("fromRotationMatrix()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop([arbitrary3dRotationMatrix()], { + interruptAfterTimeLimit: TEST_DURATION, + numRuns: Infinity, + })( + "matches the reference implementation", + async (rotation) => { + const expected = await nvectorTestClient.fromRotationMatrix(rotation); + + expect(expected).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + + const actual = fromRotationMatrix(rotation); + + expect(actual).toMatchObject([ + expect.any(Number), + expect.any(Number), + expect.any(Number), + ]); + expect(actual[0]).toBeCloseTo(expected[0], 15); + expect(actual[1]).toBeCloseTo(expected[1], 15); + expect(actual[2]).toBeCloseTo(expected[2], 15); + }, + TEST_DURATION + 1000, + ); +}); + +describe("toRotationMatrix()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop( + [ + fc + .tuple( + arbitrary3dUnitVector(), + fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), + ) + .filter(([vector, frame = Z_AXIS_NORTH]) => { + // 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 [, y, z] = transform(frame, vector); + const yDirNorm = Math.hypot(-z, y); + if (yDirNorm > 0 && yDirNorm <= 1e-100) { + return false; + } + + return true; + }), + ], + { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, + )( + "matches the reference implementation", + async ([vector, rotation]) => { + const expected = await nvectorTestClient.toRotationMatrix( + vector, + rotation, + ); + + 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 = toRotationMatrix(vector, rotation); + + 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, + ); + + it("handles the poles", () => { + expect(toRotationMatrix([0, 0, 1])).toMatchObject([ + [-1, 0, 0], + [0, 1, -0], + [0, 0, -1], + ]); + }); +}); + +describe("toRotationMatrixUsingWanderAzimuth()", () => { + let nvectorTestClient: NvectorTestClient; + + beforeAll(async () => { + nvectorTestClient = await createNvectorTestClient(); + }); + + afterAll(() => { + nvectorTestClient?.close(); + }); + + it.prop( + [ + fc + .tuple( + arbitrary3dUnitVector(), + arbitraryRadians(), + fc.option(arbitrary3dRotationMatrix(), { nil: undefined }), + ) + .filter(([vector, wanderAzimuth, frame = Z_AXIS_NORTH]) => { + // Avoid situations where components of the xyz2R matrix are close + // to zero. The Python implementation rounds to zero in these cases, + // which produces very different results. + const [latitude, longitude] = toGeodeticCoordinates(vector, frame); + const rotation = eulerXYZToRotationMatrix( + longitude, + -latitude, + wanderAzimuth, + ); + if ( + rotation.some((row) => + row.some( + (value) => value !== 0 && value < 1e-15 && value > -1e-15, + ), + ) + ) { + return false; + } + + return true; + }), + ], + { interruptAfterTimeLimit: TEST_DURATION, numRuns: Infinity }, + )( + "matches the reference implementation", + async ([vector, wanderAzimuth, frame]) => { + const expected = + await nvectorTestClient.toRotationMatrixUsingWanderAzimuth( + vector, + wanderAzimuth, + frame, + ); + + 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 = toRotationMatrixUsingWanderAzimuth( + vector, + wanderAzimuth, + frame, + ); + + 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, + ); +}); diff --git a/test/vitest/unit.spec.ts b/test/vitest/unit.spec.ts deleted file mode 100644 index 59ca892..0000000 --- a/test/vitest/unit.spec.ts +++ /dev/null @@ -1,9 +0,0 @@ -import { it } from "@fast-check/vitest"; -import { describe, expect } from "vitest"; -import { unit } from "../../src/index.js"; - -describe("unit()", () => { - it("returns a chosen vector when norm is 0", () => { - expect(unit([0, 0, 0])).toEqual([1, 0, 0]); - }); -}); diff --git a/test/vitest/apply.spec.ts b/test/vitest/vector.spec.ts similarity index 67% rename from test/vitest/apply.spec.ts rename to test/vitest/vector.spec.ts index 09cc878..f171201 100644 --- a/test/vitest/apply.spec.ts +++ b/test/vitest/vector.spec.ts @@ -1,5 +1,6 @@ -import { expect, test } from "vitest"; -import { apply, type Vector3 } from "../../src/index.js"; +import { it } from "@fast-check/vitest"; +import { apply, normalize, type Vector } from "nvector-geodesy"; +import { describe, expect, test } from "vitest"; test.each` label | fn | v | expected @@ -8,7 +9,13 @@ test.each` ${"Interpolate between two vectors"} | ${(a: number, b: number) => a + (b - a) / 2} | ${[[1, 2, 3], [4, 5, 6]]} | ${[2.5, 3.5, 4.5]} `( "apply() $label", - ({ fn, v, expected }: { fn: () => number; v: []; expected: Vector3 }) => { + ({ fn, v, expected }: { fn: () => number; v: []; expected: Vector }) => { expect(apply(fn, ...v)).toEqual(expected); }, ); + +describe("normalize()", () => { + it("returns a chosen vector when norm is 0", () => { + expect(normalize([0, 0, 0])).toEqual([1, 0, 0]); + }); +}); diff --git a/test/vitest/xyz2R.spec.ts b/test/vitest/xyz2R.spec.ts deleted file mode 100644 index 253476c..0000000 --- a/test/vitest/xyz2R.spec.ts +++ /dev/null @@ -1,54 +0,0 @@ -import { it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { xyz2R } from "../../src/index.js"; -import { arbitraryRadians } from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { createNvectorTestClient } from "../nvector-test-api.js"; - -const TEST_DURATION = 5000; - -describe("xyz2R()", () => { - let nvectorTestClient: NvectorTestClient; - - beforeAll(async () => { - nvectorTestClient = await createNvectorTestClient(); - }); - - afterAll(() => { - nvectorTestClient?.close(); - }); - - it.prop([arbitraryRadians(), arbitraryRadians(), arbitraryRadians()], { - interruptAfterTimeLimit: TEST_DURATION, - numRuns: Infinity, - })( - "matches the reference implementation", - async (x, y, z) => { - const expected = await nvectorTestClient.xyz2R(x, y, z); - - 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 = xyz2R(x, y, z); - - 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], 15); - expect(actual[0][1]).toBeCloseTo(expected[0][1], 15); - expect(actual[0][2]).toBeCloseTo(expected[0][2], 15); - expect(actual[1][0]).toBeCloseTo(expected[1][0], 15); - expect(actual[1][1]).toBeCloseTo(expected[1][1], 15); - expect(actual[1][2]).toBeCloseTo(expected[1][2], 15); - expect(actual[2][0]).toBeCloseTo(expected[2][0], 15); - expect(actual[2][1]).toBeCloseTo(expected[2][1], 15); - expect(actual[2][2]).toBeCloseTo(expected[2][2], 15); - }, - TEST_DURATION + 1000, - ); -}); diff --git a/test/vitest/zyx2R.spec.ts b/test/vitest/zyx2R.spec.ts deleted file mode 100644 index 409404d..0000000 --- a/test/vitest/zyx2R.spec.ts +++ /dev/null @@ -1,54 +0,0 @@ -import { it } from "@fast-check/vitest"; -import { afterAll, beforeAll, describe, expect } from "vitest"; -import { zyx2R } from "../../src/index.js"; -import { arbitraryRadians } from "../arbitrary.js"; -import type { NvectorTestClient } from "../nvector-test-api.js"; -import { createNvectorTestClient } from "../nvector-test-api.js"; - -const TEST_DURATION = 5000; - -describe("zyx2R()", () => { - let nvectorTestClient: NvectorTestClient; - - beforeAll(async () => { - nvectorTestClient = await createNvectorTestClient(); - }); - - afterAll(() => { - nvectorTestClient?.close(); - }); - - it.prop([arbitraryRadians(), arbitraryRadians(), arbitraryRadians()], { - interruptAfterTimeLimit: TEST_DURATION, - numRuns: Infinity, - })( - "matches the reference implementation", - async (z, y, x) => { - const expected = await nvectorTestClient.zyx2R(z, y, x); - - 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 = zyx2R(z, y, x); - - 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], 15); - expect(actual[0][1]).toBeCloseTo(expected[0][1], 15); - expect(actual[0][2]).toBeCloseTo(expected[0][2], 15); - expect(actual[1][0]).toBeCloseTo(expected[1][0], 15); - expect(actual[1][1]).toBeCloseTo(expected[1][1], 15); - expect(actual[1][2]).toBeCloseTo(expected[1][2], 15); - expect(actual[2][0]).toBeCloseTo(expected[2][0], 15); - expect(actual[2][1]).toBeCloseTo(expected[2][1], 15); - expect(actual[2][2]).toBeCloseTo(expected[2][2], 15); - }, - TEST_DURATION + 1000, - ); -});