c_its_parser/
geo_utils.rs

1//! Conversions from ETSI to geo-types
2//!
3//! - Positions are converted to [`geo_types::Point`]
4//! - Paths ([`PathHistory`](`crate::standards::cdd_1_3_1_1::its_container::PathHistory`)/ [`Path`](`crate::standards::cdd_2_2_1::etsi_its_cdd::Path`))
5//!   and MAPEM lanes [`NodeSetXY`](`crate::standards::dsrc_2_2_1::etsi_its_dsrc::NodeSetXY`) are converted to [`geo_types::LineString`]
6//!
7//! Take a look at the individual data types in [`crate::standards`] to discover available conversion methods and initialization functions.
8//!
9//! Note: These conversions are only available with the optional `geo` feature flag.
10//! And when used in a `no_std` environment, the `libm` flags is required for conversions from X/Y delta positions.
11
12#[cfg(all(feature = "libm", not(feature = "std")))]
13use num_traits::float::Float;
14
15const EARTH_CIRCUMFERENCE: f32 = 39_940_653.; // Earth's circumference in meters
16
17// generic conversion helpers
18
19/// Convert ETSI XY deltas to absolute lon/lat position (in degrees)
20///
21/// X points east (longitude), Y points north (latitude)!
22#[must_use]
23pub fn point_from_dxy(dx: f32, dy: f32, ref_pos: &geo_types::Point) -> geo_types::Point {
24    dxy_to_geo(dx, dy, ref_pos).into()
25}
26
27// used by DSRC 2.2.1
28#[cfg(feature = "_dsrc_2_2_1")]
29impl From<crate::standards::dsrc_2_2_1::etsi_its_dsrc::Position3D> for geo_types::Point {
30    fn from(other: crate::standards::dsrc_2_2_1::etsi_its_dsrc::Position3D) -> Self {
31        geo_types::Point::new(other.long.as_deg(), other.lat.as_deg())
32    }
33}
34#[cfg(feature = "_dsrc_2_2_1")]
35impl From<geo_types::Point> for crate::standards::dsrc_2_2_1::etsi_its_dsrc::Position3D {
36    fn from(other: geo_types::Point) -> Self {
37        use crate::standards::cdd_2_2_1::etsi_its_cdd::{Latitude, Longitude};
38
39        Self {
40            lat: Latitude::from_deg(other.y()),
41            long: Longitude::from_deg(other.x()),
42            elevation: None,
43            regional: None,
44        }
45    }
46}
47
48/// convert ETSI ReferencePosition to [`geo_types::Point`]
49#[cfg(any(feature = "_cdd_1_3_1_1", feature = "_cdd_2_2_1"))]
50macro_rules! refpos_to_point {
51    ($t:ty) => {
52        impl From<$t> for geo_types::Point {
53            fn from(other: $t) -> Self {
54                geo_types::Point::new(other.longitude.as_deg(), other.latitude.as_deg())
55            }
56        }
57    };
58}
59
60// used by CDD 1.3.1 (DENM 1.3.1, CAM 1.4.1, CPM 1)
61#[cfg(feature = "_cdd_1_3_1_1")]
62refpos_to_point!(crate::standards::cdd_1_3_1_1::its_container::ReferencePosition);
63
64// used by CDD 2.2.1 (DENM 2.2.1, CPM 2.1.1)
65#[cfg(feature = "_cdd_2_2_1")]
66refpos_to_point!(crate::standards::cdd_2_2_1::etsi_its_cdd::ReferencePosition);
67
68// ETSI PathHistory
69
70#[cfg(any(feature = "_cdd_1_3_1_1", feature = "denm_2_2_1"))]
71macro_rules! ph_to_line_string {
72    ($t:ty) => {
73        impl $t {
74            /// Resolve delta positions to absolute geo positions
75            ///
76            /// Output geo positions are in degrees lon/lat starting with the reference position.
77            ///
78            /// Input data is assumed to be in ETSI format. This means that the first point is
79            /// relative to the `ref_pos` and subsequent points are relative to the point before.
80            #[allow(
81                clippy::missing_panics_doc,
82                reason = "panic on last().unwrap() is impossible here"
83            )]
84            #[must_use]
85            pub fn to_line_string(&self, ref_pos: geo_types::Point) -> geo_types::LineString {
86                self.0
87                    .iter()
88                    .fold(alloc::vec![ref_pos], |mut acc, pt| {
89                        // vector is primed with one element, so should never return None
90                        let origin = acc.last().unwrap();
91                        let delta = geo_types::Point::new(
92                            pt.path_position.delta_longitude.as_deg(),
93                            pt.path_position.delta_latitude.as_deg(),
94                        );
95
96                        acc.push(*origin + delta);
97                        acc
98                    })
99                    .into()
100            }
101        }
102    };
103}
104
105// used by CDD 1.3.1 (CAM 1.4.1 and DENM 1.3.1)
106#[cfg(feature = "_cdd_1_3_1_1")]
107ph_to_line_string!(crate::standards::cdd_1_3_1_1::its_container::PathHistory);
108
109// used by CDD 2.2.1 (DENM 2.1.1)
110#[cfg(feature = "denm_2_2_1")]
111ph_to_line_string!(crate::standards::cdd_2_2_1::etsi_its_cdd::Path);
112
113// Used by DSRC 2.2.1 (MAPEM lanes)
114#[cfg(feature = "mapem_2_2_1")]
115impl crate::standards::dsrc_2_2_1::etsi_its_dsrc::NodeSetXY {
116    /// Resolve delta positions to absolute geo positions
117    ///
118    /// Output geo positions are in degrees lon/lat excluding the reference position.
119    ///
120    /// Input data is assumed to be in ETSI format. This means that the first point is
121    /// relative to the `ref_pos` and subsequent points are relative to the point before.
122    #[allow(
123        clippy::missing_panics_doc,
124        reason = "panic on last().unwrap() is impossible here"
125    )]
126    #[must_use]
127    pub fn to_line_string(&self, ref_pos: geo_types::Point) -> geo_types::LineString {
128        let mut path = self.0.iter().fold(alloc::vec![ref_pos], |mut acc, pt| {
129            // vector is primed with one element, so should never return None
130            let origin = acc.last().unwrap();
131            let pos = pt.delta.to_geo(origin);
132
133            acc.push(pos);
134            acc
135        });
136        path.remove(0); // remove ref_pos again
137
138        path.into()
139    }
140}
141
142#[cfg(feature = "mapem_2_2_1")]
143impl crate::standards::dsrc_2_2_1::etsi_its_dsrc::NodeOffsetPointXY {
144    /// Convert to absolute lon/lat (in degrees)
145    ///
146    /// [`NodeOffsetPointXY`](`Self`) may contain XY delta positions or absolute lat/lon positions.
147    /// XY positions will be converted to delta geo-coordinates near the `ref_pos`.
148    #[must_use]
149    pub fn to_geo(&self, ref_pos: &geo_types::Point) -> geo_types::Point {
150        use crate::standards::dsrc_2_2_1::etsi_its_dsrc::NodeOffsetPointXY;
151
152        let (lon, lat) = match self {
153            NodeOffsetPointXY::node_XY1(etsi) => {
154                dxy_to_geo((&etsi.x).into(), (&etsi.y).into(), ref_pos)
155            }
156            NodeOffsetPointXY::node_XY2(etsi) => {
157                dxy_to_geo((&etsi.x).into(), (&etsi.y).into(), ref_pos)
158            }
159            NodeOffsetPointXY::node_XY3(etsi) => {
160                dxy_to_geo((&etsi.x).into(), (&etsi.y).into(), ref_pos)
161            }
162            NodeOffsetPointXY::node_XY4(etsi) => {
163                dxy_to_geo((&etsi.x).into(), (&etsi.y).into(), ref_pos)
164            }
165            NodeOffsetPointXY::node_XY5(etsi) => {
166                dxy_to_geo((&etsi.x).into(), (&etsi.y).into(), ref_pos)
167            }
168            NodeOffsetPointXY::node_XY6(etsi) => {
169                dxy_to_geo((&etsi.x).into(), (&etsi.y).into(), ref_pos)
170            }
171            NodeOffsetPointXY::node_LatLon(etsi) => (etsi.lon.as_deg(), etsi.lat.as_deg()),
172            NodeOffsetPointXY::regional(_) => (0., 0.),
173        };
174
175        geo_types::Point::new(lon, lat)
176    }
177
178    /// Convert to delta distance (in meters)
179    ///
180    /// [`NodeOffsetPointXY`](`Self`) may contain XY delta positions or absolute lat/lon positions.
181    /// lat/lon delta coordinates will be converted to XY offsets near the `ref_pos`.
182    ///
183    /// Output will be according to ETSI coordinate system (X pointing east, X pointing north)!
184    #[must_use]
185    pub fn to_ddist(&self, ref_pos: &geo_types::Point) -> geo_types::Point {
186        use crate::standards::dsrc_2_2_1::etsi_its_dsrc::NodeOffsetPointXY;
187
188        let (x, y) = match self {
189            NodeOffsetPointXY::node_XY1(etsi) => ((&etsi.x).into(), (&etsi.y).into()),
190            NodeOffsetPointXY::node_XY2(etsi) => ((&etsi.x).into(), (&etsi.y).into()),
191            NodeOffsetPointXY::node_XY3(etsi) => ((&etsi.x).into(), (&etsi.y).into()),
192            NodeOffsetPointXY::node_XY4(etsi) => ((&etsi.x).into(), (&etsi.y).into()),
193            NodeOffsetPointXY::node_XY5(etsi) => ((&etsi.x).into(), (&etsi.y).into()),
194            NodeOffsetPointXY::node_XY6(etsi) => ((&etsi.x).into(), (&etsi.y).into()),
195            NodeOffsetPointXY::node_LatLon(etsi) => latlon_to_dcart(etsi, ref_pos),
196            NodeOffsetPointXY::regional(_) => (0., 0.),
197        };
198
199        geo_types::Point::new(x.into(), y.into())
200    }
201}
202
203#[cfg(any(feature = "std", feature = "libm"))]
204/// Convert ETSI XY deltas to absolute lon/lat position (in degrees)
205///
206/// X points east (longitude), Y points north (latitude)!
207fn dxy_to_geo(dx: f32, dy: f32, ref_pos: &geo_types::Point) -> (f64, f64) {
208    // latitude degree per meter is independent from longitude
209    let dlat = f64::from(dy) / f64::from(EARTH_CIRCUMFERENCE) * 360.;
210
211    // longitude degree per meter has a different value depending on the latitude
212    let ref_lat_rad = ref_pos.to_radians().y();
213    let dlon = f64::from(dx) / (f64::from(EARTH_CIRCUMFERENCE) * ref_lat_rad.cos()) * 360.;
214
215    (ref_pos.x() + dlon, ref_pos.y() + dlat)
216}
217
218/// Convert absolute lon/lat position to ETSI XY position (in meters)
219///
220/// X points east (longitude), Y points north (latitude)!
221#[cfg(all(feature = "mapem_2_2_1", any(feature = "std", feature = "libm")))]
222fn latlon_to_dcart(
223    dpos: &crate::standards::dsrc_2_2_1::etsi_its_dsrc::NodeLLmD64b,
224    ref_pos: &geo_types::Point,
225) -> (f32, f32) {
226    // latitude degree per meter is independent from longitude
227    #[allow(clippy::cast_possible_truncation)]
228    let dlat_deg = dpos.lat.as_deg() as f32 - ref_pos.y() as f32;
229    let dy = dlat_deg / 360. * EARTH_CIRCUMFERENCE;
230
231    // longitude degree per meter has a different value depending on the latitude
232    #[allow(clippy::cast_possible_truncation)]
233    let ref_lat_rad = ref_pos.to_radians().y() as f32;
234    #[allow(clippy::cast_possible_truncation)]
235    let dlon_deg = dpos.lon.as_deg() as f32 - ref_pos.x() as f32;
236    let dx = dlon_deg / 360. * (EARTH_CIRCUMFERENCE * ref_lat_rad.cos());
237
238    (dx, dy)
239}
240
241#[cfg(feature = "cpm_1")]
242impl crate::standards::cpm_1::cpm_pdu_descriptions::NodeOffsetPointZ {
243    #[must_use]
244    pub fn to_meters(&self) -> f32 {
245        use crate::standards::cpm_1::cpm_pdu_descriptions::NodeOffsetPointZ;
246
247        match self {
248            NodeOffsetPointZ::node_Z1(etsi) => etsi.into(),
249            NodeOffsetPointZ::node_Z2(etsi) => etsi.into(),
250            NodeOffsetPointZ::node_Z3(etsi) => etsi.into(),
251            NodeOffsetPointZ::node_Z4(etsi) => etsi.into(),
252            NodeOffsetPointZ::node_Z5(etsi) => etsi.into(),
253            NodeOffsetPointZ::node_Z6(etsi) => etsi.into(),
254        }
255    }
256}
257
258#[cfg(test)]
259mod tests {
260    #[test]
261    fn dxy_to_geo_test() {
262        use crate::geo_utils::dxy_to_geo;
263
264        let ref_pos = geo_types::point! {x: 9.936_521, y: 53.550_728};
265
266        // trivial test
267        assert_eq!((ref_pos.x(), ref_pos.y()), dxy_to_geo(0., 0., &ref_pos));
268
269        // 1/10th nautical mile north/ south
270        let (dlon, dlat) = dxy_to_geo(0., 185., &ref_pos);
271        assert_float_eq::assert_float_absolute_eq!(ref_pos.x() + 0., dlon);
272        assert_float_eq::assert_float_absolute_eq!(ref_pos.y() + 0.001_667, dlat);
273
274        let (dlon, dlat) = dxy_to_geo(0., -185., &ref_pos);
275        assert_float_eq::assert_float_absolute_eq!(ref_pos.x() + 0., dlon);
276        assert_float_eq::assert_float_absolute_eq!(ref_pos.y() + -0.001_667, dlat);
277
278        // east/west from online calculation tool
279        let (dlon, dlat) = dxy_to_geo(66., 0., &ref_pos);
280        assert_float_eq::assert_float_absolute_eq!(ref_pos.x() + 0.001_001, dlon);
281        assert_float_eq::assert_float_absolute_eq!(ref_pos.y() + 0., dlat);
282
283        // combine both
284        let (dlon, dlat) = dxy_to_geo(66., 185., &ref_pos);
285        assert_float_eq::assert_float_absolute_eq!(ref_pos.x() + 0.001_001, dlon);
286        assert_float_eq::assert_float_absolute_eq!(ref_pos.y() + 0.001_667, dlat);
287    }
288
289    #[test]
290    #[cfg(feature = "mapem_2_2_1")]
291    fn nodeset_to_line_string() {
292        use crate::standards::cdd_2_2_1::etsi_its_cdd::{Latitude, Longitude};
293        use crate::standards::dsrc_2_2_1::etsi_its_dsrc::{
294            NodeLLmD64b,
295            NodeOffsetPointXY,
296            NodeSetXY,
297            NodeXY,
298            NodeXY32b,
299            OffsetB16,
300        };
301
302        let ref_pos = geo_types::point! {x: 9.936_521, y: 53.550_728};
303
304        // latlon test
305        {
306            let point1 = NodeLLmD64b::new(
307                Longitude::from_deg(ref_pos.x() + 0.001),
308                Latitude::from_deg(ref_pos.y() + 0.005),
309            );
310            let point2 = NodeLLmD64b::new(
311                Longitude::from_deg(ref_pos.x() + -0.042),
312                Latitude::from_deg(ref_pos.y() + 0.),
313            );
314            let nodes = alloc::vec![
315                NodeXY::new(NodeOffsetPointXY::node_LatLon(point1), None),
316                NodeXY::new(NodeOffsetPointXY::node_LatLon(point2), None),
317            ];
318
319            let geo_nodes = NodeSetXY(nodes).to_line_string(ref_pos).into_points();
320            let geo_point1 = geo_types::Point::new(9.936_521 + 0.001, 53.550_728 + 0.005);
321            assert_float_eq::assert_float_absolute_eq!(geo_point1.x(), geo_nodes[0].x());
322            assert_float_eq::assert_float_absolute_eq!(geo_point1.y(), geo_nodes[0].y());
323            let geo_point2 = geo_types::Point::new(9.936_521 - 0.042, 53.550_728 + 0.);
324            assert_float_eq::assert_float_absolute_eq!(geo_point2.x(), geo_nodes[1].x());
325            assert_float_eq::assert_float_absolute_eq!(geo_point2.y(), geo_nodes[1].y());
326        }
327
328        // delta X/Y test
329        {
330            let point1 = NodeXY32b::new(
331                OffsetB16::from_meters(0.).unwrap(),
332                OffsetB16::from_meters(185.).unwrap(),
333            );
334            let point2 = NodeXY32b::new(
335                OffsetB16::from_meters(66.).unwrap(),
336                OffsetB16::from_meters(0.).unwrap(),
337            );
338            let nodes = alloc::vec![
339                NodeXY::new(NodeOffsetPointXY::node_XY6(point1), None),
340                NodeXY::new(NodeOffsetPointXY::node_XY6(point2), None),
341            ];
342
343            let geo_nodes = NodeSetXY(nodes).to_line_string(ref_pos).into_points();
344            let geo_point1 = geo_types::Point::new(9.936_521, 53.550_728 + 0.001_667);
345            assert_float_eq::assert_float_absolute_eq!(geo_point1.x(), geo_nodes[0].x());
346            assert_float_eq::assert_float_absolute_eq!(geo_point1.y(), geo_nodes[0].y());
347
348            let geo_point2 = geo_types::Point::new(9.936_521 + 0.001_001, 53.550_728 + 0.001_667);
349            assert_float_eq::assert_float_absolute_eq!(geo_point2.x(), geo_nodes[1].x());
350            assert_float_eq::assert_float_absolute_eq!(geo_point2.y(), geo_nodes[1].y());
351        }
352    }
353}
c_its_parser/geo_utils.rs

c_its_parser/
geo_utils.rs
