rust: add haversine_destination function in position.rs

Signed-off-by: Nicolas Buffon <[email protected]>

rust/benches/position.rs

@@ -1,5 +1,7 @@
 use criterion::{criterion_group, criterion_main, Criterion};
-use libits::mobility::position::{position_from_degrees, vincenty_destination};
+use libits::mobility::position::{
+    haversine_destination, position_from_degrees, vincenty_destination,
+};
 
 fn bench_vincenty_destination(c: &mut Criterion) {
     let position = position_from_degrees(48.62519582726, 2.24150938995, 0.);
@@ -11,5 +13,19 @@ fn bench_vincenty_destination(c: &mut Criterion) {
     });
 }
 
-criterion_group!(benches, bench_vincenty_destination);
+fn bench_haversine_destination(c: &mut Criterion) {
+    let position = position_from_degrees(48.62519582726, 2.24150938995, 0.);
+    let bearing = 90f64;
+    let distance = 100.;
+
+    c.bench_function("Haversine destination 100m 90deg", |b| {
+        b.iter(|| haversine_destination(&position, bearing, distance))
+    });
+}
+
+criterion_group!(
+    benches,
+    bench_vincenty_destination,
+    bench_haversine_destination
+);
 criterion_main!(benches);

rust/src/mobility/position.rs

@@ -8,14 +8,15 @@
 // Software description: This Intelligent Transportation Systems (ITS) [MQTT](https://mqtt.org/) client based on the [JSon](https://www.json.org) [ETSI](https://www.etsi.org/committee/its) specification transcription provides a ready to connect project for the mobility (connected and autonomous vehicles, road side units, vulnerable road users,...).
 
 use std::fmt::{Display, Formatter, Result};
+use std::hash::{Hash, Hasher};
 
 const EARTH_RADIUS: f64 = 6_371_000.;
 const EARTH_FLATTENING: f64 = 1. / 298.257223563;
 const EQUATORIAL_RADIUS: f64 = 6_378_137.0;
 const POLAR_RADIUS: f64 = 6_356_752.3;
 
 /// Describes a geodesic position using SI units
-#[derive(Clone, Copy, Default, Debug)]
+#[derive(Clone, Copy, Default, Debug, PartialEq)]
 pub struct Position {
     /// Latitude in radians
     pub latitude: f64,
@@ -39,6 +40,14 @@ impl Display for Position {
     }
 }
 
