dis_rs/
utils.rs

1struct EcefToGeoConstants;
2
3#[allow(clippy::excessive_precision)]
4impl EcefToGeoConstants {
5    const WGS_84_SEMI_MAJOR_AXIS: f64 = 6_378_137.0; //WGS-84 semi-major axis
6    const E2: f64 = 6.694_379_990_137_799_7e-3; // WGS-84 first eccentricity squared
7    const A1: f64 = 4.269_767_270_715_753_5e+4; //a1 = a*e2
8    const A2: f64 = 1.823_091_254_607_545_5e+9; //a2 = a1*a1
9    const A3: f64 = 1.429_172_228_981_241_3e+2; //a3 = a1*e2/2
10    const A4: f64 = 4.557_728_136_518_863_7e+9; //a4 = 2.5*a2
11    const A5: f64 = 4.284_058_993_005_565_9e+4; //a5 = a1+a3
12    const A6: f64 = 9.933_056_200_098_622_0e-1; //a6 = 1-e2
13}
14
15/// Applies Geocentric (ECEF) to Geodetic (LLA) conversion
16///
17/// ECEF input parameters are in meters.
18/// Return value of consists of a tuple `(lat, lon, alt)`, where the ``lat`` and ``lon`` are in radians, ``altitude`` is in meters (MSL).
19///
20/// Adapted from <https://danceswithcode.net/engineeringnotes/geodetic_to_ecef/geodetic_to_ecef.html>
21#[must_use]
22#[allow(clippy::many_single_char_names)]
23pub fn ecef_to_geodetic_lla(ecef_x: f64, ecef_y: f64, ecef_z: f64) -> (f64, f64, f64) {
24    // TODO handle special case for centre of earth, where lat/lon are ignored (CDIS 7.1 ad. c).
25    let zp = ecef_z.abs();
26    let w2 = ecef_x * ecef_x + ecef_y * ecef_y;
27    let w = w2.sqrt();
28    let r2 = w2 + ecef_z * ecef_z;
29    let r = r2.sqrt();
30    let longitude = ecef_y.atan2(ecef_x);
31
32    let s2 = ecef_z * ecef_z / r2;
33    let c2 = w2 / r2;
34    let u = EcefToGeoConstants::A2 / r;
35    let v = EcefToGeoConstants::A3 - EcefToGeoConstants::A4 / r;
36    let (latitude, s, ss, c) = if c2 > 0.3 {
37        let s = (zp / r) * (1.0 + c2 * (EcefToGeoConstants::A1 + u + s2 * v) / r);
38        let latitude = s.asin(); //Lat
39        let ss = s * s;
40        let c = (1.0 - ss).sqrt();
41        (latitude, s, ss, c)
42    } else {
43        let c = (w / r) * (1.0 - s2 * (EcefToGeoConstants::A5 - u - c2 * v) / r);
44        let latitude = c.acos(); //Lat
45        let ss = 1.0 - c * c;
46        let s = ss.sqrt();
47        (latitude, s, ss, c)
48    };
49
50    let g = 1.0 - EcefToGeoConstants::E2 * ss;
51    let rg = EcefToGeoConstants::WGS_84_SEMI_MAJOR_AXIS / g.sqrt();
52    let rf = EcefToGeoConstants::A6 * rg;
53    let u = w - rg * c;
54    let v = zp - rf * s;
55    let f = c * u + s * v;
56    let m = c * v - s * u;
57    let p = m / (rf / g + f);
58    let latitude = latitude + p; //Lat
59    let altitude = f + m * p / 2.0; //Altitude
60    let latitude = if ecef_z < 0.0 {
61        latitude * -1.0 //Lat
62    } else {
63        latitude
64    };
65
66    (latitude, longitude, altitude)
67}
68
69/// Applies Geodetic (LLA) to Geocentric (ECEF) conversion
70///
71/// Geodetic input parameters are in meters.
72/// Return value consists of a tuple `(lat, lon, alt)`, where the ``lat`` and ``lon`` are in _radians_, ``altitude`` is in _meters_ (MSL).
73///
74/// Adapted from <https://danceswithcode.net/engineeringnotes/geodetic_to_ecef/geodetic_to_ecef.html>
75#[must_use]
76pub fn geodetic_lla_to_ecef(latitude: f64, longitude: f64, altitude_msl: f64) -> (f64, f64, f64) {
77    let n = EcefToGeoConstants::WGS_84_SEMI_MAJOR_AXIS
78        / (1.0 - EcefToGeoConstants::E2 * latitude.sin() * latitude.sin()).sqrt();
79    let ecef_x = (n + altitude_msl) * latitude.cos() * longitude.cos(); //ECEF x
80    let ecef_y = (n + altitude_msl) * latitude.cos() * longitude.sin(); //ECEF y
81    let ecef_z = (n * (1.0 - EcefToGeoConstants::E2) + altitude_msl) * latitude.sin(); //ECEF z
82
83    (ecef_x, ecef_y, ecef_z)
84}
dis_rs/utils.rs

dis_rs/
utils.rs
