unit_sphere/
vector.rs

1// Copyright (c) 2024-2025 Ken Barker
2
3// Permission is hereby granted, free of charge, to any person obtaining a copy
4// of this software and associated documentation files (the "Software"),
5// to deal in the Software without restriction, including without limitation the
6// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7// sell copies of the Software, and to permit persons to whom the Software is
8// furnished to do so, subject to the following conditions:
9
10// The above copyright notice and this permission notice shall be included in
11// all copies or substantial portions of the Software.
12
13// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19// THE SOFTWARE.
20
21//! The `vector` module contains functions for performing great circle
22//! calculations using `Vector3d`s to represent points and great circle poles
23//! on a unit sphere.
24//!
25//! A `Vector3d` is a [nalgebra](https://crates.io/crates/nalgebra) `Vector3<f64>`.
26
27use crate::{great_circle, Vector3d};
28use angle_sc::{trig, Angle, Radians};
29
30pub mod intersection;
31
32/// The minimum value of the sine of an angle to normalise.
33/// Approximately 7.504e-9 seconds
34pub const MIN_SIN_ANGLE: f64 = 16384.0 * f64::EPSILON;
35
36/// The minimum length of a vector to normalize.
37pub const MIN_SQ_NORM: f64 = MIN_SIN_ANGLE * MIN_SIN_ANGLE;
38
39/// The minimum value of the square of distance.
40pub const MIN_SQ_DISTANCE: f64 = great_circle::MIN_VALUE * great_circle::MIN_VALUE;
41
42/// Convert a latitude and longitude to a point on the unit sphere.
43///
44/// @pre |lat| <= 90.0 degrees.
45/// * `lat` - the latitude.
46/// * `lon` - the longitude.
47///
48/// returns a `Vector3d` of the point on the unit sphere.
49#[must_use]
50pub fn to_point(lat: Angle, lon: Angle) -> Vector3d {
51    Vector3d::new(
52        lat.cos().0 * lon.cos().0,
53        lat.cos().0 * lon.sin().0,
54        lat.sin().0,
55    )
56}
57
58/// Calculate the latitude of a point.
59///
60/// * `a` - the point.
61///
62/// returns the latitude of the point
63#[must_use]
64pub fn latitude(a: &Vector3d) -> Angle {
65    Angle::from_y_x(a.z, libm::hypot(a.x, a.y))
66}
67
68/// Calculate the longitude of a point.
69///
70/// * `a` - the point.
71///
72/// returns the longitude of the point
73#[must_use]
74pub fn longitude(a: &Vector3d) -> Angle {
75    Angle::from_y_x(a.y, a.x)
76}
77
78/// Determine whether a `Vector3d` is a unit vector.
79///
80/// * `a` - the vector.
81///
82/// returns true if `a` is a unit vector, false otherwise.
83#[must_use]
84pub fn is_unit(a: &Vector3d) -> bool {
85    const MIN_POINT_SQ_LENGTH: f64 = 1.0 - 12.0 * f64::EPSILON;
86    const MAX_POINT_SQ_LENGTH: f64 = 1.0 + 12.0 * f64::EPSILON;
87
88    (MIN_POINT_SQ_LENGTH..=MAX_POINT_SQ_LENGTH).contains(&(a.norm_squared()))
89}
90
91/// Normalize a vector to lie on the surface of the unit sphere.
92///
93/// Note: this function returns an `Option` so uses the British spelling of
94/// `normalise` to differentiate it from the standard `normalize` function.
95/// * `a` the `Vector3d`
96/// * `min_sq_value` the minimum square of a vector length to normalize.
97///
98/// return the nomalized point or None if the vector is too small to normalize.
99#[must_use]
100pub fn normalise(a: &Vector3d, min_sq_value: f64) -> Option<Vector3d> {
101    if a.norm_squared() < min_sq_value {
102        None
103    } else {
104        Some(a.normalize())
105    }
106}
107
108/// Calculate the square of the Euclidean distance between two points.
109///
110/// Note: points do NOT need to be valid Points.
111/// @post for unit vectors: result <= 4
112/// * `a`, `b` the points.
113///
114/// returns the square of the Euclidean distance between the points.
115#[must_use]
116pub fn sq_distance(a: &Vector3d, b: &Vector3d) -> f64 {
117    (b - a).norm_squared()
118}
119
120/// Calculate the shortest (Euclidean) distance between two Points.
121///
122/// @post for unit vectors: result <= 2
123/// * `a`, `b` the points.
124///
125/// returns the shortest (Euclidean) distance between the points.
126#[must_use]
127pub fn distance(a: &Vector3d, b: &Vector3d) -> f64 {
128    (b - a).norm()
129}
130
131/// Determine whether two `Vector3d`s are orthogonal (perpendicular).
132///
133/// * `a`, `b` the `Vector3d`s.
134///
135/// returns true if a and b are orthogonal, false otherwise.
136#[must_use]
137pub fn are_orthogonal(a: &Vector3d, b: &Vector3d) -> bool {
138    const MAX_LENGTH: f64 = 4.0 * f64::EPSILON;
139
140    (-MAX_LENGTH..=MAX_LENGTH).contains(&(a.dot(b)))
141}
142
143/// Calculate the relative longitude of point a from point b.
144///
145/// * `a`, `b` - the points.
146///
147/// returns the relative longitude of point a from point b,
148/// negative if a is West of b, positive otherwise.
149#[must_use]
150pub fn delta_longitude(a: &Vector3d, b: &Vector3d) -> Angle {
151    let a_lon = a.xy();
152    let b_lon = b.xy();
153    Angle::from_y_x(b_lon.perp(&a_lon), b_lon.dot(&a_lon))
154}
155
156/// Determine whether point a is West of point b.
157///
158/// It calculates and compares the perp product of the two points.
159/// * `a`, `b` - the points.
160///
161/// returns true if a is West of b, false otherwise.
162#[must_use]
163pub fn is_west_of(a: &Vector3d, b: &Vector3d) -> bool {
164    // Compare with -epsilon to handle floating point errors
165    b.xy().perp(&a.xy()) <= -f64::EPSILON
166}
167
168/// Calculate the right hand pole vector of a Great Circle from an initial
169/// position and an azimuth.
170///
171/// See: <http://www.movable-type.co.uk/scripts/latlong-vectors.html#distance>
172/// * `lat` - start point Latitude.
173/// * `lon` - start point Longitude.
174/// * `azi` - start point azimuth.
175///
176/// returns the right hand pole vector of the great circle.
177#[must_use]
178pub fn calculate_pole(lat: Angle, lon: Angle, azi: Angle) -> Vector3d {
179    let x = trig::UnitNegRange::clamp(
180        lon.sin().0 * azi.cos().0 - lat.sin().0 * lon.cos().0 * azi.sin().0,
181    );
182    let y = trig::UnitNegRange::clamp(
183        0.0 - lon.cos().0 * azi.cos().0 - lat.sin().0 * lon.sin().0 * azi.sin().0,
184    );
185    let z = trig::UnitNegRange(lat.cos().0 * azi.sin().0);
186
187    Vector3d::new(x.0, y.0, z.0)
188}
189
190/// Calculate the azimuth at a point on the Great Circle defined by pole.
191///
192/// * `point` - the point.
193/// * `pole` - the right hand pole of the Great Circle.
194///
195/// returns the azimuth at the point on the great circle.
196#[must_use]
197pub fn calculate_azimuth(point: &Vector3d, pole: &Vector3d) -> Angle {
198    const MAX_LAT: f64 = 1.0 - great_circle::MIN_VALUE;
199
200    let sin_lat = point.z;
201    // if the point is close to the North or South poles, azimuth is 180 or 0.
202    if MAX_LAT <= libm::fabs(sin_lat) {
203        // azimuth is zero or 180 degrees
204        return if sin_lat.is_sign_negative() {
205            Angle::default()
206        } else {
207            Angle::new(trig::UnitNegRange(0.0), trig::UnitNegRange(-1.0))
208        };
209    }
210
211    Angle::from_y_x(pole.z, pole.xy().perp(&point.xy()))
212}
213
214/// Calculate the direction vector along a Great Circle from an initial
215/// position and an azimuth.
216///
217/// See: Panou and Korakitis equations: 30, 31, & 32a
218/// <https://arxiv.org/abs/1811.03513>
219/// * `lat` - start point Latitude.
220/// * `lon` - start point Longitude.
221/// * `azi` - start point azimuth.
222///
223/// returns the direction vector at the point on the great circle.
224#[must_use]
225pub fn calculate_direction(lat: Angle, lon: Angle, azi: Angle) -> Vector3d {
226    let x = trig::UnitNegRange::clamp(
227        0.0 - lat.sin().0 * lon.cos().0 * azi.cos().0 - lon.sin().0 * azi.sin().0,
228    );
229    let y = trig::UnitNegRange::clamp(
230        0.0 - lat.sin().0 * lon.sin().0 * azi.cos().0 + lon.cos().0 * azi.sin().0,
231    );
232    let z = trig::UnitNegRange(lat.cos().0 * azi.cos().0);
233
234    Vector3d::new(x.0, y.0, z.0)
235}
236
237/// Calculate the direction vector of a Great Circle arc.
238///
239/// * `a` - the start point.
240/// * `pole` - the pole of a Great Circle.
241///
242/// returns the direction vector at the point on the great circle.
243#[must_use]
244pub fn direction(a: &Vector3d, pole: &Vector3d) -> Vector3d {
245    pole.cross(a)
246}
247
248/// Calculate the position of a point along a Great Circle arc.
249///
250/// * `a` - the start point.
251/// * `dir` - the direction vector of a Great Circle at a.
252/// * `distance` - the a Great Circle as an Angle.
253///
254/// returns the position vector at the point on the great circle.
255#[must_use]
256pub fn position(a: &Vector3d, dir: &Vector3d, distance: Angle) -> Vector3d {
257    distance.cos().0 * a + distance.sin().0 * dir
258}
259
260/// Calculate the direction vector of a Great Circle rotated by angle.
261///
262/// * `dir` - the direction vector of a Great Circle arc.
263/// * `pole` - the pole of a Great Circle.
264/// * `angle` - the angle to rotate the direction vector by.
265///
266/// returns the direction vector at the point on the great circle
267/// rotated by angle.
268#[must_use]
269pub fn rotate(dir: &Vector3d, pole: &Vector3d, angle: Angle) -> Vector3d {
270    position(dir, pole, angle)
271}
272
273/// Calculate the position of a point rotated by angle at radius.
274///
275/// * `a` - the start point.
276/// * `pole` - the pole of a Great Circle.
277/// * `angle` - the angle to rotate the direction vector by.
278/// * `radius` - the radius from the start point.
279///
280/// returns the position vector at angle and radius from the start point.
281#[must_use]
282pub fn rotate_position(a: &Vector3d, pole: &Vector3d, angle: Angle, radius: Angle) -> Vector3d {
283    position(a, &rotate(&direction(a, pole), pole, angle), radius)
284}
285
286/// The sine of the across track distance of a point relative to a Great Circle pole.
287///
288/// It is simply the dot product of the pole and the point: pole . point
289/// * `pole` - the Great Circle pole.
290/// * `point` - the point.
291///
292/// returns the sine of the across track distance of point relative to the pole.
293#[must_use]
294fn sin_xtd(pole: &Vector3d, point: &Vector3d) -> trig::UnitNegRange {
295    trig::UnitNegRange::clamp(pole.dot(point))
296}
297
298/// The across track distance of a point relative to a Great Circle pole.
299///
300/// * `pole` - the Great Circle pole.
301/// * `point` - the point.
302///
303/// returns the across track distance of point relative to pole, in `Radians`.
304#[must_use]
305pub fn cross_track_distance(pole: &Vector3d, point: &Vector3d) -> Radians {
306    let sin_d = sin_xtd(pole, point);
307    if libm::fabs(sin_d.0) < f64::EPSILON {
308        Radians(0.0)
309    } else {
310        Radians(libm::asin(sin_d.0))
311    }
312}
313
314/// The square of the Euclidean cross track distance of a point relative to a
315/// Great Circle pole.
316///
317/// * `pole` - the Great Circle pole.
318/// * `point` - the point.
319///
320/// returns the square of the euclidean distance of point relative to pole.
321#[must_use]
322pub fn sq_cross_track_distance(pole: &Vector3d, point: &Vector3d) -> f64 {
323    let sin_d = sin_xtd(pole, point);
324    if libm::fabs(sin_d.0) < f64::EPSILON {
325        0.0
326    } else {
327        2.0 * (1.0 - trig::swap_sin_cos(sin_d).0)
328    }
329}
330
331/// Calculate the closest point on a plane to the given point.
332///
333/// See: [Closest Point on Plane](https://gdbooks.gitbooks.io/3dcollisions/content/Chapter1/closest_point_on_plane.html)
334/// * `pole` - the Great Circle pole (aka normal) of the plane.
335/// * `point` - the point.
336///
337/// returns the closest point on a plane to the given point.
338#[must_use]
339fn calculate_point_on_plane(pole: &Vector3d, point: &Vector3d) -> Vector3d {
340    let t = sin_xtd(pole, point);
341    point - pole * t.0
342}
343
344/// The sine of the along track distance of a point along a Great Circle arc.
345///
346/// It is the triple product of the pole, a and the point:
347/// (pole X a) . point = pole . (a X point)
348/// * `a` - the start point of the Great Circle arc.
349/// * `pole` - the pole of the Great Circle arc.
350/// * `point` - the point.
351///
352/// returns the sine of the along track distance of point relative to the start
353/// of a great circle arc.
354#[must_use]
355pub fn sin_atd(a: &Vector3d, pole: &Vector3d, point: &Vector3d) -> trig::UnitNegRange {
356    trig::UnitNegRange::clamp(pole.cross(a).dot(point))
357}
358
359/// Calculate the relative distance of two points on a Great Circle arc.
360///
361/// @pre both points must be on the Great Circle defined by `pole`.
362/// * `a` - the start point of the Great Circle arc.
363/// * `pole` - the pole of the Great Circle arc.
364/// * `point` - a point in the Great Circle.
365///
366/// returns the Great Circle along track distance in `Radians`.
367#[must_use]
368pub fn calculate_great_circle_atd(a: &Vector3d, pole: &Vector3d, point: &Vector3d) -> Radians {
369    let sq_atd = sq_distance(a, point);
370    if sq_atd < MIN_SQ_DISTANCE {
371        Radians(0.0)
372    } else {
373        Radians(libm::copysign(
374            great_circle::e2gc_distance(libm::sqrt(sq_atd)).0,
375            sin_atd(a, pole, point).0,
376        ))
377    }
378}
379
380/// The Great Circle distance of a point along the arc relative to a,
381/// (+ve) ahead of a, (-ve) behind a.
382///
383/// * `a` - the start point of the Great Circle arc.
384/// * `pole` - the pole of the Great Circle arc.
385/// * `point` - the point.
386///
387/// returns the along track distance of point relative to the start of a great circle arc.
388#[must_use]
389pub fn along_track_distance(a: &Vector3d, pole: &Vector3d, point: &Vector3d) -> Radians {
390    let plane_point = calculate_point_on_plane(pole, point);
391    normalise(&plane_point, MIN_SQ_NORM).map_or_else(
392        || Radians(0.0), // point is too close to a pole
393        |c| calculate_great_circle_atd(a, pole, &c),
394    )
395}
396
397/// Calculate the square of the Euclidean along track distance of a point
398/// from the start of an Arc.
399///
400/// It is calculated using the closest point on the plane to the point.
401/// * `a` - the start point of the Great Circle arc.
402/// * `pole` - the pole of the Great Circle arc.
403/// * `point` - the point.
404///
405/// returns the square of the Euclidean along track distance
406#[must_use]
407pub fn sq_along_track_distance(a: &Vector3d, pole: &Vector3d, point: &Vector3d) -> f64 {
408    let plane_point = calculate_point_on_plane(pole, point);
409    normalise(&plane_point, MIN_SQ_NORM).map_or_else(
410        || 0.0, // point is too close to a pole
411        |c| {
412            let sq_d = sq_distance(a, &(c));
413            if sq_d < MIN_SQ_DISTANCE {
414                0.0
415            } else {
416                sq_d
417            }
418        },
419    )
420}
421
422/// Calculate Great Circle along and across track distances.
423///
424/// * `a` - the start point of the Great Circle arc.
425/// * `pole` - the pole of the Great Circle arc.
426/// * `p` - the point.
427///
428/// returns the along and across track distances of point relative to the
429/// start of a great circle arc.
430#[allow(clippy::similar_names)]
431#[must_use]
432pub fn calculate_atd_and_xtd(a: &Vector3d, pole: &Vector3d, p: &Vector3d) -> (Radians, Radians) {
433    let mut atd = Radians(0.0);
434    let mut xtd = Radians(0.0);
435
436    let sq_d = sq_distance(a, p);
437    if sq_d >= MIN_SQ_DISTANCE {
438        // point is not close to a
439        let sin_xtd = sin_xtd(pole, p).0;
440        if libm::fabs(sin_xtd) >= f64::EPSILON {
441            xtd = Radians(libm::asin(sin_xtd));
442        }
443
444        // the closest point on the plane of the pole to the point
445        let plane_point = p - pole * sin_xtd;
446        atd = normalise(&plane_point, MIN_SQ_NORM).map_or_else(
447            || Radians(0.0), // point is too close to a pole
448            |c| calculate_great_circle_atd(a, pole, &c),
449        );
450    }
451
452    (atd, xtd)
453}
454
455#[cfg(test)]
456mod tests {
457    use super::*;
458    use crate::LatLong;
459    use angle_sc::{is_within_tolerance, Degrees, Radians};
460
461    #[test]
462    fn test_normalise() {
463        let zero = Vector3d::new(0.0, 0.0, 0.0);
464        assert!(normalise(&zero, MIN_SQ_NORM).is_none());
465
466        // Greenwich equator
467        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
468        assert!(normalise(&g_eq, MIN_SQ_NORM).is_some());
469
470        // A vector just too small to normalize
471        let too_small = Vector3d::new(16383.0 * f64::EPSILON, 0.0, 0.0);
472        assert!(normalise(&too_small, MIN_SQ_NORM).is_none());
473
474        assert_eq!(
475            2.0844083160439303e-10,
476            libm::asin(MIN_SIN_ANGLE).to_degrees()
477        );
478
479        // A vector just large enough to normalize
480        let small = Vector3d::new(MIN_SIN_ANGLE, 0.0, 0.0);
481        let result = normalise(&small, MIN_SQ_NORM);
482        assert!(result.is_some());
483
484        assert!(is_unit(&result.unwrap()));
485        assert_eq!(result.unwrap(), g_eq);
486    }
487
488    #[test]
489    fn test_point_lat_longs() {
490        // Test South pole
491        let lat_lon_south = LatLong::new(Degrees(-90.0), Degrees(180.0));
492        let point_south = Vector3d::from(&lat_lon_south);
493
494        assert_eq!(Degrees(-90.0), Degrees::from(latitude(&point_south)));
495        assert_eq!(Degrees(0.0), Degrees::from(longitude(&point_south)));
496
497        let result = LatLong::from(&point_south);
498        assert_eq!(-90.0, result.lat().0);
499        // Note: longitude is now zero, since the poles do not have a Longitude
500        assert_eq!(0.0, result.lon().0);
501
502        // Test Greenwich equator
503        let lat_lon_0_0 = LatLong::new(Degrees(0.0), Degrees(0.0));
504        let point_0 = Vector3d::from(&lat_lon_0_0);
505        assert!(is_unit(&point_0));
506        assert_eq!(lat_lon_0_0, LatLong::from(&point_0));
507
508        // Test IDL equator
509        let lat_lon_0_180 = LatLong::new(Degrees(0.0), Degrees(180.0));
510        let point_1 = Vector3d::from(&lat_lon_0_180);
511        assert!(is_unit(&point_1));
512        assert_eq!(false, is_west_of(&point_0, &point_1));
513        assert_eq!(
514            Radians(core::f64::consts::PI),
515            Radians::from(delta_longitude(&point_0, &point_1)).abs()
516        );
517
518        let lat_lon_0_m180 = LatLong::new(Degrees(0.0), Degrees(-180.0));
519        let point_2 = Vector3d::from(&lat_lon_0_m180);
520        assert!(is_unit(&point_2));
521        // Converts back to +ve longitude
522        assert_eq!(lat_lon_0_180, LatLong::from(&point_2));
523
524        assert_eq!(false, is_west_of(&point_0, &point_2));
525        assert_eq!(
526            -core::f64::consts::PI,
527            Radians::from(delta_longitude(&point_0, &point_2)).0
528        );
529
530        let lat_lon_0_r3 = LatLong::new(Degrees(0.0), Degrees(3.0_f64.to_degrees()));
531        let point_3 = Vector3d::from(&lat_lon_0_r3);
532        assert!(is_unit(&point_3));
533        let result = LatLong::from(&point_3);
534        assert_eq!(0.0, result.lat().0);
535        assert_eq!(
536            3.0_f64,
537            Radians::from(delta_longitude(&point_3, &point_0)).0
538        );
539        assert_eq!(3.0_f64.to_degrees(), result.lon().0);
540        assert!(is_west_of(&point_0, &point_3));
541        assert_eq!(-3.0, Radians::from(delta_longitude(&point_0, &point_3)).0);
542
543        assert_eq!(false, is_west_of(&point_1, &point_3));
544        assert_eq!(
545            core::f64::consts::PI - 3.0,
546            Radians::from(delta_longitude(&point_1, &point_3)).0
547        );
548
549        let lat_lon_0_mr3 = LatLong::new(Degrees(0.0), Degrees(-3.0_f64.to_degrees()));
550        let point_4 = Vector3d::from(&lat_lon_0_mr3);
551        assert!(is_unit(&point_4));
552        assert_eq!(3.0, Radians::from(delta_longitude(&point_0, &point_4)).0);
553
554        let result = LatLong::from(&point_4);
555        assert_eq!(0.0, result.lat().0);
556        assert_eq!(-3.0_f64.to_degrees(), result.lon().0);
557        assert!(is_west_of(&point_1, &point_4));
558        assert_eq!(
559            3.0 - core::f64::consts::PI,
560            Radians::from(delta_longitude(&point_1, &point_4)).0
561        );
562    }
563
564    #[test]
565    fn test_point_distance() {
566        let lat_lon_south = LatLong::new(Degrees(-90.0), Degrees(0.0));
567        let south_pole = Vector3d::from(&lat_lon_south);
568
569        let lat_lon_north = LatLong::new(Degrees(90.0), Degrees(0.0));
570        let north_pole = Vector3d::from(&lat_lon_north);
571
572        assert_eq!(0.0, sq_distance(&south_pole, &south_pole));
573        assert_eq!(0.0, sq_distance(&north_pole, &north_pole));
574        assert_eq!(4.0, sq_distance(&south_pole, &north_pole));
575
576        assert_eq!(0.0, distance(&south_pole, &south_pole));
577        assert_eq!(0.0, distance(&north_pole, &north_pole));
578        assert_eq!(2.0, distance(&south_pole, &north_pole));
579
580        // Greenwich equator
581        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
582
583        // Test IDL equator
584        let idl_eq = Vector3d::new(-1.0, 0.0, 0.0);
585
586        assert_eq!(0.0, sq_distance(&g_eq, &g_eq));
587        assert_eq!(0.0, sq_distance(&idl_eq, &idl_eq));
588        assert_eq!(4.0, sq_distance(&g_eq, &idl_eq));
589
590        assert_eq!(0.0, distance(&g_eq, &g_eq));
591        assert_eq!(0.0, distance(&idl_eq, &idl_eq));
592        assert_eq!(2.0, distance(&g_eq, &idl_eq));
593    }
594
595    #[test]
596    fn test_calculate_azimuth_at_poles() {
597        // Greenwich equator
598        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
599        let south_pole = Vector3d::new(0.0, 0.0, -1.0);
600        let result = calculate_azimuth(&south_pole, &g_eq);
601        assert_eq!(Angle::default(), result);
602
603        let north_pole = Vector3d::new(0.0, 0.0, 1.0);
604        let result = calculate_azimuth(&north_pole, &g_eq);
605        assert_eq!(Angle::default().opposite(), result);
606    }
607
608    #[test]
609    fn test_calculate_pole_azimuth_and_direction() {
610        // Greenwich equator
611        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
612
613        // 90 degrees East on the equator
614        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
615
616        // 90 degrees West on the equator
617        let w_eq = Vector3d::new(0.0, -1.0, 0.0);
618
619        let angle_90 = Angle::from(Degrees(90.0));
620        let pole_a = calculate_pole(
621            Angle::from(Degrees(0.0)),
622            Angle::from(Degrees(0.0)),
623            angle_90,
624        );
625        assert!(are_orthogonal(&g_eq, &pole_a));
626
627        let dir_a = calculate_direction(
628            Angle::from(Degrees(0.0)),
629            Angle::from(Degrees(0.0)),
630            angle_90,
631        );
632        assert!(are_orthogonal(&g_eq, &dir_a));
633        assert!(are_orthogonal(&pole_a, &dir_a));
634        assert_eq!(dir_a, direction(&g_eq, &pole_a));
635
636        let north_pole = Vector3d::new(0.0, 0.0, 1.0);
637        assert_eq!(north_pole, pole_a);
638
639        let result = g_eq.cross(&e_eq);
640        assert_eq!(north_pole, result);
641
642        let result = calculate_azimuth(&g_eq, &pole_a);
643        assert_eq!(angle_90, result);
644
645        let pole_b = calculate_pole(
646            Angle::from(Degrees(0.0)),
647            Angle::from(Degrees(0.0)),
648            -angle_90,
649        );
650        assert!(are_orthogonal(&g_eq, &pole_b));
651
652        let dir_b = calculate_direction(
653            Angle::from(Degrees(0.0)),
654            Angle::from(Degrees(0.0)),
655            -angle_90,
656        );
657        assert!(are_orthogonal(&g_eq, &dir_b));
658        assert!(are_orthogonal(&pole_b, &dir_b));
659        assert_eq!(dir_b, direction(&g_eq, &pole_b));
660
661        let south_pole = Vector3d::new(0.0, 0.0, -1.0);
662        assert_eq!(south_pole, pole_b);
663
664        let result = g_eq.cross(&w_eq);
665        assert_eq!(south_pole, result);
666
667        let result = calculate_azimuth(&g_eq, &pole_b);
668        assert_eq!(-angle_90, result);
669    }
670
671    #[test]
672    fn test_calculate_position() {
673        // Greenwich equator
674        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
675
676        // 90 degrees East on the equator
677        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
678
679        let pole_0 = g_eq.cross(&e_eq);
680
681        let angle_90 = Angle::from(Degrees(90.0));
682
683        let pos_1 = position(&g_eq, &direction(&g_eq, &pole_0), angle_90);
684        assert_eq!(e_eq, pos_1);
685
686        let pos_2 = rotate_position(&g_eq, &pole_0, Angle::default(), angle_90);
687        assert_eq!(e_eq, pos_2);
688
689        let pos_3 = rotate_position(&g_eq, &pole_0, angle_90, angle_90);
690        assert_eq!(pole_0, pos_3);
691    }
692
693    #[test]
694    fn test_calculate_cross_track_distance_and_square() {
695        // Greenwich equator
696        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
697
698        // 90 degrees East on the equator
699        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
700
701        let pole_0 = g_eq.cross(&e_eq);
702
703        let longitude = Degrees(1.0);
704
705        for lat in -89..90 {
706            let latitude = Degrees(lat as f64);
707            let latlong = LatLong::new(latitude, longitude);
708            let point = Vector3d::from(&latlong);
709
710            let expected = (lat as f64).to_radians();
711            let xtd = cross_track_distance(&pole_0, &point);
712            // Accuracy reduces outside of this range
713            let tolerance = if (-83..84).contains(&lat) {
714                2.0 * f64::EPSILON
715            } else {
716                32.0 * f64::EPSILON
717            };
718            assert!(is_within_tolerance(expected, xtd.0, tolerance));
719
720            let expected = great_circle::gc2e_distance(Radians(expected));
721            let expected = expected * expected;
722            let xtd2 = sq_cross_track_distance(&pole_0, &point);
723            // Accuracy reduces outside of this range
724            let tolerance = if (-83..84).contains(&lat) {
725                4.0 * f64::EPSILON
726            } else {
727                64.0 * f64::EPSILON
728            };
729            assert!(is_within_tolerance(expected, xtd2, tolerance));
730        }
731    }
732
733    #[test]
734    fn test_calculate_along_track_distance_and_square() {
735        // Greenwich equator
736        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
737
738        // 90 degrees East on the equator
739        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
740
741        let pole_0 = g_eq.cross(&e_eq);
742
743        // North of Equator
744        let latitude = Degrees(1.0);
745
746        for lon in -179..180 {
747            let longitude = Degrees(lon as f64);
748            let latlong = LatLong::new(latitude, longitude);
749            let point = Vector3d::from(&latlong);
750
751            let expected = (lon as f64).to_radians();
752            let atd = along_track_distance(&g_eq, &pole_0, &point);
753            // Accuracy reduces outside of this range
754            let tolerance = if (-153..154).contains(&lon) {
755                2.0 * f64::EPSILON
756            } else {
757                32.0 * f64::EPSILON
758            };
759            assert!(is_within_tolerance(expected, atd.0, tolerance));
760
761            let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &point);
762            assert!(is_within_tolerance(expected, atd.0, tolerance));
763            assert!(is_within_tolerance(1_f64.to_radians(), xtd.0, f64::EPSILON));
764
765            let expected = great_circle::gc2e_distance(Radians(expected));
766            let expected = expected * expected;
767            let atd2 = sq_along_track_distance(&g_eq, &pole_0, &point);
768            // Accuracy reduces outside of this range
769            let tolerance = if (-86..87).contains(&lon) {
770                2.0 * f64::EPSILON
771            } else {
772                32.0 * f64::EPSILON
773            };
774            assert!(is_within_tolerance(expected, atd2, tolerance));
775        }
776    }
777
778    #[test]
779    fn test_special_cases() {
780        // Greenwich equator
781        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
782
783        // 90 degrees East on the equator
784        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
785
786        let pole_0 = g_eq.cross(&e_eq);
787
788        // points are at the poles, so atc and sq_atd are zero
789        assert_eq!(0.0, along_track_distance(&g_eq, &pole_0, &pole_0).0);
790        assert_eq!(0.0, sq_along_track_distance(&g_eq, &pole_0, &pole_0));
791
792        let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &g_eq);
793        assert_eq!(0.0, atd.0);
794        assert_eq!(0.0, xtd.0);
795
796        let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &pole_0);
797        assert_eq!(0.0, atd.0);
798        assert_eq!(core::f64::consts::FRAC_PI_2, xtd.0);
799
800        let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &-pole_0);
801        assert_eq!(0.0, atd.0);
802        assert_eq!(-core::f64::consts::FRAC_PI_2, xtd.0);
803
804        // Test for 100% code coverage
805        let near_north_pole = LatLong::new(Degrees(89.99999), Degrees(0.0));
806        let p = Vector3d::from(&near_north_pole);
807        let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &p);
808        assert_eq!(0.0, atd.0);
809        assert!(is_within_tolerance(
810            core::f64::consts::FRAC_PI_2,
811            xtd.0,
812            0.000001
813        ));
814    }
815}
unit_sphere/vector.rs

unit_sphere/
vector.rs