+impl Hash for Position {
+    fn hash<H: Hasher>(&self, state: &mut H) {
+        ((self.latitude * 1e8).round() as i64).hash(state);
+        ((self.latitude * 1e8).round() as i64).hash(state);
+        ((self.latitude * 1e3).round() as i64).hash(state);
+    }
+}
+
 pub fn position_from_degrees(lat: f64, lon: f64, alt: f64) -> Position {
     Position {
         latitude: lat.to_radians(),
@@ -74,6 +83,26 @@ pub fn haversine_distance(first: &Position, second: &Position) -> f64 {
     EARTH_RADIUS * c
 }
 
+/// φ is latitude, λ is longitude, θ is the bearing (clockwise from north), δ is the angular distance d/R; d being the distance travelled, R the earth’s radius
+pub fn haversine_destination(position: &Position, bearing: f64, distance: f64) -> Position {
+    let φ1 = position.latitude;
+    let λ1 = position.longitude;
+    let δ = distance / EARTH_RADIUS;
+
+    let φ2 = f64::asin(φ1.sin() * δ.cos() + φ1.cos() * δ.sin() * bearing.cos());
+    let λ2 = λ1
+        + f64::atan2(
+            bearing.sin() * δ.sin() * φ1.cos(),
+            δ.cos() - φ1.sin() * φ2.sin(),
+        );
+
+    Position {
+        latitude: φ2,
+        longitude: λ2,
+        altitude: position.altitude,
+    }
+}
+
 /// Destination computation from origin, bearing and distance using Vincenty formulae
 ///
 ///  Vincenty formulae written FOLLOWING:
@@ -160,7 +189,8 @@ pub fn enu_destination(
 #[cfg(test)]
 mod tests {
     use crate::mobility::position::{
-        bearing, enu_destination, haversine_distance, position_from_degrees, vincenty_destination,
+        bearing, enu_destination, haversine_destination, haversine_distance, position_from_degrees,
+        vincenty_destination,
     };
 
     macro_rules! precision_round {
@@ -287,7 +317,7 @@ mod tests {
         330.
     );
 
-    macro_rules! test_bearing_destination {
+    macro_rules! test_vincenty_destination {
         ($test_name:ident, $bearing:expr, $distance:expr, $exp_dst:expr) => {
             #[test]
             fn $test_name() {
@@ -326,52 +356,117 @@ mod tests {
             }
         };
     }
-    test_bearing_destination!(
-        north_bearing_360_deg_100m_destination,
+    test_vincenty_destination!(
+        vincenty_north_bearing_360_deg_100m_destination,
         360f64,
         100.,
         position_from_degrees(48.62609508779, 2.24150940001, 0.)
     );
-    test_bearing_destination!(
-        north_bearing_0_deg_100m_destination,
+    test_vincenty_destination!(
+        vincenty_north_bearing_0_deg_100m_destination,
         0f64,
         100.,
         position_from_degrees(48.62609508779, 2.24150940001, 0.)
     );
-    test_bearing_destination!(
-        south_bearing_180_deg_100m_destination,
+    test_vincenty_destination!(
+        vincenty_south_bearing_180_deg_100m_destination,
         180f64,
         100.,
         position_from_degrees(48.62429656659, 2.24150940001, 0.)
     );
-    test_bearing_destination!(
-        south_bearing_minus_180_deg_100m_destination,
+    test_vincenty_destination!(
+        vincenty_south_bearing_minus_180_deg_100m_destination,
         -180f64,
         100.,
         position_from_degrees(48.62429656659, 2.24150940001, 0.)
     );
-    test_bearing_destination!(
-        east_bearing_90_deg_100m_destination,
+    test_vincenty_destination!(
+        vincenty_east_bearing_90_deg_100m_destination,
         90f64,
         100.,
         position_from_degrees(48.62519580005, 2.24286588773, 0.)
     );
-    test_bearing_destination!(
-        east_bearing_minus_270_deg_100m_destination,
+    test_vincenty_destination!(
+        vincenty_east_bearing_minus_270_deg_100m_destination,
         -270f64,
         100.,
         position_from_degrees(48.62519580005, 2.24286588773, 0.)
     );
-    test_bearing_destination!(
-        west_bearing_270_deg_100m_destination,
+    test_vincenty_destination!(
+        vincenty_west_bearing_270_deg_100m_destination,
         270f64,
         100.,
         position_from_degrees(48.62519580005, 2.24015289217, 0.)
     );
-    test_bearing_destination!(
-        west_bearing_minus_90_deg_100m_destination,
+    test_vincenty_destination!(
+        vincenty_west_bearing_minus_90_deg_100m_destination,
         -90f64,
         100.,
         position_from_degrees(48.62519580005, 2.24015289217, 0.)
     );
+
+    macro_rules! test_haversine_destination {
+        ($test_name:ident, $bearing:expr, $distance:expr, $exp_dst:expr) => {
+            #[test]
+            fn $test_name() {
+                let position = position_from_degrees(48.62519582726, 2.24150938995, 0.);
+                let epsilon = 1e-7;
+                let position_deg_precision = 10e7;
+
+                let destination =
+                    haversine_destination(&position, $bearing.to_radians(), $distance);
+                let rounded_lat =
+                    precision_round!(destination.latitude.to_degrees(), position_deg_precision);
+                let lat_delta = (rounded_lat - $exp_dst.latitude.to_degrees()).abs();
+                let rounded_lon =
+                    precision_round!(destination.longitude.to_degrees(), position_deg_precision);
+                let lon_delta = (rounded_lon - $exp_dst.longitude.to_degrees()).abs();
+
+                println!("Expected: {}", $exp_dst);
+                println!("Actual: {}", destination);
+
+                assert!(
+                    lat_delta < epsilon,
+                    "{} !< {} (expected: {}, actual: {})",
+                    lat_delta,
+                    epsilon,
+                    $exp_dst.latitude.to_degrees(),
+                    rounded_lat
+                );
+
+                assert!(
+                    lon_delta < epsilon,
+                    "{} !< {} (expected: {}, actual: {})",
+                    lon_delta,
+                    epsilon,
+                    $exp_dst.longitude.to_degrees(),
+                    rounded_lon
+                );
+            }
+        };
+    }
+    test_haversine_destination!(
+        haversine_north_bearing_360_deg_100m_destination,
+        360f64,
+        100.,
+        position_from_degrees(48.62609508779, 2.24150940001, 0.)
+    );
+    test_haversine_destination!(
+        haversine_north_bearing_0_deg_100m_destination,
+        0f64,
+        100.,
+        position_from_degrees(48.62609508779, 2.24150940001, 0.)
+    );
+    test_haversine_destination!(
+        haversine_south_bearing_180_deg_100m_destination,
+        180f64,
+        100.,
+        position_from_degrees(48.62429656659, 2.24150940001, 0.)
+    );
+    test_haversine_destination!(
+        haversine_south_bearing_minus_180_deg_100m_destination,
+        -180f64,
+        100.,
+        position_from_degrees(48.62429656659, 2.24150940001, 0.)
+    );
 }