libits_client/reception/exchange/
reference_position.rs

1// Software Name: its-client
2// SPDX-FileCopyrightText: Copyright (c) 2016-2022 Orange
3// SPDX-License-Identifier: MIT License
4//
5// This software is distributed under the MIT license, see LICENSE.txt file for more details.
6//
7// Author: Frédéric GARDES <frederic.gardes@orange.com> et al.
8// 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,...).
9use core::fmt;
10use std::f64::consts;
11
12use cheap_ruler::{CheapRuler, DistanceUnit};
13use navigation::Location;
14use serde::{Deserialize, Serialize};
15
16const EARTH_RADIUS: u32 = 6371000;
17// in meters
18const LG_MOD: u8 = 180;
19// Max longitude on WGS 84
20const COORDINATE_SIGNIFICANT_DIGIT: u8 = 7;
21
22const ALTITUDE_SIGNIFICANT_DIGIT: u8 = 3;
23
24#[derive(Clone, Default, Debug, Eq, Hash, PartialEq, Serialize, Deserialize)]
25pub struct ReferencePosition {
26    pub latitude: i32,
27    pub longitude: i32,
28    pub altitude: i32,
29}
30
31impl ReferencePosition {
32    pub fn get_distance(&self, other: &ReferencePosition) -> u32 {
33        let longitude = get_coordinate(self.longitude) * consts::PI / LG_MOD as f64;
34        let latitude = get_coordinate(self.latitude) * consts::PI / LG_MOD as f64;
35        let other_longitude = get_coordinate(other.longitude) * consts::PI / LG_MOD as f64;
36        let other_latitude = get_coordinate(other.latitude) * consts::PI / LG_MOD as f64;
37
38        let longitude_distance = other_longitude - longitude;
39        let latitude_distance = other_latitude - latitude;
40
41        // Haversine formula
42        let a = (latitude_distance / 2.0).sin() * (latitude_distance / 2.0).sin()
43            + latitude.cos()
44                * other_latitude.cos()
45                * (longitude_distance / 2.0).sin()
46                * (longitude_distance / 2.0).sin();
47
48        let c = 2.0 * a.sqrt().atan2((1.0 - a).sqrt());
49
50        let distance = EARTH_RADIUS as f64 * c;
51        distance.round() as u32
52    }
53
54    pub fn get_heading(&self, other: &ReferencePosition) -> u16 {
55        // FIXME use cheap_ruler instead of navigation crates
56        let self_location = Location::new(
57            get_coordinate(self.latitude),
58            get_coordinate(self.longitude),
59        );
60        let other_location = Location::new(
61            get_coordinate(other.latitude),
62            get_coordinate(other.longitude),
63        );
64
65        let bearing = self_location.calc_bearing_to(&other_location);
66        (bearing * 10_f64).round() as u16
67    }
68
69    pub fn get_destination(&self, distance: f64, bearing: f64) -> Self {
70        let longitude_coordinate = get_coordinate(self.longitude);
71        let latitude_coordinate = get_coordinate(self.latitude);
72        let cr = CheapRuler::new(latitude_coordinate, DistanceUnit::Meters);
73        let p1 = (longitude_coordinate, latitude_coordinate).into();
74        let destination = cr.destination(&p1, distance, bearing);
75        ReferencePosition {
76            longitude: get_etsi_coordinate(destination.x()),
77            latitude: get_etsi_coordinate(destination.y()),
78            altitude: self.altitude,
79        }
80    }
81}
82
83impl fmt::Display for ReferencePosition {
84    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
85        write!(
86            f,
87            "lat {}/long {}/alt {}",
88            get_coordinate(self.latitude),
89            get_coordinate(self.longitude),
90            get_altitude(self.altitude),
91        )
92    }
93}
94
95fn get_coordinate(etsi_coordinate: i32) -> f64 {
96    let base: i32 = 10;
97    etsi_coordinate as f64 / base.pow(COORDINATE_SIGNIFICANT_DIGIT as u32) as f64
98}
99
100fn get_etsi_coordinate(coordinate: f64) -> i32 {
101    let base: i32 = 10;
102    (coordinate * base.pow(COORDINATE_SIGNIFICANT_DIGIT as u32) as f64) as i32
103}
104
105fn get_altitude(etsi_altitude: i32) -> f64 {
106    let base: i32 = 10;
107    etsi_altitude as f64 / base.pow(ALTITUDE_SIGNIFICANT_DIGIT as u32) as f64
108}
109
110#[cfg(test)]
111mod tests {
112    use navigation::Location;
113
114    use crate::reception::exchange::ReferencePosition;
115
116    fn teqmo_lane_merge_reference_postion() -> ReferencePosition {
117        // center is at TEQMO lane merge position
118        ReferencePosition {
119            latitude: 486244870,
120            longitude: 22436370,
121            ..Default::default() // no altitude
122        }
123    }
124
125    fn teqmo_city_reference_postion() -> ReferencePosition {
126        // center is at TEQMO city
127        ReferencePosition {
128            latitude: 486249990,
129            longitude: 22412116,
130            ..Default::default() // no altitude
131        }
132    }
133
134    #[test]
135    fn compute_100_meters_distance() {
136        let position = teqmo_lane_merge_reference_postion();
137        // I take a point at 100 meters
138        let other_position = ReferencePosition {
139            latitude: 486237420,
140            longitude: 22428750,
141            ..Default::default() // no altitude
142        };
143        assert_eq!(position.get_distance(&other_position), 100);
144    }
145
146    #[test]
147    fn compute_31_meters_distance() {
148        // center is at TEQMO city
149        let position = teqmo_city_reference_postion();
150        // I take a point at 31 meters
151        let other_position = ReferencePosition {
152            latitude: 486252239,
153            longitude: 22409705,
154            ..Default::default() // no altitude
155        };
156        assert_eq!(position.get_distance(&other_position), 31);
157    }
158
159    #[test]
160    fn calc_bearing_montlhery1_to_montlhery2() {
161        let montlhery_1 = Location::new(48.6250323, 2.2412096);
162        let montlhery_2 = Location::new(48.6249755, 2.2412662);
163
164        assert_eq!(
165            "146.63",
166            format!("{:.2}", montlhery_1.calc_bearing_to(&montlhery_2))
167        );
168        assert_eq!(
169            "142.57",
170            format!(
171                "{:.2}",
172                montlhery_1.estimate_bearing_to(&montlhery_2, 69.0, 53.0)
173            )
174        );
175    }
176
177    #[test]
178    fn calc_bearing_montlhery3_to_montlhery4() {
179        let montlhery_3 = Location::new(48.6234734, 2.2397949);
180        let montlhery_4 = Location::new(48.6234641, 2.2398374);
181
182        assert_eq!(
183            "108.32",
184            format!("{:.2}", montlhery_3.calc_bearing_to(&montlhery_4))
185        );
186        assert_eq!(
187            "105.90",
188            format!(
189                "{:.2}",
190                montlhery_3.estimate_bearing_to(&montlhery_4, 69.0, 53.0)
191            )
192        );
193    }
194
195    #[test]
196    fn get_heading_montlhery3_to_montlhery4() {
197        let montlhery_3 = ReferencePosition {
198            latitude: 486234734,
199            longitude: 22397949,
200            altitude: 15710,
201        };
202        let montlhery_4 = ReferencePosition {
203            latitude: 486234641,
204            longitude: 22398374,
205            altitude: 15773,
206        };
207        assert_eq!(1083, montlhery_3.get_heading(&montlhery_4));
208    }
209
210    #[test]
211    fn calc_bearing_boulder_to_dia() {
212        // 39.8617° N, 104.6731° W
213        let dia = Location::new(39.8617, -104.6731);
214
215        // 40.0274° N, 105.2519° W
216        let boulder = Location::new(40.0274, -105.2519);
217
218        assert_eq!("110.48", format!("{:.*}", 2, boulder.calc_bearing_to(&dia)));
219        assert_eq!(
220            "110.44",
221            format!("{:.*}", 2, boulder.estimate_bearing_to(&dia, 69.0, 53.0))
222        );
223    }
224
225    #[test]
226    fn it_can_get_south_destination() {
227        let position = teqmo_lane_merge_reference_postion();
228        // I take a point at 100 meters on south
229        let other_position = ReferencePosition {
230            latitude: 486235877,
231            longitude: position.longitude,
232            ..Default::default() // no altitude
233        };
234        assert_eq!(position.get_destination(100.0, 180.0), other_position);
235        assert_eq!(position.get_destination(100.0, -180.0), other_position);
236    }
237
238    #[test]
239    fn it_can_get_north_destination() {
240        let position = teqmo_lane_merge_reference_postion();
241        // I take a point at 100 meters on north
242        let other_position = ReferencePosition {
243            latitude: 486253862,
244            longitude: position.longitude,
245            ..Default::default() // no altitude
246        };
247        assert_eq!(position.get_destination(100.0, 0.0), other_position);
248        assert_eq!(position.get_destination(100.0, 360.0), other_position);
249    }
250
251    #[test]
252    fn it_can_get_east_destination() {
253        let position = teqmo_lane_merge_reference_postion();
254        // I take a point at 100 meters on south
255        let other_position = ReferencePosition {
256            latitude: position.latitude,
257            longitude: 22449934,
258            ..Default::default() // no altitude
259        };
260        assert_eq!(position.get_destination(100.0, 90.0), other_position);
261        assert_eq!(position.get_destination(100.0, -270.0), other_position);
262    }
263
264    #[test]
265    fn it_can_get_west_destination() {
266        let position = teqmo_lane_merge_reference_postion();
267        // I take a point at 100 meters on south
268        let other_position = ReferencePosition {
269            latitude: position.latitude,
270            longitude: 22422805,
271            ..Default::default() // no altitude
272        };
273        assert_eq!(position.get_destination(100.0, 270.0), other_position);
274        assert_eq!(position.get_destination(100.0, -90.0), other_position);
275    }
276}
libits_client/reception/exchange/reference_position.rs

libits_client/reception/exchange/
reference_position.rs
