quantized_mesh/
coords.rs

1//! Coordinate transformations and geodetic constants.
2
3use std::f64::consts::PI;
4
5/// WGS84 ellipsoid semi-major axis (equatorial radius) in meters.
6pub const WGS84_SEMI_MAJOR_AXIS: f64 = 6_378_137.0;
7
8/// WGS84 ellipsoid semi-minor axis (polar radius) in meters.
9pub const WGS84_SEMI_MINOR_AXIS: f64 = 6_356_752.314_245;
10
11/// WGS84 ellipsoid flattening.
12pub const WGS84_FLATTENING: f64 = 1.0 / 298.257_223_563;
13
14/// WGS84 first eccentricity squared.
15pub const WGS84_E2: f64 = 2.0 * WGS84_FLATTENING - WGS84_FLATTENING * WGS84_FLATTENING;
16
17/// Convert degrees to radians.
18#[inline]
19pub fn deg_to_rad(deg: f64) -> f64 {
20    deg * PI / 180.0
21}
22
23/// Convert radians to degrees.
24#[inline]
25pub fn rad_to_deg(rad: f64) -> f64 {
26    rad * 180.0 / PI
27}
28
29/// Convert geodetic coordinates (degrees, meters) to ECEF (meters).
30///
31/// ECEF (Earth-Centered Earth-Fixed) is a Cartesian coordinate system
32/// with origin at Earth's center of mass.
33///
34/// # Arguments
35///
36/// * `lon_deg` - Longitude in degrees (-180 to 180)
37/// * `lat_deg` - Latitude in degrees (-90 to 90)
38/// * `height` - Height above ellipsoid in meters
39///
40/// # Returns
41///
42/// [X, Y, Z] coordinates in meters where:
43/// - X axis points to 0° longitude, 0° latitude
44/// - Y axis points to 90° longitude, 0° latitude
45/// - Z axis points to North Pole
46pub fn geodetic_to_ecef(lon_deg: f64, lat_deg: f64, height: f64) -> [f64; 3] {
47    let lon = deg_to_rad(lon_deg);
48    let lat = deg_to_rad(lat_deg);
49
50    let sin_lat = lat.sin();
51    let cos_lat = lat.cos();
52    let sin_lon = lon.sin();
53    let cos_lon = lon.cos();
54
55    // Radius of curvature in the prime vertical
56    let n = WGS84_SEMI_MAJOR_AXIS / (1.0 - WGS84_E2 * sin_lat * sin_lat).sqrt();
57
58    let x = (n + height) * cos_lat * cos_lon;
59    let y = (n + height) * cos_lat * sin_lon;
60    let z = (n * (1.0 - WGS84_E2) + height) * sin_lat;
61
62    [x, y, z]
63}
64
65/// Convert ECEF coordinates (meters) to geodetic (degrees, meters).
66///
67/// Uses iterative method for accurate results.
68///
69/// # Arguments
70///
71/// * `x` - X coordinate in meters
72/// * `y` - Y coordinate in meters
73/// * `z` - Z coordinate in meters
74///
75/// # Returns
76///
77/// (longitude, latitude, height) where longitude and latitude are in degrees.
78pub fn ecef_to_geodetic(x: f64, y: f64, z: f64) -> (f64, f64, f64) {
79    let lon = y.atan2(x);
80
81    let p = (x * x + y * y).sqrt();
82    let mut lat = (z / p).atan(); // Initial approximation
83
84    // Iterative refinement (Bowring's method)
85    for _ in 0..5 {
86        let sin_lat = lat.sin();
87        let n = WGS84_SEMI_MAJOR_AXIS / (1.0 - WGS84_E2 * sin_lat * sin_lat).sqrt();
88        lat = (z + WGS84_E2 * n * sin_lat).atan2(p);
89    }
90
91    let sin_lat = lat.sin();
92    let cos_lat = lat.cos();
93    let n = WGS84_SEMI_MAJOR_AXIS / (1.0 - WGS84_E2 * sin_lat * sin_lat).sqrt();
94
95    let height = if cos_lat.abs() > 1e-10 {
96        p / cos_lat - n
97    } else {
98        z.abs() / sin_lat.abs() - n * (1.0 - WGS84_E2)
99    };
100
101    (rad_to_deg(lon), rad_to_deg(lat), height)
102}
103
104/// Scale ECEF coordinates to unit ellipsoid.
105///
106/// This is used for horizon occlusion calculations.
107#[inline]
108pub fn ecef_to_ellipsoid_scaled(ecef: &[f64; 3]) -> [f64; 3] {
109    [
110        ecef[0] / WGS84_SEMI_MAJOR_AXIS,
111        ecef[1] / WGS84_SEMI_MAJOR_AXIS,
112        ecef[2] / WGS84_SEMI_MINOR_AXIS,
113    ]
114}
115
116/// Compute the magnitude (length) of a 3D vector.
117#[inline]
118pub fn vec3_magnitude(v: &[f64; 3]) -> f64 {
119    (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt()
120}
121
122/// Normalize a 3D vector.
123#[inline]
124pub fn vec3_normalize(v: &[f64; 3]) -> [f64; 3] {
125    let mag = vec3_magnitude(v);
126    if mag < 1e-10 {
127        return [0.0, 0.0, 1.0];
128    }
129    [v[0] / mag, v[1] / mag, v[2] / mag]
130}
131
132/// Compute distance between two 3D points.
133#[inline]
134pub fn vec3_distance(a: &[f64; 3], b: &[f64; 3]) -> f64 {
135    let dx = a[0] - b[0];
136    let dy = a[1] - b[1];
137    let dz = a[2] - b[2];
138    (dx * dx + dy * dy + dz * dz).sqrt()
139}
140
141#[cfg(test)]
142mod tests {
143    use super::*;
144
145    const EPSILON: f64 = 1.0;
146
147    #[test]
148    fn test_deg_rad_conversion() {
149        assert!((deg_to_rad(180.0) - PI).abs() < 1e-10);
150        assert!((rad_to_deg(PI) - 180.0).abs() < 1e-10);
151        assert!((deg_to_rad(90.0) - PI / 2.0).abs() < 1e-10);
152    }
153
154    #[test]
155    fn test_geodetic_to_ecef_equator() {
156        let [x, y, z] = geodetic_to_ecef(0.0, 0.0, 0.0);
157
158        assert!((x - WGS84_SEMI_MAJOR_AXIS).abs() < EPSILON);
159        assert!(y.abs() < EPSILON);
160        assert!(z.abs() < EPSILON);
161    }
162
163    #[test]
164    fn test_geodetic_to_ecef_north_pole() {
165        let [x, y, z] = geodetic_to_ecef(0.0, 90.0, 0.0);
166
167        assert!(x.abs() < EPSILON);
168        assert!(y.abs() < EPSILON);
169        assert!((z - WGS84_SEMI_MINOR_AXIS).abs() < EPSILON);
170    }
171
172    #[test]
173    fn test_geodetic_to_ecef_90_longitude() {
174        let [x, y, z] = geodetic_to_ecef(90.0, 0.0, 0.0);
175
176        assert!(x.abs() < EPSILON);
177        assert!((y - WGS84_SEMI_MAJOR_AXIS).abs() < EPSILON);
178        assert!(z.abs() < EPSILON);
179    }
180
181    #[test]
182    fn test_geodetic_to_ecef_with_height() {
183        let height = 1000.0;
184        let [x, _y, _z] = geodetic_to_ecef(0.0, 0.0, height);
185
186        assert!((x - (WGS84_SEMI_MAJOR_AXIS + height)).abs() < EPSILON);
187    }
188
189    #[test]
190    fn test_ecef_geodetic_roundtrip() {
191        let test_cases = [
192            (0.0, 0.0, 0.0),
193            (90.0, 0.0, 0.0),
194            (-90.0, 0.0, 0.0),
195            (0.0, 45.0, 0.0),
196            (0.0, -45.0, 0.0),
197            (139.7, 35.7, 100.0), // Tokyo
198            (-122.4, 37.8, 50.0), // San Francisco
199        ];
200
201        for (lon, lat, h) in test_cases {
202            let ecef = geodetic_to_ecef(lon, lat, h);
203            let (lon2, lat2, h2) = ecef_to_geodetic(ecef[0], ecef[1], ecef[2]);
204
205            assert!(
206                (lon - lon2).abs() < 1e-6,
207                "Longitude mismatch: {lon} vs {lon2}"
208            );
209            assert!(
210                (lat - lat2).abs() < 1e-6,
211                "Latitude mismatch: {lat} vs {lat2}"
212            );
213            assert!((h - h2).abs() < 1e-3, "Height mismatch: {h} vs {h2}");
214        }
215    }
216
217    #[test]
218    fn test_ellipsoid_scaled() {
219        let ecef = [WGS84_SEMI_MAJOR_AXIS, 0.0, 0.0];
220        let scaled = ecef_to_ellipsoid_scaled(&ecef);
221
222        assert!((scaled[0] - 1.0).abs() < 1e-10);
223        assert!(scaled[1].abs() < 1e-10);
224        assert!(scaled[2].abs() < 1e-10);
225    }
226
227    #[test]
228    fn test_vec3_operations() {
229        let v = [3.0, 4.0, 0.0];
230        assert!((vec3_magnitude(&v) - 5.0).abs() < 1e-10);
231
232        let n = vec3_normalize(&v);
233        assert!((n[0] - 0.6).abs() < 1e-10);
234        assert!((n[1] - 0.8).abs() < 1e-10);
235        assert!(n[2].abs() < 1e-10);
236
237        let a = [0.0, 0.0, 0.0];
238        let b = [3.0, 4.0, 0.0];
239        assert!((vec3_distance(&a, &b) - 5.0).abs() < 1e-10);
240    }
241}
quantized_mesh/coords.rs

quantized_mesh/
coords.rs
