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

unit_sphere/
vector.rs
