unit_sphere/
vector.rs

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

unit_sphere/
vector.rs
