From a196bae72c23f0634bd6557c44298e982e59461b Mon Sep 17 00:00:00 2001 From: Erin Millard Date: Tue, 28 May 2024 15:43:03 +1000 Subject: [PATCH] WIP --- test/vitest/examples/example-01.spec.ts | 56 +++++++++++++--------- test/vitest/examples/example-02.spec.ts | 63 +++++++++++++++---------- test/vitest/examples/example-03.spec.ts | 20 +++++--- test/vitest/examples/example-04.spec.ts | 16 ++++--- test/vitest/examples/example-05.spec.ts | 16 +++++-- test/vitest/examples/example-06.spec.ts | 15 +++--- test/vitest/examples/example-07.spec.ts | 10 +++- test/vitest/examples/example-08.spec.ts | 46 +++++++++++++----- test/vitest/examples/example-09.spec.ts | 29 +++++++++--- test/vitest/examples/example-10.spec.ts | 25 +++++++--- 10 files changed, 197 insertions(+), 99 deletions(-) diff --git a/test/vitest/examples/example-01.spec.ts b/test/vitest/examples/example-01.spec.ts index 2444eda..bb54cdd 100644 --- a/test/vitest/examples/example-01.spec.ts +++ b/test/vitest/examples/example-01.spec.ts @@ -11,12 +11,17 @@ import { /** * 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; @@ -38,36 +43,45 @@ test("Example 1", () => { // SOLUTION: - // Step 1: First, the given latitudes and longitudes are converted to - // n-vectors: + // 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: + // 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): + // 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): + // 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): + // 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); // meters - expect(abN[1]).toBeCloseTo(332997.8749892695, 8); // meters - expect(abN[2]).toBeCloseTo(17404.27136193635, 8); // meters + 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 0d0cbe7..5a9f9da 100644 --- a/test/vitest/examples/example-02.spec.ts +++ b/test/vitest/examples/example-02.spec.ts @@ -22,47 +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 bc: Vector = [3000, 2000, 100]; + // PROBLEM: - // Position and orientation of B is given: - // unit to get unit length of vector + // 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 three angles are yaw, pitch, and roll - const r = eulerZYXToRotationMatrix(radians(10), radians(20), radians(30)); - // A custom reference ellipsoid is given (replacing WGS-84): - // (WGS-72) + // 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 position of C. + // Find the exact position of object C as an n-vector and a depth. // SOLUTION: - // Step1: Find R_EN: - const rn = toRotationMatrix(b); + // 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); - // Step2: Find R_EB, from R_EN and R_NB: - // Note: closest frames cancel - const rb = multiply(rn, r); + // 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); - // Step3: Decompose the delta vector in E: - // no transpose of R_EB, since the vector is in B - const bce = transform(rb, bc); + // Step 3 + // + // The delta vector is now decomposed in E: + const bcE = transform(rEB, bcB); - // Step4: Find the position of C, using the functions that goes from one - // position and a delta, to a new position: - const [c, cDepth] = destination(b, bce, bDepth, 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); - // When displaying the resulting position for humans, it is more convenient - // to see lat, long: + // Use human-friendly outputs: const [lat, lon] = toGeodeticCoordinates(c); - - // Here we also assume that the user wants the output to be height (= -depth): const height = -cDepth; expect(degrees(lat)).toBeCloseTo(53.32637826433107, 13); expect(degrees(lon)).toBeCloseTo(63.46812343514746, 13); - expect(height).toBeCloseTo(406.0071960700098, 15); // meters + 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 90dd182..0a62273 100644 --- a/test/vitest/examples/example-03.spec.ts +++ b/test/vitest/examples/example-03.spec.ts @@ -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 pb: Vector = apply((n) => n * 6371e3, [0.71, -0.72, 0.1]); // m + // PROBLEM: - // Find position B as geodetic latitude, longitude and height + // 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: - // Find n-vector from the p-vector: + // Step 1 + // + // We have a function that converts ECEF-vectors to n-vectors: const [b, bDepth] = fromECEF(pb); - // Convert to lat, long and height: + // 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); // meters + 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 338553f..7fda528 100644 --- a/test/vitest/examples/example-04.spec.ts +++ b/test/vitest/examples/example-04.spec.ts @@ -14,22 +14,24 @@ import { * @see https://www.ffi.no/en/research/n-vector/#example_4 */ test("Example 4", () => { - // Position B is given with lat, long and height: + // PROBLEM: + + // Geodetic latitude, longitude and height are given for position B: const bLat = 1; const bLon = 2; const bHeight = 3; - // Find the vector p_EB_E ("ECEF-vector") + // Find the ECEF-vector for this position. // SOLUTION: - // Step1: Convert to n-vector: + // 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: + // Step 2: Convert to an ECEF-vector: const pb = toECEF(b, -bHeight); - expect(pb[0]).toBeCloseTo(6373290.277218279, 8); // meters - expect(pb[1]).toBeCloseTo(222560.2006747365, 8); // meters - expect(pb[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 ecd5179..d0b4b07 100644 --- a/test/vitest/examples/example-05.spec.ts +++ b/test/vitest/examples/example-05.spec.ts @@ -24,7 +24,6 @@ test.each` ${"or input as lat/long in deg:"} | ${fromGeodeticCoordinates(radians(88), radians(0))} | ${fromGeodeticCoordinates(radians(89), radians(-170))} | ${332.4564441053448} | ${332.4187248568097} `( "Example 5 ($label)", - // Position A and B are given as n_EA_E and n_EB_E: ({ a, b, @@ -36,16 +35,23 @@ test.each` expectedGCD: number; expectedED: number; }) => { - // m, mean Earth radius + // PROBLEM: + + // Given two positions A and B as n-vectors, 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: - // The great circle distance is given by equation (16) in Gade (2010): - // Well conditioned for all angles: + // Find the great circle distance: const gcd = Math.atan2(norm(cross(a, b)), dot(a, b)) * r; - // The Euclidean distance is given by: + // Find the Euclidean distance: const ed = norm(apply((b, a) => b - a, b, a)) * r; expect(gcd / 1000).toBeCloseTo(expectedGCD, 10); // kilometers diff --git a/test/vitest/examples/example-06.spec.ts b/test/vitest/examples/example-06.spec.ts index 23690cc..aa6134d 100644 --- a/test/vitest/examples/example-06.spec.ts +++ b/test/vitest/examples/example-06.spec.ts @@ -23,7 +23,6 @@ test.each` ${"or input as lat/long in deg:"} | ${fromGeodeticCoordinates(radians(89.9), radians(-150))} | ${fromGeodeticCoordinates(radians(89.9), radians(150))} | ${89.91282199988446} | ${173.4132244463705} `( "Example 6 ($label)", - // Position B is given at time t0 as pt0 and at time t1 as pt1: ({ pt0, pt1, @@ -35,16 +34,19 @@ test.each` expectedLat: number; expectedLon: number; }) => { - // The times are given as: + // PROBLEM: + + // Given the position of B at time t0 and t1, pt0 and pt1: const t0 = 10; const t1 = 20; - const ti = 16; // time of interpolation + const ti = 16; - // 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: - // Using standard interpolation: + // Standard interpolation can be used directly with n-vector: const pti = normalize( apply( (pt0, pt1) => pt0 + ((ti - t0) * (pt1 - pt0)) / (t1 - t0), @@ -53,8 +55,7 @@ test.each` ), ); - // When displaying the resulting position for humans, it is more convenient - // to see lat, long: + // Use human-friendly outputs: const [lat, lon] = toGeodeticCoordinates(pti); expect(degrees(lat)).toBeCloseTo(expectedLat, 12); diff --git a/test/vitest/examples/example-07.spec.ts b/test/vitest/examples/example-07.spec.ts index 5108d73..f254831 100644 --- a/test/vitest/examples/example-07.spec.ts +++ b/test/vitest/examples/example-07.spec.ts @@ -20,7 +20,6 @@ test.each` ${"or input as lat/long in deg:"} | ${fromGeodeticCoordinates(radians(90), radians(0))} | ${fromGeodeticCoordinates(radians(60), radians(10))} | ${fromGeodeticCoordinates(radians(50), radians(-20))} | ${[0.3841171702926, -0.04660240548568945, 0.9221074857571395]} `( "Example 7 ($label)", - // Three positions A, B and C are given: ({ a, b, @@ -32,9 +31,16 @@ test.each` c: Vector; expectedM: Vector; }) => { + // PROBLEM: + + // Three positions A, B, and C are given as n-vectors. + // + // 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: + // 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(expectedM[0], 16); diff --git a/test/vitest/examples/example-08.spec.ts b/test/vitest/examples/example-08.spec.ts index 22f9d28..9a05482 100644 --- a/test/vitest/examples/example-08.spec.ts +++ b/test/vitest/examples/example-08.spec.ts @@ -27,7 +27,6 @@ test.each` ${"or input as lat/long in deg:"} | ${fromGeodeticCoordinates(radians(80), radians(-90))} | ${79.99154867339445} | ${-90.01769837291397} `( "Example 8 ($label)", - // Position A is given as a: ({ a, expectedLat, @@ -37,40 +36,61 @@ test.each` expectedLat: number; expectedLon: number; }) => { - // The initial azimuth and great circle distance (s_AB), and Earth radius - // (r_Earth) are also given: + // PROBLEM: + + // Position A is given as n-vector. 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; // m - const r = 6371e3; // m, mean Earth radius + 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: - // Step1: Find unit vectors for north and east (see equations (9) and (10) - // in Gade (2010): + // 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: + // 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: + // 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: + // Use human-friendly outputs: const [lat, lon] = toGeodeticCoordinates(b); expect(degrees(lat)).toBeCloseTo(expectedLat, 13); diff --git a/test/vitest/examples/example-09.spec.ts b/test/vitest/examples/example-09.spec.ts index d02b67e..c7e14c4 100644 --- a/test/vitest/examples/example-09.spec.ts +++ b/test/vitest/examples/example-09.spec.ts @@ -25,7 +25,6 @@ test.each` ${"or input as lat/long in deg:"} | ${fromGeodeticCoordinates(radians(50), radians(180))} | ${fromGeodeticCoordinates(radians(90), radians(180))} | ${fromGeodeticCoordinates(radians(60), radians(160))} | ${fromGeodeticCoordinates(radians(80), radians(-140))} | ${74.16344802135536} | ${180} `( "Example 9 ($label)", - // Two paths A and B are given by two pairs of positions: ({ a1, a2, @@ -41,18 +40,34 @@ test.each` expectedLat: number; expectedLon: number; }) => { + // 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, while path B is given by b1 and b2. + // + // Find the position C where the two paths intersect. + // SOLUTION: - // Find the intersection between the two paths, n_EC_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 a1, by selecting sign from the dot product - // between n_EC_E_tmp and a1: + // 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: + // Use human-friendly outputs: const [lat, lon] = toGeodeticCoordinates(c); expect(degrees(lat)).toBeCloseTo(expectedLat, 16); diff --git a/test/vitest/examples/example-10.spec.ts b/test/vitest/examples/example-10.spec.ts index aca917c..4a69aa7 100644 --- a/test/vitest/examples/example-10.spec.ts +++ b/test/vitest/examples/example-10.spec.ts @@ -22,7 +22,6 @@ test.each` ${"or input as lat/long in deg:"} | ${fromGeodeticCoordinates(radians(0), radians(0))} | ${fromGeodeticCoordinates(radians(10), radians(0))} | ${fromGeodeticCoordinates(radians(1), radians(0.1))} | ${11117.79911014538} | ${11117.79346740667} `( "Example 10 ($label)", - // Position A1 and A2 and B are given as a1, a2, and b: ({ a1, a2, @@ -36,21 +35,35 @@ test.each` expectedGCD: number; expectedED: number; }) => { - const r = 6371e3; // m, mean Earth radius + // PROBLEM: - // Find the cross track distance from path A to position B. + // Path A is given by the two n-vectors a1 and a2 (as in the previous + // example), and a position B is given by b. + // + // 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: - // Find the unit normal to the great circle between a1 and a2: + // 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: (acos(x) - pi/2 = -asin(x)) + // Find the great circle cross track distance: const gcd = -Math.asin(dot(c, b)) * r; - // Find the Euclidean cross track distance: + // 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(expectedGCD, 9); // meters expect(ed).toBeCloseTo(expectedED, 9); // meters },