geoconvert/
utility.rs

1use crate::ThisOrThat;
2
3#[allow(dead_code)]
4pub(crate) mod dms {
5    /// Degrees per quarter turn
6    pub const QD: i32 = 90;
7    /// Minutes per degree
8    pub const DM: i32 = 60;
9    /// Seconds per minute
10    pub const MS: i32 = 60;
11    /// Degrees per half turn
12    pub const HD: i32 = 2 * QD;
13    /// Degrees per turn
14    pub const TD: i32 = 2 * HD;
15    /// Seconds per degree
16    pub const DS: i32 = DM * MS;
17}
18
19fn special_sum(u: f64, v: f64) -> (f64, f64) {
20    let s = u + v;
21    let up = s - v;
22    let vpp = s - up;
23
24    let up = up - u;
25    let vpp = vpp - v;
26
27    let t = s.is_zero().ternary_lazy(||s, || -(up + vpp));
28
29    (s, t)
30}
31
32/// Evaluate a polynomial
33pub(crate) fn polyval(p: &[f64], x: f64) -> f64 {
34    p
35        .iter()
36        .fold(0_f64, |acc, val| acc*x + val)
37}
38
39pub(crate) trait GeoMath {
40    fn is_zero(&self) -> bool;
41    fn eps_eq(&self, other: Self) -> bool;
42    fn ang_normalize(&self) -> Self;
43    fn ang_diff(&self, other: Self) -> Self;
44    fn eatanhe(&self, es: Self) -> Self;
45    fn remainder(&self, denom: Self) -> Self;
46    fn taupf(&self, es: Self) -> Self;
47    fn tauf(&self, es: Self) -> Self;
48}
49
50impl GeoMath for f64 {
51    fn is_zero(&self) -> bool {
52        self.abs() < f64::EPSILON
53    }
54
55    fn eps_eq(&self, other: f64) -> bool {
56        (*self - other).abs() < f64::EPSILON
57    }
58
59    fn ang_normalize(&self) -> f64 {
60        let value = self.remainder(f64::from(dms::TD));
61        let hd = f64::from(dms::HD);
62
63        if value.abs().eps_eq(hd) {
64            hd.copysign(*self)
65        }
66        else {
67            value
68        }
69    }
70
71    fn ang_diff(&self, other: f64) -> f64 {
72        let td = f64::from(dms::TD);
73        // Use remainder instead of AngNormalize, since we treat boundary cases
74        // later taking account of the error
75        let (diff, err) = special_sum((-*self).remainder(td), other % td);
76        // This second sum can only change d if abs(d) < 128, so don't need to
77        // apply remainder yet again.
78        let (diff, err) = special_sum(diff.remainder(td), err);
79        
80        let hd = f64::from(dms::HD);
81        // Fix the sign if d = -180, 0, 180.
82        if diff.is_zero() || diff.abs().eps_eq(hd) {
83            // If e == 0, take sign from y - x
84            // else (e != 0, implies d = +/-180), d and e must have opposite signs
85            let sign = if err.is_zero() { other - *self } else { -err };
86            diff.copysign(sign)
87        }
88        else {
89            diff
90        }
91    }
92
93    fn eatanhe(&self, es: f64) -> f64 {
94        if es.is_sign_positive() {
95            es * (es * *self).atanh()
96        } else {
97            -es * (es * *self).atanh()
98        }
99    }
100
101    fn remainder(&self, denom: Self) -> Self {
102        *self - (*self / denom).round() * denom
103    }
104
105    fn taupf(&self, es: f64) -> f64 {
106        let tau1 = 1.0_f64.hypot(*self);
107        let sig = (*self / tau1).eatanhe(es).sinh();
108
109        1.0_f64.hypot(sig) * *self - sig * tau1
110    }
111
112    #[allow(clippy::similar_names)]
113    fn tauf(&self, es: f64) -> f64 {
114        let numit = 5;
115        let tol = f64::EPSILON.sqrt() / 10.0;
116
117        let e2m = 1.0 - es.powi(2);
118        let mut tau = if self.abs() > 70.0 {
119            self * 1_f64.eatanhe(es).exp()
120        } else {
121            self / e2m
122        };
123
124        let stol = tol * self.abs().max(1.0);
125        for _ in 0..numit {
126            let taupa = tau.taupf(es);
127            let dtau = (self - taupa) * (1.0 + e2m * tau.powi(2))
128                / (e2m * 1.0_f64.hypot(tau) * 1.0_f64.hypot(taupa));
129            tau += dtau;
130            if dtau.abs() < stol {
131                break;
132            }
133        }
134        tau
135    }
136}
geoconvert/utility.rs

geoconvert/
utility.rs
