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::{Vector3d, great_circle};
28use angle_sc::{Angle, Radians, trig};
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, a.x.hypot(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 <= sin_lat.abs() {
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 sin_d.0.abs() < f64::EPSILON {
331        Radians(0.0)
332    } else {
333        Radians(sin_d.0.asin())
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 sin_d.0.abs() < 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(
397            great_circle::e2gc_distance(sq_atd.sqrt())
398                .0
399                .copysign(sin_atd(a, pole, point).0),
400        )
401        // Radians(
402        //     great_circle::e2gc_distance(atd)
403        //         .0
404        //         .copysign(sin_atd(a, pole, point).0),
405        // )
406    }
407}
408
409/// The Great Circle distance of a point along the arc relative to a,
410/// (+ve) ahead of a, (-ve) behind a.
411///
412/// * `a` - the start point of the Great Circle arc.
413/// * `pole` - the pole of the Great Circle arc.
414/// * `point` - the point.
415///
416/// returns the along track distance of point relative to the start of a great circle arc.
417#[must_use]
418pub fn along_track_distance(a: &Vector3d, pole: &Vector3d, point: &Vector3d) -> Radians {
419    let plane_point = calculate_point_on_plane(pole, point);
420    normalise(&plane_point, MIN_SQ_NORM).map_or_else(
421        || Radians(0.0), // point is too close to a pole
422        |c| calculate_great_circle_atd(a, pole, &c),
423    )
424}
425
426/// Calculate the square of the Euclidean along track distance of a point
427/// from the start of an Arc.
428///
429/// It is calculated using the closest point on the plane to the point.
430/// * `a` - the start point of the Great Circle arc.
431/// * `pole` - the pole of the Great Circle arc.
432/// * `point` - the point.
433///
434/// returns the square of the Euclidean along track distance
435#[must_use]
436pub fn sq_along_track_distance(a: &Vector3d, pole: &Vector3d, point: &Vector3d) -> f64 {
437    let plane_point = calculate_point_on_plane(pole, point);
438    normalise(&plane_point, MIN_SQ_NORM).map_or_else(
439        || 0.0, // point is too close to a pole
440        |c| {
441            let sq_d = sq_distance(a, &(c));
442            if sq_d < MIN_SQ_DISTANCE { 0.0 } else { sq_d }
443        },
444    )
445}
446
447/// Calculate Great Circle along and across track distances.
448///
449/// * `a` - the start point of the Great Circle arc.
450/// * `pole` - the pole of the Great Circle arc.
451/// * `p` - the point.
452///
453/// returns the along and across track distances of point relative to the
454/// start of a great circle arc.
455#[allow(clippy::similar_names)]
456#[must_use]
457pub fn calculate_atd_and_xtd(a: &Vector3d, pole: &Vector3d, p: &Vector3d) -> (Radians, Radians) {
458    let mut atd = Radians(0.0);
459    let mut xtd = Radians(0.0);
460
461    let sq_d = sq_distance(a, p);
462    if sq_d >= MIN_SQ_DISTANCE {
463        // point is not close to a
464        let sin_xtd = sin_xtd(pole, p).0;
465        if sin_xtd.abs() >= f64::EPSILON {
466            xtd = Radians(sin_xtd.asin());
467        }
468
469        // the closest point on the plane of the pole to the point
470        let plane_point = p - pole * sin_xtd;
471        atd = normalise(&plane_point, MIN_SQ_NORM).map_or_else(
472            || Radians(0.0), // point is too close to a pole
473            |c| calculate_great_circle_atd(a, pole, &c),
474        );
475    }
476
477    (atd, xtd)
478}
479
480#[cfg(test)]
481mod tests {
482    use super::*;
483    use crate::LatLong;
484    use angle_sc::{Degrees, Radians, is_within_tolerance};
485
486    #[test]
487    fn test_normalise() {
488        let zero = Vector3d::new(0.0, 0.0, 0.0);
489        assert!(normalise(&zero, MIN_SQ_NORM).is_none());
490
491        // Greenwich equator
492        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
493        assert!(normalise(&g_eq, MIN_SQ_NORM).is_some());
494
495        // A vector just too small to normalize
496        let too_small = Vector3d::new(16383.0 * f64::EPSILON, 0.0, 0.0);
497        assert!(normalise(&too_small, MIN_SQ_NORM).is_none());
498
499        assert_eq!(2.0844083160439303e-10, MIN_SIN_ANGLE.to_degrees().asin());
500
501        // A vector just large enough to normalize
502        let small = Vector3d::new(MIN_SIN_ANGLE, 0.0, 0.0);
503        let result = normalise(&small, MIN_SQ_NORM);
504        assert!(result.is_some());
505
506        assert!(is_unit(&result.unwrap()));
507        assert_eq!(result.unwrap(), g_eq);
508    }
509
510    #[test]
511    fn test_point_lat_longs() {
512        // Test South pole
513        let lat_lon_south = LatLong::new(Degrees(-90.0), Degrees(180.0));
514        let point_south = Vector3d::from(&lat_lon_south);
515
516        assert_eq!(Degrees(-90.0), Degrees::from(latitude(&point_south)));
517        assert_eq!(Degrees(0.0), Degrees::from(longitude(&point_south)));
518
519        let result = LatLong::from(&point_south);
520        assert_eq!(-90.0, result.lat().0);
521        // Note: longitude is now zero, since the poles do not have a Longitude
522        assert_eq!(0.0, result.lon().0);
523
524        // Test Greenwich equator
525        let lat_lon_0_0 = LatLong::new(Degrees(0.0), Degrees(0.0));
526        let point_0 = Vector3d::from(&lat_lon_0_0);
527        assert!(is_unit(&point_0));
528        assert_eq!(lat_lon_0_0, LatLong::from(&point_0));
529
530        // Test IDL equator
531        let lat_lon_0_180 = LatLong::new(Degrees(0.0), Degrees(180.0));
532        let point_1 = Vector3d::from(&lat_lon_0_180);
533        assert!(is_unit(&point_1));
534        assert_eq!(false, is_west_of(&point_0, &point_1));
535        assert_eq!(
536            Radians(core::f64::consts::PI),
537            Radians::from(delta_longitude(&point_0, &point_1)).abs()
538        );
539
540        let lat_lon_0_m180 = LatLong::new(Degrees(0.0), Degrees(-180.0));
541        let point_2 = Vector3d::from(&lat_lon_0_m180);
542        assert!(is_unit(&point_2));
543        // Converts back to +ve longitude
544        assert_eq!(lat_lon_0_180, LatLong::from(&point_2));
545
546        assert_eq!(false, is_west_of(&point_0, &point_2));
547        assert_eq!(
548            -core::f64::consts::PI,
549            Radians::from(delta_longitude(&point_0, &point_2)).0
550        );
551
552        let lat_lon_0_r3 = LatLong::new(Degrees(0.0), Degrees(3.0_f64.to_degrees()));
553        let point_3 = Vector3d::from(&lat_lon_0_r3);
554        assert!(is_unit(&point_3));
555        let result = LatLong::from(&point_3);
556        assert_eq!(0.0, result.lat().0);
557        assert_eq!(
558            3.0_f64,
559            Radians::from(delta_longitude(&point_3, &point_0)).0
560        );
561        assert_eq!(3.0_f64.to_degrees(), result.lon().0);
562        assert!(is_west_of(&point_0, &point_3));
563        assert_eq!(-3.0, Radians::from(delta_longitude(&point_0, &point_3)).0);
564
565        assert_eq!(false, is_west_of(&point_1, &point_3));
566        assert_eq!(
567            core::f64::consts::PI - 3.0,
568            Radians::from(delta_longitude(&point_1, &point_3)).0
569        );
570
571        let lat_lon_0_mr3 = LatLong::new(Degrees(0.0), Degrees(-3.0_f64.to_degrees()));
572        let point_4 = Vector3d::from(&lat_lon_0_mr3);
573        assert!(is_unit(&point_4));
574        assert_eq!(3.0, Radians::from(delta_longitude(&point_0, &point_4)).0);
575
576        let result = LatLong::from(&point_4);
577        assert_eq!(0.0, result.lat().0);
578        assert_eq!(-3.0_f64.to_degrees(), result.lon().0);
579        assert!(is_west_of(&point_1, &point_4));
580        assert_eq!(
581            3.0 - core::f64::consts::PI,
582            Radians::from(delta_longitude(&point_1, &point_4)).0
583        );
584    }
585
586    #[test]
587    fn test_point_distance() {
588        let lat_lon_south = LatLong::new(Degrees(-90.0), Degrees(0.0));
589        let south_pole = Vector3d::from(&lat_lon_south);
590
591        let lat_lon_north = LatLong::new(Degrees(90.0), Degrees(0.0));
592        let north_pole = Vector3d::from(&lat_lon_north);
593
594        assert_eq!(0.0, sq_distance(&south_pole, &south_pole));
595        assert_eq!(0.0, sq_distance(&north_pole, &north_pole));
596        assert_eq!(4.0, sq_distance(&south_pole, &north_pole));
597
598        assert_eq!(0.0, distance(&south_pole, &south_pole));
599        assert_eq!(0.0, distance(&north_pole, &north_pole));
600        assert_eq!(2.0, distance(&south_pole, &north_pole));
601
602        // Greenwich equator
603        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
604
605        // Test IDL equator
606        let idl_eq = Vector3d::new(-1.0, 0.0, 0.0);
607
608        assert_eq!(0.0, sq_distance(&g_eq, &g_eq));
609        assert_eq!(0.0, sq_distance(&idl_eq, &idl_eq));
610        assert_eq!(4.0, sq_distance(&g_eq, &idl_eq));
611
612        assert_eq!(0.0, distance(&g_eq, &g_eq));
613        assert_eq!(0.0, distance(&idl_eq, &idl_eq));
614        assert_eq!(2.0, distance(&g_eq, &idl_eq));
615    }
616
617    #[test]
618    fn test_calculate_azimuth_at_poles() {
619        // Greenwich equator
620        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
621        let south_pole = Vector3d::new(0.0, 0.0, -1.0);
622        let result = calculate_azimuth(&south_pole, &g_eq);
623        assert_eq!(Angle::default(), result);
624
625        let north_pole = Vector3d::new(0.0, 0.0, 1.0);
626        let result = calculate_azimuth(&north_pole, &g_eq);
627        assert_eq!(Angle::default().opposite(), result);
628    }
629
630    #[test]
631    fn test_calculate_pole_azimuth_and_direction() {
632        // Greenwich equator
633        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
634
635        // 90 degrees East on the equator
636        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
637
638        // 90 degrees West on the equator
639        let w_eq = Vector3d::new(0.0, -1.0, 0.0);
640
641        let angle_90 = Angle::from(Degrees(90.0));
642        let pole_a = calculate_pole(
643            Angle::from(Degrees(0.0)),
644            Angle::from(Degrees(0.0)),
645            angle_90,
646        );
647        assert!(are_orthogonal(&g_eq, &pole_a));
648
649        let dir_a = calculate_direction(
650            Angle::from(Degrees(0.0)),
651            Angle::from(Degrees(0.0)),
652            angle_90,
653        );
654        assert!(are_orthogonal(&g_eq, &dir_a));
655        assert!(are_orthogonal(&pole_a, &dir_a));
656        assert_eq!(dir_a, direction(&g_eq, &pole_a));
657
658        let north_pole = Vector3d::new(0.0, 0.0, 1.0);
659        assert_eq!(north_pole, pole_a);
660
661        let result = g_eq.cross(&e_eq);
662        assert_eq!(north_pole, result);
663
664        let result = calculate_azimuth(&g_eq, &pole_a);
665        assert_eq!(angle_90, result);
666
667        let pole_b = calculate_pole(
668            Angle::from(Degrees(0.0)),
669            Angle::from(Degrees(0.0)),
670            -angle_90,
671        );
672        assert!(are_orthogonal(&g_eq, &pole_b));
673
674        let dir_b = calculate_direction(
675            Angle::from(Degrees(0.0)),
676            Angle::from(Degrees(0.0)),
677            -angle_90,
678        );
679        assert!(are_orthogonal(&g_eq, &dir_b));
680        assert!(are_orthogonal(&pole_b, &dir_b));
681        assert_eq!(dir_b, direction(&g_eq, &pole_b));
682
683        let south_pole = Vector3d::new(0.0, 0.0, -1.0);
684        assert_eq!(south_pole, pole_b);
685
686        let result = g_eq.cross(&w_eq);
687        assert_eq!(south_pole, result);
688
689        let result = calculate_azimuth(&g_eq, &pole_b);
690        assert_eq!(-angle_90, result);
691    }
692
693    #[test]
694    fn test_calculate_position() {
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 angle_90 = Angle::from(Degrees(90.0));
704
705        let pos_1 = position(&g_eq, &direction(&g_eq, &pole_0), angle_90);
706        assert_eq!(e_eq, pos_1);
707
708        let pos_2 = rotate_position(&g_eq, &pole_0, Angle::default(), angle_90);
709        assert_eq!(e_eq, pos_2);
710
711        let pos_3 = rotate_position(&g_eq, &pole_0, angle_90, angle_90);
712        assert_eq!(pole_0, pos_3);
713    }
714
715    #[test]
716    fn test_calculate_cross_track_distance_and_square() {
717        // Greenwich equator
718        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
719
720        // 90 degrees East on the equator
721        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
722
723        let pole_0 = g_eq.cross(&e_eq);
724
725        let longitude = Degrees(1.0);
726
727        for lat in -89..90 {
728            let latitude = Degrees(lat as f64);
729            let latlong = LatLong::new(latitude, longitude);
730            let point = Vector3d::from(&latlong);
731
732            assert_eq!(lat < 0, is_south_of(&point, &g_eq));
733            assert_eq!(lat >= 0, !is_south_of(&point, &e_eq));
734            assert_eq!(lat < 0, is_right_of(&pole_0, &point));
735
736            let expected = (lat as f64).to_radians();
737            let xtd = cross_track_distance(&pole_0, &point);
738            // Accuracy reduces outside of this range
739            let tolerance = if (-83..84).contains(&lat) {
740                2.0 * f64::EPSILON
741            } else {
742                32.0 * f64::EPSILON
743            };
744            assert!(is_within_tolerance(expected, xtd.0, tolerance));
745
746            let expected = great_circle::gc2e_distance(Radians(expected));
747            let expected = expected * expected;
748            let xtd2 = sq_cross_track_distance(&pole_0, &point);
749            // Accuracy reduces outside of this range
750            let tolerance = if (-83..84).contains(&lat) {
751                4.0 * f64::EPSILON
752            } else {
753                64.0 * f64::EPSILON
754            };
755            assert!(is_within_tolerance(expected, xtd2, tolerance));
756        }
757    }
758
759    #[test]
760    fn test_calculate_along_track_distance_and_square() {
761        // Greenwich equator
762        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
763
764        // 90 degrees East on the equator
765        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
766
767        let pole_0 = g_eq.cross(&e_eq);
768
769        // North of Equator
770        let latitude = Degrees(1.0);
771
772        for lon in -179..180 {
773            let longitude = Degrees(lon as f64);
774            let latlong = LatLong::new(latitude, longitude);
775            let point = Vector3d::from(&latlong);
776
777            let expected = (lon as f64).to_radians();
778            let atd = along_track_distance(&g_eq, &pole_0, &point);
779            // Accuracy reduces outside of this range
780            let tolerance = if (-153..154).contains(&lon) {
781                4.0 * f64::EPSILON
782            } else {
783                32.0 * f64::EPSILON
784            };
785            assert!(is_within_tolerance(expected, atd.0, tolerance));
786
787            let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &point);
788            assert!(is_within_tolerance(expected, atd.0, tolerance));
789            assert!(is_within_tolerance(1_f64.to_radians(), xtd.0, f64::EPSILON));
790
791            let expected = great_circle::gc2e_distance(Radians(expected));
792            let expected = expected * expected;
793            let atd2 = sq_along_track_distance(&g_eq, &pole_0, &point);
794            // Accuracy reduces outside of this range
795            let tolerance = if (-86..87).contains(&lon) {
796                2.0 * f64::EPSILON
797            } else {
798                32.0 * f64::EPSILON
799            };
800            assert!(is_within_tolerance(expected, atd2, tolerance));
801        }
802    }
803
804    #[test]
805    fn test_special_cases() {
806        // Greenwich equator
807        let g_eq = Vector3d::new(1.0, 0.0, 0.0);
808
809        // 90 degrees East on the equator
810        let e_eq = Vector3d::new(0.0, 1.0, 0.0);
811
812        let pole_0 = g_eq.cross(&e_eq);
813
814        // points are at the poles, so atc and sq_atd are zero
815        assert_eq!(0.0, along_track_distance(&g_eq, &pole_0, &pole_0).0);
816        assert_eq!(0.0, sq_along_track_distance(&g_eq, &pole_0, &pole_0));
817
818        let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &g_eq);
819        assert_eq!(0.0, atd.0);
820        assert_eq!(0.0, xtd.0);
821
822        let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &pole_0);
823        assert_eq!(0.0, atd.0);
824        assert_eq!(core::f64::consts::FRAC_PI_2, xtd.0);
825
826        let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &-pole_0);
827        assert_eq!(0.0, atd.0);
828        assert_eq!(-core::f64::consts::FRAC_PI_2, xtd.0);
829
830        // Test for 100% code coverage
831        let near_north_pole = LatLong::new(Degrees(89.99999), Degrees(0.0));
832        let p = Vector3d::from(&near_north_pole);
833        let (atd, xtd) = calculate_atd_and_xtd(&g_eq, &pole_0, &p);
834        assert_eq!(0.0, atd.0);
835        assert!(is_within_tolerance(
836            core::f64::consts::FRAC_PI_2,
837            xtd.0,
838            0.000001
839        ));
840    }
841}
unit_sphere/vector.rs

unit_sphere/
vector.rs
