mapping_algorithms/geo/
haversine.rs

1// SPDX-License-Identifier: MIT
2/*
3 * Copyright (c) [2023 - Present] Emily Matheys <emilymatt96@gmail.com>
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining a copy
6 * of this software and associated documentation files (the "Software"), to deal
7 * in the Software without restriction, including without limitation the rights
8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 * copies of the Software, and to permit persons to whom the Software is
10 * furnished to do so, subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be included in all
13 * copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 * SOFTWARE.
22 */
23
24use nalgebra::{Point2, RealField, Scalar};
25use num_traits::Float;
26
27#[inline]
28fn half_angle_sine_squared<T>(input: T) -> T
29where
30    T: Float,
31{
32    (input.to_radians() / (T::one() + T::one())).sin().powi(2)
33}
34
35/// Calculates the Haversine distance between two points on a sphere using floating-point arithmetic.
36///
37/// # Arguments
38/// * `point_a`: A [`Point2'] representing the first geographical point.
39/// * `point_b`: A [`Point2`] representing the second geographical point.
40/// * `sphere_radius`: A `T` representing the radius of the sphere, typically the Earth's radius in kilometers or miles.
41///
42/// # Generics
43/// `T`: Either an [`prim@f32`] or [`prim@f64`]
44///
45/// # Returns
46/// A 'T', the distance between `point_a` and `point_b` along the surface of the sphere, using the Haversine formula.
47#[cfg_attr(
48    feature = "tracing",
49    tracing::instrument("Calculate Haversine Distance", skip_all)
50)]
51pub fn calculate_haversine_distance<T>(
52    point_a: Point2<T>,
53    point_b: Point2<T>,
54    sphere_radius: T,
55) -> T
56where
57    T: Scalar + Float,
58{
59    let delta_lat = point_b.x - point_a.x;
60    let delta_lon = point_b.y - point_a.y;
61
62    let lat1_radians = point_a.x.to_radians();
63    let lat2_radians = point_b.x.to_radians();
64
65    let basic_haversine = half_angle_sine_squared(delta_lat)
66        + half_angle_sine_squared(delta_lon) * lat1_radians.cos() * lat2_radians.cos();
67
68    let inverse_haversine = (T::one() + T::one())
69        * basic_haversine
70            .sqrt()
71            .atan2((T::one() - basic_haversine).sqrt());
72
73    sphere_radius * inverse_haversine
74}
75
76/// Calculates the initial bearing (forward azimuth) from the first point to the second point.
77///
78/// This function computes the initial bearing, or forward azimuth, between two points on the surface
79/// of a sphere, assuming a spherical model. The bearing is the direction one must travel
80/// from the first point to reach the second point, expressed as an angle from North (0 radians)
81/// in a clockwise direction.
82///
83/// # Arguments
84/// * `point_a`: A [`Point2`] representing the starting geographical point (latitude and longitude).
85/// * `point_b`: A [`Point2`] representing the destination geographical point (latitude and longitude).
86///
87/// # Generics
88/// `T`: Either an [`prim@f32`] or [`prim@f64`]
89///
90/// # Returns
91/// * A value that representing the initial bearing from `point_a` to `point_b`, in radians, The result is normalized to a range of 0 to 2 PI radians.
92#[cfg_attr(
93    feature = "tracing",
94    tracing::instrument("Calculate Bearing Between Points", skip_all)
95)]
96pub fn calculate_sphere_bearing<T>(point_a: Point2<T>, point_b: Point2<T>) -> T
97where
98    T: Scalar + Float + RealField,
99{
100    let lat1_rad = point_a.x.to_radians();
101    let lat2_rad = point_b.x.to_radians();
102
103    let lon_delta_radians = (point_b.y - point_a.y).to_radians();
104
105    let x = <T as Float>::sin(lon_delta_radians) * <T as Float>::cos(lat2_rad);
106    let y = (<T as Float>::cos(lat1_rad) * <T as Float>::sin(lat2_rad))
107        - (<T as Float>::sin(lat1_rad)
108            * <T as Float>::cos(lat2_rad)
109            * <T as Float>::cos(lon_delta_radians));
110
111    (<T as Float>::atan2(x, y) + T::two_pi()) % T::two_pi()
112}
113
114#[cfg(feature = "pregenerated")]
115macro_rules! impl_haversine_formula {
116    ($prec:expr, doc $doc:tt) => {
117        ::paste::paste! {
118            pub(super) mod [<$doc _precision>] {
119                #[doc = "A premade variant of the haversine distance calculation function, made for " $doc " precision floating-point arithmetic."]
120                pub fn calculate_haversine_distance(point_a: nalgebra::Point2<$prec>, point_b: nalgebra::Point2<$prec>, sphere_radius: $prec) -> $prec {
121                    super::calculate_haversine_distance(point_a,point_b,sphere_radius)
122                }
123
124                #[doc = "A premade variant of the sphere bearing calculation function, made for " $doc " precision floating-point arithmetic."]
125                pub fn calculate_sphere_bearing(point_a: nalgebra::Point2<$prec>, point_b: nalgebra::Point2<$prec>) -> $prec {
126                    super::calculate_sphere_bearing(point_a,point_b)}
127                }
128            }
129        }
130}
131
132#[cfg(feature = "pregenerated")]
133impl_haversine_formula!(f32, doc single);
134#[cfg(feature = "pregenerated")]
135impl_haversine_formula!(f64, doc double);
136
137#[cfg(test)]
138mod tests {
139    use super::*;
140
141    #[test]
142    fn test_distance() {
143        let point_a = Point2::new(52.5200, 13.4050); // Berlin, Germany
144        let point_b = Point2::new(48.8566, 2.3522); // Paris, France
145
146        let earth_radius_km = 6371.0;
147        let distance = calculate_haversine_distance(point_a, point_b, earth_radius_km);
148        let expected_distance = 877.46; // Approximate distance in km
149        assert!(
150            (distance - expected_distance).abs() < 0.01,
151            "Distance between Berlin and Paris should be roughly 877.46 km, found {}",
152            distance
153        );
154    }
155
156    #[test]
157    fn test_bearing() {
158        let point_a = Point2::new(39.099_91, -94.581213); // Kansas City
159        let point_b = Point2::new(38.627_09, -90.200_2); // St Louis
160
161        let bearing = calculate_sphere_bearing(point_a, point_b);
162        let expected_bearing = 96.51; // Approximate bearing in degrees
163        assert!(
164            (bearing - expected_bearing.to_radians()).abs() < 0.01,
165            "Bearing from Kansas City to St Louis should be roughly 96.51 degrees, found {}",
166            bearing.to_degrees()
167        );
168    }
169}
mapping_algorithms/geo/haversine.rs

mapping_algorithms/geo/
haversine.rs
