satkit/
itrfcoord.rs

Help
1use std::f64::consts::PI;
2
3use crate::consts::WGS84_A;
4use crate::consts::WGS84_F;
5
6use nalgebra as na;
7
8use crate::types::Quaternion as Quat;
9use crate::types::Vec3;
10
11use anyhow::Result;
12
13///
14/// Representation of a coordinate in the
15/// International Terrestrial Reference Frame (ITRF)
16///
17/// This coordinate object can be created from and also
18/// output to Geodetic coordinates (latitude, longitude,
19/// height above ellipsoid).
20///
21/// Functions are also available to provide rotation
22/// quaternions to the East-North-Up frame
23/// and North-East-Down frame at this coordinate
24///
25#[derive(PartialEq, PartialOrd, Copy, Clone, Debug)]
26pub struct ITRFCoord {
27    pub itrf: Vec3,
28}
29
30impl std::fmt::Display for ITRFCoord {
31    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
32        let (lat, lon, hae) = self.to_geodetic_deg();
33        write!(
34            f,
35            "ITRFCoord(lat: {:8.4} deg, lon: {:8.4} deg, altitude: {:5.2} km)",
36            lat,
37            lon,
38            hae / 1.0e3
39        )
40    }
41}
42
43impl std::ops::Add<Vec3> for ITRFCoord {
44    type Output = Self;
45    fn add(self, other: Vec3) -> Self::Output {
46        Self {
47            itrf: self.itrf + other,
48        }
49    }
50}
51
52impl std::ops::Add<Vec3> for &ITRFCoord {
53    type Output = ITRFCoord;
54    fn add(self, other: Vec3) -> Self::Output {
55        ITRFCoord {
56            itrf: self.itrf + other,
57        }
58    }
59}
60
61impl std::ops::Add<&Vec3> for ITRFCoord {
62    type Output = Self;
63    fn add(self, other: &Vec3) -> Self::Output {
64        Self {
65            itrf: self.itrf + other,
66        }
67    }
68}
69
70impl std::ops::Add<&Vec3> for &ITRFCoord {
71    type Output = ITRFCoord;
72    fn add(self, other: &Vec3) -> Self::Output {
73        ITRFCoord {
74            itrf: self.itrf + other,
75        }
76    }
77}
78
79impl std::ops::Sub<Vec3> for ITRFCoord {
80    type Output = Self;
81    fn sub(self, other: Vec3) -> Self::Output {
82        Self {
83            itrf: self.itrf - other,
84        }
85    }
86}
87
88impl std::ops::Sub<Self> for ITRFCoord {
89    type Output = Vec3;
90    fn sub(self, other: Self) -> Vec3 {
91        self.itrf - other.itrf
92    }
93}
94
95impl std::ops::Sub<ITRFCoord> for &ITRFCoord {
96    type Output = Vec3;
97    fn sub(self, other: ITRFCoord) -> Vec3 {
98        self.itrf - other.itrf
99    }
100}
101
102impl std::ops::Sub<&ITRFCoord> for &ITRFCoord {
103    type Output = Vec3;
104    fn sub(self, other: &ITRFCoord) -> Vec3 {
105        self.itrf - other.itrf
106    }
107}
108
109impl std::ops::Sub<&Self> for ITRFCoord {
110    type Output = Vec3;
111    fn sub(self, other: &Self) -> Vec3 {
112        self.itrf - other.itrf
113    }
114}
115
116impl std::convert::From<[f64; 3]> for ITRFCoord {
117    fn from(v: [f64; 3]) -> Self {
118        Self {
119            itrf: Vec3::from(v),
120        }
121    }
122}
123
124impl std::convert::From<&[f64]> for ITRFCoord {
125    fn from(v: &[f64]) -> Self {
126        assert!(v.len() == 3);
127        Self {
128            itrf: Vec3::from_row_slice(v),
129        }
130    }
131}
132
133impl std::convert::From<Vec3> for ITRFCoord {
134    fn from(v: Vec3) -> Self {
135        Self { itrf: v }
136    }
137}
138
139impl std::convert::From<ITRFCoord> for Vec3 {
140    fn from(itrf: ITRFCoord) -> Self {
141        itrf.itrf
142    }
143}
144
145impl ITRFCoord {
146    /// Returns an ITRF Coordinate given the geodetic inputs
147    ///   with degree units for latitude & longitude
148    ///
149    /// # Arguments:
150    ///
151    /// * `lat` - Geodetic latitude in degrees
152    /// * `lon` - Geodetic longitude in degrees
153    /// * `hae` - Height above ellipsoid, in meters
154    ///
155    /// # Examples:
156    /// ```
157    /// // Create coord for ~ Boston, MA
158    /// use satkit::itrfcoord::ITRFCoord;
159    /// let itrf = ITRFCoord::from_geodetic_deg(42.466, -71.1516, 150.0);
160    /// ```
161    ///
162    pub fn from_geodetic_deg(lat: f64, lon: f64, hae: f64) -> Self {
163        Self::from_geodetic_rad(lat.to_radians(), lon.to_radians(), hae)
164    }
165
166    ///
167    /// Returns an ITRF Coordinate given Cartesian ITRF coordinates
168    ///
169    /// # Arguments:
170    ///
171    /// * `v` - `nalgebra::Vector3<f64>` representing ITRF coordinates in meters
172    ///
173    /// # Examples:
174    ///
175    /// ```
176    /// // Create coord for ~ Boston, MA
177    /// use satkit::itrfcoord::ITRFCoord;
178    /// use nalgebra as na;
179    /// let itrf = ITRFCoord::from_vector(&na::Vector3::new(1522386.15660978, -4459627.78585002,  4284030.6890791));
180    /// ```
181    ///
182    ///
183    pub const fn from_vector(v: &na::Vector3<f64>) -> Self {
184        Self { itrf: *v }
185    }
186
187    /// Returns an ITRF Coordinate given Cartesian ITRF coordinates represented as a slice
188    ///
189    /// # Arguments:
190    ///
191    /// * `v` - Slice representing ITRF coordinates in meters
192    ///
193    /// # Examples:
194    ///
195    /// ```
196    /// // Create coord for ~ Boston, MA
197    /// use satkit::itrfcoord::ITRFCoord;
198    /// let itrf = ITRFCoord::from_slice(&[1522386.15660978, -4459627.78585002,  4284030.6890791]);
199    /// ```
200    ///
201    pub fn from_slice(v: &[f64]) -> Result<Self> {
202        if v.len() != 3 {
203            anyhow::bail!("Input slice must have 3 elements");
204        }
205        Ok(Self {
206            itrf: Vec3::from_row_slice(v),
207        })
208    }
209
210    /// Returns an ITRF Coordinate given the geodetic inputs
211    ///   with radian units for latitude & longitude
212    ///
213    /// # Arguments:
214    ///
215    /// * `lat` - Geodetic latitude in radians
216    /// * `lon` - Geodetic longitude in radians
217    /// * `hae` - Height above ellipsoid, in meters
218    ///
219    /// # Examples:
220    /// ```
221    /// // Create coord for ~ Boston, MA
222    /// use satkit::itrfcoord::ITRFCoord;
223    /// use std::f64::consts::PI;
224    /// const DEG2RAD: f64 = PI / 180.0;
225    /// let itrf = ITRFCoord::from_geodetic_rad(42.466*DEG2RAD, -71.1516*DEG2RAD, 150.0);
226    /// ```
227    ///
228    pub fn from_geodetic_rad(lat: f64, lon: f64, hae: f64) -> Self {
229        let sinp: f64 = lat.sin();
230        let cosp: f64 = lat.cos();
231        let sinl: f64 = lon.sin();
232        let cosl: f64 = lon.cos();
233
234        let f2 = (1.0 - WGS84_F).powi(2);
235        let c = 1.0 / cosp.mul_add(cosp, f2 * sinp * sinp).sqrt();
236        let s = f2 * c;
237
238        Self {
239            itrf: Vec3::from([
240                WGS84_A.mul_add(c, hae) * cosp * cosl,
241                WGS84_A.mul_add(c, hae) * cosp * sinl,
242                WGS84_A.mul_add(s, hae) * sinp,
243            ]),
244        }
245    }
246
247    /// Returns 3-element tuple representing geodetic coordinates
248    ///
249    /// # Tuple contents:
250    ///
251    /// * `.0` - latitude in radians
252    /// * `.1` - longitude in radians
253    /// * `.2` - height above ellipsoid, in meters
254    ///
255    pub fn to_geodetic_rad(&self) -> (f64, f64, f64) {
256        const B: f64 = WGS84_A * (1.0 - WGS84_F);
257        const E2: f64 = 1.0 - (1.0 - WGS84_F) * (1.0 - WGS84_F);
258        const EP2: f64 = E2 / (1.0 - E2);
259
260        let rho = self.itrf[0].hypot(self.itrf[1]);
261        let mut beta: f64 = f64::atan2(self.itrf[2], (1.0 - WGS84_F) * rho);
262        let mut sinbeta: f64 = beta.sin();
263        let mut cosbeta: f64 = beta.cos();
264        let mut phi: f64 = f64::atan2(
265            (B * EP2).mul_add(sinbeta.powi(3), self.itrf[2]),
266            (WGS84_A * E2).mul_add(-cosbeta.powi(3), rho),
267        );
268        let mut betanew: f64 = f64::atan2((1.0 - WGS84_F) * phi.sin(), phi.cos());
269        for _x in 0..5 {
270            beta = betanew;
271            sinbeta = beta.sin();
272            cosbeta = beta.cos();
273            phi = f64::atan2(
274                (B * EP2).mul_add(sinbeta.powi(3), self.itrf[2]),
275                (WGS84_A * E2).mul_add(-cosbeta.powi(3), rho),
276            );
277            betanew = f64::atan2((1.0 - WGS84_F) * phi.sin(), phi.cos());
278        }
279        let lat: f64 = phi;
280        let lon: f64 = f64::atan2(self.itrf[1], self.itrf[0]);
281        let sinphi: f64 = phi.sin();
282        let n: f64 = WGS84_A / (E2 * sinphi).mul_add(-sinphi, 1.0).sqrt();
283        let h = rho.mul_add(phi.cos(), (E2 * n).mul_add(sinphi, self.itrf[2]) * sinphi) - n;
284        (lat, lon, h)
285    }
286
287    /// Returns 3-element tuple representing geodetic coordinates
288    ///
289    /// # Tuple contents:
290    ///
291    /// * `.0` - latitude in degrees
292    /// * `.1` - longitude in degrees
293    /// * `.2` - height above ellipsoid, in meters
294    ///
295    pub fn to_geodetic_deg(&self) -> (f64, f64, f64) {
296        let (lat_rad, lon_rad, hae) = self.to_geodetic_rad();
297        (lat_rad.to_degrees(), lon_rad.to_degrees(), hae)
298    }
299
300    /// Return geodetic longitude in radians, [-π, π]
301    ///
302    #[inline]
303    pub fn longitude_rad(&self) -> f64 {
304        f64::atan2(self.itrf[1], self.itrf[0])
305    }
306
307    /// Return geodetic longitude in degrees, [-180, 180]
308    #[inline]
309    pub fn longitude_deg(&self) -> f64 {
310        self.longitude_rad().to_degrees()
311    }
312
313    /// return geodetic latitude in radians, [-π/2, π/2]
314    #[inline]
315    pub fn latitude_rad(&self) -> f64 {
316        let (lat, _a, _b) = self.to_geodetic_rad();
317        lat
318    }
319
320    /// Return height above ellipsoid in meters
321    #[inline]
322    pub fn hae(&self) -> f64 {
323        let (_a, _b, hae) = self.to_geodetic_rad();
324        hae
325    }
326
327    /// Return geodetic latitude in degrees, [-180, 180]
328    #[inline]
329    pub fn latitude_deg(&self) -> f64 {
330        self.latitude_rad().to_degrees()
331    }
332
333    /// Compute location when moving a given Distance at a given heading along the Earth's surface
334    /// Altitude assumed to be zero
335    ///
336    /// # Arguments:
337    /// * `distance_m` - Distance in meters to travel along surface of Earth
338    /// * `heading_rad` - Initial heading, in radians
339    ///
340    /// # Returns:
341    /// * ITRFCoord representing final position
342    ///
343    /// # References:
344    /// * Uses Vincenty's formula
345    ///   See: <https://en.wikipedia.org/wiki/Vincenty%27s_formulae>
346    ///
347    /// # Arguments:
348    ///
349    /// * `distance_m` - Distance in meters to travel along surface of Earth
350    /// * `heading_rad` - Initial heading, in radians
351    ///
352    /// # Returns:
353    ///
354    /// * ITRFCoord representing final position
355    ///
356    pub fn move_with_heading(&self, distance_m: f64, heading_rad: f64) -> Self {
357        let phi1 = self.latitude_rad();
358        #[allow(non_upper_case_globals)]
359        const a: f64 = WGS84_A;
360        #[allow(non_upper_case_globals)]
361        const b: f64 = (1.0 - WGS84_F) * WGS84_A;
362
363        let u1 = ((1.0 - WGS84_F) * phi1.tan()).atan();
364        let sigma1 = f64::atan2(u1.tan(), heading_rad.cos());
365        let sinalpha = u1.cos() * heading_rad.sin();
366        let usq = sinalpha.mul_add(-sinalpha, 1.0) * (a / b).mul_add(a / b, -1.0);
367        let big_a = (usq / 16384.0).mul_add(
368            usq.mul_add(usq.mul_add(175.0f64.mul_add(-usq, 320.0), -768.0), 4096.0),
369            1.0,
370        );
371        let big_b =
372            usq / 1024.0 * usq.mul_add(usq.mul_add(47.0f64.mul_add(-usq, 74.0), -128.0), 256.0);
373        let mut sigma = distance_m / b / big_a;
374        let mut costwosigmam = 0.0;
375        for _ in 0..5 {
376            costwosigmam = 2.0f64.mul_add(sigma1, sigma).cos();
377            let dsigma = big_b
378                * sigma.sin()
379                * (0.25 * big_b).mul_add(
380                    sigma.cos().mul_add(
381                        2.0f64.mul_add(costwosigmam.powi(2), -1.0),
382                        -(big_b / 6.0
383                            * costwosigmam
384                            * 4.0f64.mul_add(sigma.sin().powi(2), -3.0)
385                            * 4.0f64.mul_add(costwosigmam.powi(2), -3.0)),
386                    ),
387                    costwosigmam,
388                );
389            sigma = distance_m / b / big_a + dsigma;
390        }
391        let phi2 = f64::atan2(
392            u1.sin()
393                .mul_add(sigma.cos(), u1.cos() * sigma.sin() * heading_rad.cos()),
394            (1.0 - WGS84_F)
395                * sinalpha.hypot(
396                    u1.sin()
397                        .mul_add(sigma.sin(), -(u1.cos() * sigma.cos() * heading_rad.cos())),
398                ),
399        );
400        let lam = f64::atan2(
401            sigma.sin() * heading_rad.sin(),
402            u1.cos()
403                .mul_add(sigma.cos(), -(u1.sin() * sigma.sin() * heading_rad.cos())),
404        );
405        let cossqalpha = sinalpha.mul_add(-sinalpha, 1.0);
406        let big_c =
407            WGS84_F / 16.0 * cossqalpha * WGS84_F.mul_add(3.0f64.mul_add(-cossqalpha, 4.0), 4.0);
408        let delta_lon = ((1.0 - big_c) * WGS84_F * sinalpha).mul_add(
409            -(big_c * sigma.sin()).mul_add(
410                (big_c * sigma.cos())
411                    .mul_add(2.0f64.mul_add(costwosigmam.powi(2), -1.0), costwosigmam),
412                sigma,
413            ),
414            lam,
415        );
416        let lambda2 = delta_lon + self.longitude_rad();
417        Self::from_geodetic_rad(phi2, lambda2, 0.0)
418    }
419
420    /// Geodesic distance between two coordinates
421    ///
422    /// Return Geodesic distance (shortest distance along Earth's surface) in meters
423    /// between self and another ITRF coordinate
424    ///
425    /// Also returns initial and final heading
426    ///
427    /// # Arguments:
428    ///
429    /// * `other` - ITRF coordinate for which distance will be computed
430    ///
431    /// # Outputs:
432    ///   Tuple with following values
433    ///
434    /// * `0` - Distance in meters
435    /// * `1` - Starting heading (at self) in radians
436    /// * `2` - Final heading (at other) in radians
437    //
438    /// # References
439    //  * Vincenty's formula inverse
440    ///   See: <https://en.wikipedia.org/wiki/Vincenty%27s_formulae>
441    ///   See: <https://geodesyapps.ga.gov.au/vincenty-inverse>
442    ///
443    pub fn geodesic_distance(&self, other: &Self) -> (f64, f64, f64) {
444        #[allow(non_upper_case_globals)]
445        const a: f64 = WGS84_A;
446        #[allow(non_upper_case_globals)]
447        const b: f64 = (1.0 - WGS84_F) * WGS84_A;
448
449        let lata = self.latitude_rad();
450        let latb = other.latitude_rad();
451        let lona = self.longitude_rad();
452        let lonb = other.longitude_rad();
453        let u1 = ((1.0 - WGS84_F) * lata.tan()).atan();
454        let u2 = ((1.0 - WGS84_F) * latb.tan()).atan();
455        let lam = lonb - lona;
456        let londiff = lam;
457
458        let mut lam = lonb - lona;
459        let mut cossqalpha = 0.0;
460        let mut sinsigma = 0.0;
461        let mut cossigma = 0.0;
462        let mut cos2sm = 0.0;
463        let mut sigma = 0.0;
464        for _ in 0..5 {
465            sinsigma = (u2.cos() * lam.sin()).hypot(
466                u1.cos()
467                    .mul_add(u2.sin(), -(u1.sin() * u2.cos() * lam.cos())),
468            );
469            cossigma = u1.sin().mul_add(u2.sin(), u1.cos() * u2.cos() * lam.cos());
470            sigma = f64::atan2(sinsigma, cossigma);
471            let sinalpha = (u1.cos() * u2.cos() * lam.sin()) / sigma.sin();
472            cossqalpha = sinalpha.mul_add(-sinalpha, 1.0);
473            cos2sm = sigma.cos() - (2.0 * u1.sin() * u2.sin()) / cossqalpha;
474            let c = WGS84_F / 16.0
475                * cossqalpha
476                * WGS84_F.mul_add(3.0f64.mul_add(-cossqalpha, 4.0), 4.0);
477            lam = ((1.0 - c) * WGS84_F * sinalpha).mul_add(
478                (c * sinsigma).mul_add(
479                    (c * cossigma).mul_add(2.0f64.mul_add(cos2sm.powi(2), -1.0), cos2sm),
480                    sigma,
481                ),
482                londiff,
483            );
484        }
485
486        let usq = cossqalpha * (a / b).mul_add(a / b, -1.0);
487        let biga = (usq / 16384.0).mul_add(
488            usq.mul_add(usq.mul_add(175.0f64.mul_add(-usq, 320.0), -768.0), 4096.0),
489            1.0,
490        );
491        let bigb =
492            usq / 1024.0 * usq.mul_add(usq.mul_add(47.0f64.mul_add(-usq, 74.0), -128.0), 256.0);
493        let dsigma = bigb
494            * sinsigma
495            * (0.25 * bigb).mul_add(
496                cossigma.mul_add(
497                    2.0f64.mul_add(cos2sm.powi(2), -1.0),
498                    -(bigb / 6.0
499                        * cos2sm
500                        * 4.0f64.mul_add(sinsigma.powi(2), -3.0)
501                        * 4.0f64.mul_add(cos2sm.powi(2), -3.0)),
502                ),
503                cos2sm,
504            );
505        let s = b * biga * (sigma - dsigma);
506        let alpha1 = f64::atan2(
507            u2.cos() * lam.sin(),
508            u1.cos()
509                .mul_add(u2.sin(), -(u1.sin() * u2.cos() * lam.cos())),
510        );
511        let alpha2 = f64::atan2(
512            u1.cos() * lam.sin(),
513            (-u1.sin()).mul_add(u2.cos(), u1.cos() * u2.sin() * lam.cos()),
514        );
515        (s, alpha1, alpha2)
516    }
517
518    /// Return quaternion representing rotation from the
519    /// North-East-Down (NED) coordinate frame to the
520    /// ITRF coordinate frame
521    #[inline]
522    pub fn q_ned2itrf(&self) -> Quat {
523        let (lat, lon, _) = self.to_geodetic_rad();
524        Quat::from_axis_angle(&Vec3::z_axis(), lon)
525            * Quat::from_axis_angle(&Vec3::y_axis(), -lat - PI / 2.0)
526    }
527
528    /// Convert coordinate to a North-East-Down (NED)
529    /// coordinate relative to a reference coordinate
530    ///
531    /// # Arguments
532    ///
533    /// * `ref_coord`` - `&ITRFCoord`` representing reference
534    ///
535    /// # Return
536    ///
537    /// * `nalgebra::Vector3<f64>` representing NED position
538    ///   relative to reference.  Units are meters
539    ///
540    /// # Examples:
541    /// ```
542    /// use satkit::itrfcoord::ITRFCoord;
543    /// // Create coord
544    /// let itrf1 = ITRFCoord::from_geodetic_deg(42.466, -71.1516, 150.0);
545    /// // Crate 2nd coord 100 meters above
546    /// let itrf2 = ITRFCoord::from_geodetic_deg(42.466, -71.1516, 250.0);
547    ///
548    /// // Get NED of itrf1 relative to itrf2
549    /// let ned = itrf1.to_ned(&itrf2);
550    /// // Should return [0.0, 0.0, 100.0]
551    /// ```
552    ///
553    pub fn to_ned(&self, ref_coord: &Self) -> Vec3 {
554        self.q_ned2itrf().conjugate() * (self.itrf - ref_coord.itrf)
555    }
556
557    /// Return quaternion representing rotation from the
558    /// East-North-Up (ENU) coordinate frame to the
559    /// ITRF coordinate frame
560    pub fn q_enu2itrf(&self) -> Quat {
561        let (lat, lon, _) = self.to_geodetic_rad();
562        Quat::from_axis_angle(&Vec3::z_axis(), lon + PI / 2.0)
563            * Quat::from_axis_angle(&Vec3::x_axis(), PI / 2.0 - lat)
564    }
565
566    /// Convert coordinate to a East-North-Up (ENU)
567    /// coordinate relative to a reference coordinate
568    ///
569    /// # Arguments
570    ///
571    /// * ref_coord - &ITRFCoord representing reference
572    ///
573    /// # Return
574    ///
575    /// * `nalgebra::Vector3<f64>` representing ENU position
576    ///   relative to reference.  Units are meters
577    ///
578    /// # Examples:
579    /// ```
580    /// use satkit::itrfcoord::ITRFCoord;
581    /// // Create coord
582    /// let itrf1 = ITRFCoord::from_geodetic_deg(42.466, -71.1516, 150.0);
583    /// // Crate 2nd coord 100 meters above
584    /// let itrf2 = ITRFCoord::from_geodetic_deg(42.466, -71.1516, 250.0);
585    ///
586    /// // Get ENU of itrf1 relative to itrf2
587    /// let enu = itrf1.to_ned(&itrf2);
588    /// // Should return [0.0, 0.0, -100.0]
589    /// ```
590    ///
591    pub fn to_enu(&self, other: &Self) -> Vec3 {
592        self.q_enu2itrf().conjugate() * (self.itrf - other.itrf)
593    }
594}
595
596#[cfg(test)]
597mod tests {
598    use super::*;
599    use approx::{assert_abs_diff_eq, assert_relative_eq};
600
601    #[test]
602    fn test_geodesic() {
603        // Lets pick a random point and try it out...
604        let mumbai = ITRFCoord::from_geodetic_deg(19.16488608334183, 72.8314881731579, 0.0);
605        let dubai = ITRFCoord::from_geodetic_deg(25.207843059422945, 55.27053859644447, 0.0);
606        let (dist, h0, h1) = mumbai.geodesic_distance(&dubai);
607        // from google maps, distance is 1,926.80 km
608        // From <https://geodesyapps.ga.gov.au/vincenty-inverse>
609        // Distance = 1928536.609m
610        // Forward azimuth = 293.466588 deg
611        // Reverse azimuth = 106.780805 deg
612        assert_relative_eq!(dist, 1928536.609, max_relative = 1.0e-8);
613        assert_relative_eq!(
614            2.0f64.mul_add(PI, h0),
615            293.466588f64.to_radians(),
616            max_relative = 1.0e-6
617        );
618        assert_relative_eq!(h1 + PI, 106.780805f64.to_radians(), max_relative = 1.0e-6);
619
620        // Moving from Mumbai at the given distance and heading should get us to Dubai
621        let testpoint = mumbai.move_with_heading(dist, h0);
622
623        // Check differences
624        let delta = dubai - testpoint;
625        assert_abs_diff_eq!(delta.norm(), 0.0, epsilon = 1.0e-6);
626    }
627
628    #[test]
629    fn geodetic() {
630        let lat_deg: f64 = 42.466;
631        let lon_deg: f64 = -71.0;
632        let hae: f64 = 150.0;
633        let itrf = ITRFCoord::from_geodetic_deg(lat_deg, lon_deg, hae);
634        println!("{}", itrf);
635        // Check conversions
636        assert!(((lat_deg - 42.466) / 42.466).abs() < 1.0e-6);
637        assert!(((lon_deg + 71.0) / 71.0).abs() < 1.0e-6);
638        assert!(((hae - 150.0) / 150.0).abs() < 1.0e-6);
639    }
640
641    #[test]
642    fn test_ned_enu() {
643        let lat_deg: f64 = 42.466;
644        let lon_deg: f64 = -74.0;
645        let hae: f64 = 150.0;
646        let itrf1 = ITRFCoord::from_geodetic_deg(lat_deg, lon_deg, hae);
647        let itrf2 = ITRFCoord::from_geodetic_deg(lat_deg, lon_deg, hae + 100.0);
648        let ned = itrf2.to_ned(&itrf1);
649        let enu = itrf2.to_enu(&itrf1);
650        assert!(enu[0].abs() < 1.0e-6);
651        assert!(enu[1].abs() < 1.0e-6);
652        assert!(((enu[2] - 100.0) / 100.0).abs() < 1.0e-6);
653        assert!(ned[0].abs() < 1.0e-6);
654        assert!(ned[1].abs() < 1.0e-6);
655        assert!(((ned[2] + 100.0) / 100.0).abs() < 1.0e-6);
656
657        let dvec = Vec3::from([-100.0, -200.0, 300.0]);
658        let itrf3 = itrf2 + itrf2.q_ned2itrf() * dvec;
659        let nedvec = itrf3.to_ned(&itrf2);
660        let itrf4 = itrf2 + itrf2.q_enu2itrf() * dvec;
661        let enuvec = itrf4.to_enu(&itrf2);
662        for x in 0..3 {
663            assert!(((nedvec[x] - dvec[x]) / nedvec[x]).abs() < 1.0e-3);
664            assert!(((enuvec[x] - dvec[x]) / nedvec[x]).abs() < 1.0e-3);
665        }
666        /*
667        let q = Quat::from_axis_angle(&Vec3::z_axis(), -0.003);
668        println!("{}", q);
669        println!("{}", q.to_rotation_matrix());
670        */
671
672        let itrf1 = ITRFCoord::from_geodetic_deg(lat_deg, lon_deg, hae);
673        let itrf2 = itrf1 + itrf1.q_ned2itrf() * na::vector![0.0, 0.0, 10000.0];
674        println!("height diff = {}", itrf2.hae() - itrf1.hae());
675    }
676}
satkit/itrfcoord.rs

satkit/
itrfcoord.rs
