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        return if sin_lat.is_sign_negative() {
204            Angle::default()
205        } else {
206            Angle::default().opposite()
207        };
208    }
209
210    Angle::from_y_x(pole.z, pole.xy().perp(&point.xy()))
211}
212
213/// Calculate the direction vector along a Great Circle from an initial
214/// position and an azimuth.
215///
216/// See: Panou and Korakitis equations: 30, 31, & 32a
217/// <https://arxiv.org/abs/1811.03513>
218/// * `lat` - start point Latitude.
219/// * `lon` - start point Longitude.
220/// * `azi` - start point azimuth.
221///
222/// returns the direction vector at the point on the great circle.
223#[must_use]
224pub fn calculate_direction(lat: Angle, lon: Angle, azi: Angle) -> Vector3d {
225    let x = trig::UnitNegRange::clamp(
226        0.0 - lat.sin().0 * lon.cos().0 * azi.cos().0 - lon.sin().0 * azi.sin().0,
227    );
228    let y = trig::UnitNegRange::clamp(
229        0.0 - lat.sin().0 * lon.sin().0 * azi.cos().0 + lon.cos().0 * azi.sin().0,
230    );
231    let z = trig::UnitNegRange(lat.cos().0 * azi.cos().0);
232
233    Vector3d::new(x.0, y.0, z.0)
234}
235
236/// Calculate the direction vector of a Great Circle arc.
237///
238/// * `a` - the start point.
239/// * `pole` - the pole of a Great Circle.
240///
241/// returns the direction vector at the point on the great circle.
242#[must_use]
243pub fn direction(a: &Vector3d, pole: &Vector3d) -> Vector3d {
244    pole.cross(a)
245}
246
247/// Calculate the position of a point along a Great Circle arc.
248///
249/// * `a` - the start point.
250/// * `dir` - the direction vector of a Great Circle at a.
251/// * `distance` - the a Great Circle as an Angle.
252///
253/// returns the position vector at the point on the great circle.
254#[must_use]
255pub fn position(a: &Vector3d, dir: &Vector3d, distance: Angle) -> Vector3d {
256    distance.cos().0 * a + distance.sin().0 * dir
257}
258
259/// Calculate the direction vector of a Great Circle rotated by angle.
260///
261/// * `dir` - the direction vector of a Great Circle arc.
262/// * `pole` - the pole of a Great Circle.
263/// * `angle` - the angle to rotate the direction vector by.
264///
265/// returns the direction vector at the point on the great circle
266/// rotated by angle.
267#[must_use]
268pub fn rotate(dir: &Vector3d, pole: &Vector3d, angle: Angle) -> Vector3d {
269    position(dir, pole, angle)
270}
271
272/// Calculate the position of a point rotated by angle at radius.
273///
274/// * `a` - the start point.
275/// * `pole` - the pole of a Great Circle.
276/// * `angle` - the angle to rotate the direction vector by.
277/// * `radius` - the radius from the start point.
278///
279/// returns the position vector at angle and radius from the start point.
280#[must_use]
281pub fn rotate_position(a: &Vector3d, pole: &Vector3d, angle: Angle, radius: Angle) -> Vector3d {
282    position(a, &rotate(&direction(a, pole), pole, angle), radius)
283}
284
285/// The sine of the across track distance of a point relative to a Great Circle pole.
286///
287/// It is simply the dot product of the pole and the point: pole . point
288/// * `pole` - the Great Circle pole.
289/// * `point` - the point.
290///
291/// returns the sine of the across track distance of point relative to the pole.
292#[must_use]
293fn sin_xtd(pole: &Vector3d, point: &Vector3d) -> trig::UnitNegRange {
294    trig::UnitNegRange::clamp(pole.dot(point))
295}
296
297/// The across track distance of a point relative to a Great Circle pole.
298///
299/// * `pole` - the Great Circle pole.
300/// * `point` - the point.
301///
302/// returns the across track distance of point relative to pole, in `Radians`.
303#[must_use]
304pub fn cross_track_distance(pole: &Vector3d, point: &Vector3d) -> Radians {
305    let sin_d = sin_xtd(pole, point);
306    if libm::fabs(sin_d.0) < f64::EPSILON {
307        Radians(0.0)
308    } else {
309        Radians(libm::asin(sin_d.0))
310    }
311}
312
313/// The square of the Euclidean cross track distance of a point relative to a
314/// Great Circle pole.
315///
316/// * `pole` - the Great Circle pole.
317/// * `point` - the point.
318///
319/// returns the square of the euclidean distance of point relative to pole.
320#[must_use]
321pub fn sq_cross_track_distance(pole: &Vector3d, point: &Vector3d) -> f64 {
322    let sin_d = sin_xtd(pole, point);
323    if libm::fabs(sin_d.0) < f64::EPSILON {
324        0.0
325    } else {
326        2.0 * (1.0 - trig::swap_sin_cos(sin_d).0)
327    }
328}
329
330/// Calculate the closest point on a plane to the given point.
331///
332/// See: [Closest Point on Plane](https://gdbooks.gitbooks.io/3dcollisions/content/Chapter1/closest_point_on_plane.html)
333/// * `pole` - the Great Circle pole (aka normal) of the plane.
334/// * `point` - the point.
335///
336/// returns the closest point on a plane to the given point.
337#[must_use]
338fn calculate_point_on_plane(pole: &Vector3d, point: &Vector3d) -> Vector3d {
339    let t = sin_xtd(pole, point);
340    point - pole * t.0
341}
342
343/// The sine of the along track distance of a point along a Great Circle arc.
344///
345/// It is the triple product of the pole, a and the point:
346/// (pole X a) . point = pole . (a X point)
347/// * `a` - the start point of the Great Circle arc.
348/// * `pole` - the pole of the Great Circle arc.
349/// * `point` - the point.
350///
351/// returns the sine of the along track distance of point relative to the start
352/// of a great circle arc.
353#[must_use]
354pub fn sin_atd(a: &Vector3d, pole: &Vector3d, point: &Vector3d) -> trig::UnitNegRange {
355    trig::UnitNegRange::clamp(pole.cross(a).dot(point))
356}
357
358/// Calculate the relative distance of two points on a Great Circle arc.
359///
360/// @pre both points must be on the Great Circle defined by `pole`.
361/// * `a` - the start point of the Great Circle arc.
362/// * `pole` - the pole of the Great Circle arc.
363/// * `point` - a point in the Great Circle.
364///
365/// returns the Great Circle along track distance in `Radians`.
366#[must_use]
367pub fn calculate_great_circle_atd(a: &Vector3d, pole: &Vector3d, point: &Vector3d) -> Radians {
368    let sq_atd = sq_distance(a, point);
369    if sq_atd < MIN_SQ_DISTANCE {
370        Radians(0.0)
371    } else {
372        Radians(libm::copysign(
373            great_circle::e2gc_distance(libm::sqrt(sq_atd)).0,
374            sin_atd(a, pole, point).0,
375        ))
376    }
377}
378
379/// The Great Circle distance of a point along the arc relative to a,
380/// (+ve) ahead of a, (-ve) behind a.
381///
382/// * `a` - the start point of the Great Circle arc.
383/// * `pole` - the pole of the Great Circle arc.
384/// * `point` - the point.
385///
386/// returns the along track distance of point relative to the start of a great circle arc.
387#[must_use]
388pub fn along_track_distance(a: &Vector3d, pole: &Vector3d, point: &Vector3d) -> Radians {
389    let plane_point = calculate_point_on_plane(pole, point);
390    normalise(&plane_point, MIN_SQ_NORM).map_or_else(
391        || Radians(0.0), // point is too close to a pole
392        |c| calculate_great_circle_atd(a, pole, &c),
393    )
394}
395
396/// Calculate the square of the Euclidean along track distance of a point
397/// from the start of an Arc.
398///
399/// It is calculated using the closest point on the plane to the point.
400/// * `a` - the start point of the Great Circle arc.
401/// * `pole` - the pole of the Great Circle arc.
402/// * `point` - the point.
403///
404/// returns the square of the Euclidean along track distance
405#[must_use]
406pub fn sq_along_track_distance(a: &Vector3d, pole: &Vector3d, point: &Vector3d) -> f64 {
407    let plane_point = calculate_point_on_plane(pole, point);
408    normalise(&plane_point, MIN_SQ_NORM).map_or_else(
409        || 0.0, // point is too close to a pole
410        |c| {
411            let sq_d = sq_distance(a, &(c));
412            if sq_d < MIN_SQ_DISTANCE {
413                0.0
414            } else {
415                sq_d
416            }
417        },
418    )
419}
420
421/// Calculate Great Circle along and across track distances.
422///
423/// * `a` - the start point of the Great Circle arc.
424/// * `pole` - the pole of the Great Circle arc.
425/// * `p` - the point.
426///
427/// returns the along and across track distances of point relative to the
428/// start of a great circle arc.
429#[allow(clippy::similar_names)]
430#[must_use]
431pub fn calculate_atd_and_xtd(a: &Vector3d, pole: &Vector3d, p: &Vector3d) -> (Radians, Radians) {
432    let mut atd = Radians(0.0);
433    let mut xtd = Radians(0.0);
434
435    let sq_d = sq_distance(a, p);
436    if sq_d >= MIN_SQ_DISTANCE {
437        // point is not close to a
438        let sin_xtd = sin_xtd(pole, p).0;
439        if libm::fabs(sin_xtd) >= f64::EPSILON {
440            xtd = Radians(libm::asin(sin_xtd));
441        }
442
443        // the closest point on the plane of the pole to the point
444        let plane_point = p - pole * sin_xtd;
445        atd = normalise(&plane_point, MIN_SQ_NORM).map_or_else(
446            || Radians(0.0), // point is too close to a pole
447            |c| calculate_great_circle_atd(a, pole, &c),
448        );
449    }
450
451    (atd, xtd)
452}
453
454#[cfg(test)]
455mod tests {
456    use super::*;
457    use crate::LatLong;
458    use angle_sc::{is_within_tolerance, Degrees, Radians};
459
460    #[test]
461    fn test_normalise() {
462        let zero = Vector3d::new(0.0, 0.0, 0.0);
463        assert!(normalise(&zero, MIN_SQ_NORM).is_none());
464
465        // Greenwich equator
466        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
467        assert!(normalise(&g_eq, MIN_SQ_NORM).is_some());
468
469        // A vector just too small to normalize
470        let too_small = Vector3d::new(16383.0 * f64::EPSILON, 0.0, 0.0);
471        assert!(normalise(&too_small, MIN_SQ_NORM).is_none());
472
473        assert_eq!(
474            2.0844083160439303e-10,
475            libm::asin(MIN_SIN_ANGLE).to_degrees()
476        );
477
478        // A vector just large enough to normalize
479        let small = Vector3d::new(MIN_SIN_ANGLE, 0.0, 0.0);
480        let result = normalise(&small, MIN_SQ_NORM);
481        assert!(result.is_some());
482
483        assert!(is_unit(&result.unwrap()));
484        assert_eq!(result.unwrap(), g_eq);
485    }
486
487    #[test]
488    fn test_point_lat_longs() {
489        // Test South pole
490        let lat_lon_south = LatLong::new(Degrees(-90.0), Degrees(180.0));
491        let point_south = Vector3d::from(&lat_lon_south);
492
493        assert_eq!(Degrees(-90.0), Degrees::from(latitude(&point_south)));
494        assert_eq!(Degrees(0.0), Degrees::from(longitude(&point_south)));
495
496        let result = LatLong::from(&point_south);
497        assert_eq!(-90.0, result.lat().0);
498        // Note: longitude is now zero, since the poles do not have a Longitude
499        assert_eq!(0.0, result.lon().0);
500
501        // Test Greenwich equator
502        let lat_lon_0_0 = LatLong::new(Degrees(0.0), Degrees(0.0));
503        let point_0 = Vector3d::from(&lat_lon_0_0);
504        assert!(is_unit(&point_0));
505        assert_eq!(lat_lon_0_0, LatLong::from(&point_0));
506
507        // Test IDL equator
508        let lat_lon_0_180 = LatLong::new(Degrees(0.0), Degrees(180.0));
509        let point_1 = Vector3d::from(&lat_lon_0_180);
510        assert!(is_unit(&point_1));
511        assert_eq!(false, is_west_of(&point_0, &point_1));
512        assert_eq!(
513            Radians(core::f64::consts::PI),
514            Radians::from(delta_longitude(&point_0, &point_1)).abs()
515        );
516
517        let lat_lon_0_m180 = LatLong::new(Degrees(0.0), Degrees(-180.0));
518        let point_2 = Vector3d::from(&lat_lon_0_m180);
519        assert!(is_unit(&point_2));
520        // Converts back to +ve longitude
521        assert_eq!(lat_lon_0_180, LatLong::from(&point_2));
522
523        assert_eq!(false, is_west_of(&point_0, &point_2));
524        assert_eq!(
525            -core::f64::consts::PI,
526            Radians::from(delta_longitude(&point_0, &point_2)).0
527        );
528
529        let lat_lon_0_r3 = LatLong::new(Degrees(0.0), Degrees(3.0_f64.to_degrees()));
530        let point_3 = Vector3d::from(&lat_lon_0_r3);
531        assert!(is_unit(&point_3));
532        let result = LatLong::from(&point_3);
533        assert_eq!(0.0, result.lat().0);
534        assert_eq!(
535            3.0_f64,
536            Radians::from(delta_longitude(&point_3, &point_0)).0
537        );
538        assert_eq!(3.0_f64.to_degrees(), result.lon().0);
539        assert!(is_west_of(&point_0, &point_3));
540        assert_eq!(-3.0, Radians::from(delta_longitude(&point_0, &point_3)).0);
541
542        assert_eq!(false, is_west_of(&point_1, &point_3));
543        assert_eq!(
544            core::f64::consts::PI - 3.0,
545            Radians::from(delta_longitude(&point_1, &point_3)).0
546        );
547
548        let lat_lon_0_mr3 = LatLong::new(Degrees(0.0), Degrees(-3.0_f64.to_degrees()));
549        let point_4 = Vector3d::from(&lat_lon_0_mr3);
550        assert!(is_unit(&point_4));
551        assert_eq!(3.0, Radians::from(delta_longitude(&point_0, &point_4)).0);
552
553        let result = LatLong::from(&point_4);
554        assert_eq!(0.0, result.lat().0);
555        assert_eq!(-3.0_f64.to_degrees(), result.lon().0);
556        assert!(is_west_of(&point_1, &point_4));
557        assert_eq!(
558            3.0 - core::f64::consts::PI,
559            Radians::from(delta_longitude(&point_1, &point_4)).0
560        );
561    }
562
563    #[test]
564    fn test_point_distance() {
565        let lat_lon_south = LatLong::new(Degrees(-90.0), Degrees(0.0));
566        let south_pole = Vector3d::from(&lat_lon_south);
567
568        let lat_lon_north = LatLong::new(Degrees(90.0), Degrees(0.0));
569        let north_pole = Vector3d::from(&lat_lon_north);
570
571        assert_eq!(0.0, sq_distance(&south_pole, &south_pole));
572        assert_eq!(0.0, sq_distance(&north_pole, &north_pole));
573        assert_eq!(4.0, sq_distance(&south_pole, &north_pole));
574
575        assert_eq!(0.0, distance(&south_pole, &south_pole));
576        assert_eq!(0.0, distance(&north_pole, &north_pole));
577        assert_eq!(2.0, distance(&south_pole, &north_pole));
578
579        // Greenwich equator
580        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
581
582        // Test IDL equator
583        let idl_eq = Vector3d::new(-1.0, 0.0, 0.0);
584
585        assert_eq!(0.0, sq_distance(&g_eq, &g_eq));
586        assert_eq!(0.0, sq_distance(&idl_eq, &idl_eq));
587        assert_eq!(4.0, sq_distance(&g_eq, &idl_eq));
588
589        assert_eq!(0.0, distance(&g_eq, &g_eq));
590        assert_eq!(0.0, distance(&idl_eq, &idl_eq));
591        assert_eq!(2.0, distance(&g_eq, &idl_eq));
592    }
593
594    #[test]
595    fn test_calculate_azimuth_at_poles() {
596        // Greenwich equator
597        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
598        let south_pole = Vector3d::new(0.0, 0.0, -1.0);
599        let result = calculate_azimuth(&south_pole, &g_eq);
600        assert_eq!(Angle::default(), result);
601
602        let north_pole = Vector3d::new(0.0, 0.0, 1.0);
603        let result = calculate_azimuth(&north_pole, &g_eq);
604        assert_eq!(Angle::default().opposite(), result);
605    }
606
607    #[test]
608    fn test_calculate_pole_azimuth_and_direction() {
609        // Greenwich equator
610        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
611
612        // 90 degrees East on the equator
613        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
614
615        // 90 degrees West on the equator
616        let w_eq = Vector3d::new(0.0, -1.0, 0.0);
617
618        let angle_90 = Angle::from(Degrees(90.0));
619        let pole_a = calculate_pole(
620            Angle::from(Degrees(0.0)),
621            Angle::from(Degrees(0.0)),
622            angle_90,
623        );
624        assert!(are_orthogonal(&g_eq, &pole_a));
625
626        let dir_a = calculate_direction(
627            Angle::from(Degrees(0.0)),
628            Angle::from(Degrees(0.0)),
629            angle_90,
630        );
631        assert!(are_orthogonal(&g_eq, &dir_a));
632        assert!(are_orthogonal(&pole_a, &dir_a));
633        assert_eq!(dir_a, direction(&g_eq, &pole_a));
634
635        let north_pole = Vector3d::new(0.0, 0.0, 1.0);
636        assert_eq!(north_pole, pole_a);
637
638        let result = g_eq.cross(&e_eq);
639        assert_eq!(north_pole, result);
640
641        let result = calculate_azimuth(&g_eq, &pole_a);
642        assert_eq!(angle_90, result);
643
644        let pole_b = calculate_pole(
645            Angle::from(Degrees(0.0)),
646            Angle::from(Degrees(0.0)),
647            -angle_90,
648        );
649        assert!(are_orthogonal(&g_eq, &pole_b));
650
651        let dir_b = calculate_direction(
652            Angle::from(Degrees(0.0)),
653            Angle::from(Degrees(0.0)),
654            -angle_90,
655        );
656        assert!(are_orthogonal(&g_eq, &dir_b));
657        assert!(are_orthogonal(&pole_b, &dir_b));
658        assert_eq!(dir_b, direction(&g_eq, &pole_b));
659
660        let south_pole = Vector3d::new(0.0, 0.0, -1.0);
661        assert_eq!(south_pole, pole_b);
662
663        let result = g_eq.cross(&w_eq);
664        assert_eq!(south_pole, result);
665
666        let result = calculate_azimuth(&g_eq, &pole_b);
667        assert_eq!(-angle_90, result);
668    }
669
670    #[test]
671    fn test_calculate_position() {
672        // Greenwich equator
673        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
674
675        // 90 degrees East on the equator
676        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
677
678        let pole_0 = g_eq.cross(&e_eq);
679
680        let angle_90 = Angle::from(Degrees(90.0));
681
682        let pos_1 = position(&g_eq, &direction(&g_eq, &pole_0), angle_90);
683        assert_eq!(e_eq, pos_1);
684
685        let pos_2 = rotate_position(&g_eq, &pole_0, Angle::default(), angle_90);
686        assert_eq!(e_eq, pos_2);
687
688        let pos_3 = rotate_position(&g_eq, &pole_0, angle_90, angle_90);
689        assert_eq!(pole_0, pos_3);
690    }
691
692    #[test]
693    fn test_calculate_cross_track_distance_and_square() {
694        // Greenwich equator
695        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
696
697        // 90 degrees East on the equator
698        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
699
700        let pole_0 = g_eq.cross(&e_eq);
701
702        let longitude = Degrees(1.0);
703
704        for lat in -89..90 {
705            let latitude = Degrees(lat as f64);
706            let latlong = LatLong::new(latitude, longitude);
707            let point = Vector3d::from(&latlong);
708
709            let expected = (lat as f64).to_radians();
710            let xtd = cross_track_distance(&pole_0, &point);
711            // Accuracy reduces outside of this range
712            let tolerance = if (-83..84).contains(&lat) {
713                2.0 * f64::EPSILON
714            } else {
715                32.0 * f64::EPSILON
716            };
717            assert!(is_within_tolerance(expected, xtd.0, tolerance));
718
719            let expected = great_circle::gc2e_distance(Radians(expected));
720            let expected = expected * expected;
721            let xtd2 = sq_cross_track_distance(&pole_0, &point);
722            // Accuracy reduces outside of this range
723            let tolerance = if (-83..84).contains(&lat) {
724                4.0 * f64::EPSILON
725            } else {
726                64.0 * f64::EPSILON
727            };
728            assert!(is_within_tolerance(expected, xtd2, tolerance));
729        }
730    }
731
732    #[test]
733    fn test_calculate_along_track_distance_and_square() {
734        // Greenwich equator
735        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
736
737        // 90 degrees East on the equator
738        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
739
740        let pole_0 = g_eq.cross(&e_eq);
741
742        // North of Equator
743        let latitude = Degrees(1.0);
744
745        for lon in -179..180 {
746            let longitude = Degrees(lon as f64);
747            let latlong = LatLong::new(latitude, longitude);
748            let point = Vector3d::from(&latlong);
749
750            let expected = (lon as f64).to_radians();
751            let atd = along_track_distance(&g_eq, &pole_0, &point);
752            // Accuracy reduces outside of this range
753            let tolerance = if (-153..154).contains(&lon) {
754                2.0 * f64::EPSILON
755            } else {
756                32.0 * f64::EPSILON
757            };
758            assert!(is_within_tolerance(expected, atd.0, tolerance));
759
760            let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &point);
761            assert!(is_within_tolerance(expected, atd.0, tolerance));
762            assert!(is_within_tolerance(1_f64.to_radians(), xtd.0, f64::EPSILON));
763
764            let expected = great_circle::gc2e_distance(Radians(expected));
765            let expected = expected * expected;
766            let atd2 = sq_along_track_distance(&g_eq, &pole_0, &point);
767            // Accuracy reduces outside of this range
768            let tolerance = if (-86..87).contains(&lon) {
769                2.0 * f64::EPSILON
770            } else {
771                32.0 * f64::EPSILON
772            };
773            assert!(is_within_tolerance(expected, atd2, tolerance));
774        }
775    }
776
777    #[test]
778    fn test_special_cases() {
779        // Greenwich equator
780        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
781
782        // 90 degrees East on the equator
783        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
784
785        let pole_0 = g_eq.cross(&e_eq);
786
787        // points are at the poles, so atc and sq_atd are zero
788        assert_eq!(0.0, along_track_distance(&g_eq, &pole_0, &pole_0).0);
789        assert_eq!(0.0, sq_along_track_distance(&g_eq, &pole_0, &pole_0));
790
791        let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &g_eq);
792        assert_eq!(0.0, atd.0);
793        assert_eq!(0.0, xtd.0);
794
795        let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &pole_0);
796        assert_eq!(0.0, atd.0);
797        assert_eq!(core::f64::consts::FRAC_PI_2, xtd.0);
798
799        let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &-pole_0);
800        assert_eq!(0.0, atd.0);
801        assert_eq!(-core::f64::consts::FRAC_PI_2, xtd.0);
802
803        // Test for 100% code coverage
804        let near_north_pole = LatLong::new(Degrees(89.99999), Degrees(0.0));
805        let p = Vector3d::from(&near_north_pole);
806        let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &p);
807        assert_eq!(0.0, atd.0);
808        assert!(is_within_tolerance(
809            core::f64::consts::FRAC_PI_2,
810            xtd.0,
811            0.000001
812        ));
813    }
814}
unit_sphere/vector.rs

unit_sphere/
vector.rs
