geometry_strategy/geographic/
direct_thomas.rs

1//! Thomas second-order solution of the direct geodesic problem.
2//!
3//! Mirrors `boost::geometry::formula::thomas_direct` from
4//! `formulas/thomas_direct.hpp:39-238`.
5
6use geometry_cs::Spheroid;
7
8use super::direct::DirectResult;
9
10#[cfg(feature = "std")]
11use super::direct::normalize_longitude;
12#[cfg(feature = "std")]
13use super::spheroid_calc::SpheroidCalc;
14
15/// Forsyth–Andoyer–Lambert direct approximation with Thomas's second-order
16/// terms.
17///
18/// Inputs and output angles are radians. Mirrors
19/// `formula::thomas_direct<CT, true>` from
20/// `formulas/thomas_direct.hpp:39-238`.
21#[derive(Debug, Clone, Copy)]
22pub struct ThomasDirect {
23    /// Reference ellipsoid.
24    pub spheroid: Spheroid,
25    /// Include Thomas's second-order correction terms.
26    pub second_order: bool,
27}
28
29impl ThomasDirect {
30    /// Second-order Thomas direct on WGS84.
31    pub const WGS84: Self = Self {
32        spheroid: Spheroid::WGS84,
33        second_order: true,
34    };
35
36    /// Solve the direct geodesic problem.
37    ///
38    /// Mirrors `thomas_direct::apply` from
39    /// `formulas/thomas_direct.hpp:66-205`, including its southward-azimuth
40    /// vertical flip and longitude normalization.
41    #[cfg(feature = "std")]
42    #[inline]
43    #[must_use]
44    #[allow(
45        clippy::float_cmp,
46        clippy::if_not_else,
47        clippy::many_single_char_names,
48        clippy::similar_names,
49        clippy::too_many_lines,
50        reason = "names follow Thomas's equations and the cited Boost implementation"
51    )]
52    pub fn apply(&self, lon1: f64, lat1: f64, distance: f64, azimuth12: f64) -> DirectResult {
53        let calc = SpheroidCalc::from(self.spheroid);
54        let a = calc.a;
55        let f = calc.f;
56        let one_minus_f = 1.0 - f;
57        let pi = core::f64::consts::PI;
58        let pi_half = core::f64::consts::FRAC_PI_2;
59
60        let mut azi12_alt = azimuth12;
61        let mut lat1_alt = lat1;
62        let alter_result = if azimuth12 > pi_half {
63            azi12_alt = pi - azimuth12;
64            lat1_alt = -lat1;
65            true
66        } else if azimuth12 < -pi_half {
67            azi12_alt = -pi - azimuth12;
68            lat1_alt = -lat1;
69            true
70        } else {
71            false
72        };
73
74        let theta1 = if lat1_alt == pi_half || lat1_alt == -pi_half {
75            lat1_alt
76        } else {
77            (one_minus_f * lat1_alt.tan()).atan()
78        };
79        let sin_theta1 = theta1.sin();
80        let cos_theta1 = theta1.cos();
81        let sin_a12 = azi12_alt.sin();
82        let cos_a12 = azi12_alt.cos();
83        let m = cos_theta1 * sin_a12;
84        let theta0 = m.acos();
85        let sin_theta0 = theta0.sin();
86        let n = cos_theta1 * cos_a12;
87        let c1 = f * m;
88        let c2 = f * (1.0 - m * m) / 4.0;
89        let (d_coefficient, p) = if self.second_order {
90            let d = (1.0 - c2) * (1.0 - c2 - c1 * m);
91            (d, c2 * (1.0 + c1 * m / 2.0) / d)
92        } else {
93            let d = 1.0 - 2.0 * c2 - c1 * m;
94            (d, c2 / d)
95        };
96
97        let cos_sigma1 = if sin_theta0 == 0.0 {
98            1.0
99        } else {
100            (sin_theta1 / sin_theta0).clamp(-1.0, 1.0)
101        };
102        let sigma1 = cos_sigma1.acos();
103        let d = distance / (a * d_coefficient);
104        let u = 2.0 * (sigma1 - d);
105        let cos_d = d.cos();
106        let sin_d = d.sin();
107        let cos_u = u.cos();
108        let sin_u = u.sin();
109        let w = 1.0 - 2.0 * p * cos_u;
110        let v = cos_u * cos_d - sin_u * sin_d;
111        let y = 2.0 * p * v * w * sin_d;
112        let mut d_sigma = d - y;
113        if self.second_order {
114            let x = c2 * c2 * sin_d * cos_d * (2.0 * v * v - 1.0);
115            d_sigma += x;
116        }
117        let sin_d_sigma = d_sigma.sin();
118        let cos_d_sigma = d_sigma.cos();
119
120        let mut reverse_azimuth = m.atan2(n * cos_d_sigma - sin_theta1 * sin_d_sigma);
121        if alter_result {
122            reverse_azimuth = if reverse_azimuth == 0.0 {
123                if azimuth12 >= 0.0 { pi } else { -pi }
124            } else if reverse_azimuth > 0.0 {
125                pi - reverse_azimuth
126            } else {
127                -pi - reverse_azimuth
128            };
129        }
130
131        let s_sigma = 2.0 * sigma1 - d_sigma;
132        let mut h = c1 * d_sigma;
133        if self.second_order {
134            h = h * (1.0 - c2) - c1 * c2 * sin_d_sigma * s_sigma.cos();
135        }
136        let d_eta = (sin_d_sigma * sin_a12)
137            .atan2(cos_theta1 * cos_d_sigma - sin_theta1 * sin_d_sigma * cos_a12);
138        let d_lambda = d_eta - h;
139        let mut lat2 = if m != 0.0 {
140            let sin_a21 = reverse_azimuth.sin();
141            let tan_theta2 = (sin_theta1 * cos_d_sigma + n * sin_d_sigma) * sin_a21 / m;
142            (tan_theta2 / one_minus_f).atan()
143        } else {
144            let sigma2 = s_sigma - sigma1;
145            let tan_theta2 = sigma2.cos() / sigma2.sin().abs();
146            (tan_theta2 / one_minus_f).atan()
147        };
148        if alter_result {
149            lat2 = -lat2;
150        }
151
152        DirectResult::solved(
153            lon1,
154            lat1,
155            azimuth12,
156            self.spheroid,
157            normalize_longitude(lon1 + d_lambda),
158            lat2,
159            reverse_azimuth,
160        )
161    }
162}
163
164impl Default for ThomasDirect {
165    #[inline]
166    fn default() -> Self {
167        Self::WGS84
168    }
169}
geometry_strategy/geographic/direct_thomas.rs

geometry_strategy/geographic/
direct_thomas.rs
