plozone 0.1.2

3D spatial zone engine: geofencing, octree hole-scanning, realtime sync (WebSocket + QUIC + io_uring), voxel pathfinding, and AV sensor fusion.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218

//! Geodetic ⇄ ENU coordinate conversion (WGS84).
//!
//! All zone geometry is converted to a local **East-North-Up** (ENU) tangent
//! plane *once*, at insert time. Every subsequent spatial query is then plain
//! metric arithmetic with no geodetic math in the hot path.

/// Converts between WGS84 geodetic coordinates (lat/lon degrees + altitude
/// metres) and a local ENU frame anchored at `origin_*`.
///
/// The conversion goes geodetic → ECEF → ENU, which is accurate to the
/// millimetre out to ~100 km from the origin. Beyond that, the flat-plane
/// approximation error grows; use a tiled world with per-tile origins.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct EnuConverter {
    /// Origin latitude in degrees (WGS84).
    pub origin_lat: f64,
    /// Origin longitude in degrees (WGS84).
    pub origin_lon: f64,
    /// Origin altitude in metres above the WGS84 ellipsoid.
    pub origin_alt: f64,
}

impl EnuConverter {
    // WGS84 ellipsoid constants.
    const A: f64 = 6_378_137.0; // semi-major axis (m)
    const B: f64 = 6_356_752.314_245; // semi-minor axis (m)
    const E2: f64 = 1.0 - (Self::B * Self::B) / (Self::A * Self::A); // first eccentricity squared

    /// Anchor a new converter at the given geodetic origin.
    pub fn new(lat: f64, lon: f64, alt: f64) -> Self {
        Self { origin_lat: lat, origin_lon: lon, origin_alt: alt }
    }

    /// Geodetic (degrees) + altitude (m) → ECEF (m).
    fn geodetic_to_ecef(lat: f64, lon: f64, alt: f64) -> [f64; 3] {
        let lat_r = lat.to_radians();
        let lon_r = lon.to_radians();
        let n = Self::A / (1.0 - Self::E2 * lat_r.sin().powi(2)).sqrt();
        [
            (n + alt) * lat_r.cos() * lon_r.cos(),
            (n + alt) * lat_r.cos() * lon_r.sin(),
            (n * (1.0 - Self::E2) + alt) * lat_r.sin(),
        ]
    }

    /// Geodetic → ENU metres relative to the origin.
    pub fn to_enu(&self, lat: f64, lon: f64, alt: f64) -> [f64; 3] {
        let p = Self::geodetic_to_ecef(lat, lon, alt);
        let o = Self::geodetic_to_ecef(self.origin_lat, self.origin_lon, self.origin_alt);
        let dx = p[0] - o[0];
        let dy = p[1] - o[1];
        let dz = p[2] - o[2];

        let lat_r = self.origin_lat.to_radians();
        let lon_r = self.origin_lon.to_radians();
        let (slat, clat) = (lat_r.sin(), lat_r.cos());
        let (slon, clon) = (lon_r.sin(), lon_r.cos());

        [
            -slon * dx + clon * dy, // East
            -slat * clon * dx - slat * slon * dy + clat * dz, // North
            clat * clon * dx + clat * slon * dy + slat * dz, // Up
        ]
    }

    /// ENU metres → geodetic (degrees, degrees, metres), the inverse of
    /// [`to_enu`](Self::to_enu) via ECEF and an iterative latitude solve.
    pub fn from_enu(&self, east: f64, north: f64, up: f64) -> [f64; 3] {
        let lat_r = self.origin_lat.to_radians();
        let lon_r = self.origin_lon.to_radians();
        let (slat, clat) = (lat_r.sin(), lat_r.cos());
        let (slon, clon) = (lon_r.sin(), lon_r.cos());

        let o = Self::geodetic_to_ecef(self.origin_lat, self.origin_lon, self.origin_alt);
        // Inverse rotation (transpose of the ENU rotation matrix).
        let dx = -slon * east - slat * clon * north + clat * clon * up;
        let dy = clon * east - slat * slon * north + clat * slon * up;
        let dz = clat * north + slat * up;

        let x = o[0] + dx;
        let y = o[1] + dy;
        let z = o[2] + dz;

        // ECEF → geodetic (iterative, converge-or-max-iterations).
        let p2 = x * x + y * y;
        let p = p2.sqrt();
        let lon = y.atan2(x).to_degrees();
        let mut lat_r = (z / (p * (1.0 - Self::E2))).atan();
        for _ in 0..10 {
            let n = Self::A / (1.0 - Self::E2 * lat_r.sin().powi(2)).sqrt();
            let next = ((z + Self::E2 * n * lat_r.sin()) / p).atan();
            if (next - lat_r).abs() < 1e-14 {
                lat_r = next;
                break;
            }
            lat_r = next;
        }
        let n = Self::A / (1.0 - Self::E2 * lat_r.sin().powi(2)).sqrt();
        let alt = p / lat_r.cos() - n;
        [lat_r.to_degrees(), lon, alt]
    }
}

/// Abstraction over coordinate systems — real-world geodetic and game-space
/// alike. The rest of the crate works in an internal metric XYZ space; a
/// `CoordSystem` maps user-space coordinates into and out of it.
///
/// Kept object-safe (no `Clone` supertrait) so `Box<dyn CoordSystem>` works.
// `from_internal` mirrors `to_internal`; the `from_*` self-convention lint
// doesn't apply to a conversion pair like this.
#[allow(clippy::wrong_self_convention)]
pub trait CoordSystem: Send + Sync {
    /// User space → internal metric XYZ.
    fn to_internal(&self, pos: [f64; 3]) -> [f64; 3];
    /// Internal metric XYZ → user space.
    fn from_internal(&self, pos: [f64; 3]) -> [f64; 3];
}

// Real-world geodetic: user space is [lat, lon, alt], internal is ENU metres.
impl CoordSystem for EnuConverter {
    fn to_internal(&self, p: [f64; 3]) -> [f64; 3] {
        self.to_enu(p[0], p[1], p[2])
    }
    fn from_internal(&self, p: [f64; 3]) -> [f64; 3] {
        self.from_enu(p[0], p[1], p[2])
    }
}

impl CoordSystem for std::sync::Arc<EnuConverter> {
    fn to_internal(&self, p: [f64; 3]) -> [f64; 3] {
        (**self).to_internal(p)
    }
    fn from_internal(&self, p: [f64; 3]) -> [f64; 3] {
        (**self).from_internal(p)
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn origin_maps_to_zero() {
        let c = EnuConverter::new(10.7626, 106.6601, 0.0);
        let e = c.to_enu(10.7626, 106.6601, 0.0);
        for v in e {
            assert!(v.abs() < 1e-6, "origin should map to ENU zero, got {e:?}");
        }
    }

    #[test]
    fn east_north_up_axes_point_the_right_way() {
        let c = EnuConverter::new(0.0, 0.0, 0.0);
        // A point slightly east (larger lon) should have positive East, ~zero North.
        let east = c.to_enu(0.0, 0.001, 0.0);
        assert!(east[0] > 0.0, "east component positive: {east:?}");
        assert!(east[1].abs() < 1.0, "north component near zero: {east:?}");

        // Slightly north (larger lat) → positive North.
        let north = c.to_enu(0.001, 0.0, 0.0);
        assert!(north[1] > 0.0, "north component positive: {north:?}");

        // Higher altitude → positive Up of ~that many metres.
        let up = c.to_enu(0.0, 0.0, 25.0);
        assert!((up[2] - 25.0).abs() < 1e-3, "up ≈ altitude: {up:?}");
    }

    #[test]
    fn enu_distance_matches_metres() {
        // One arc-second of latitude ≈ 30.8 m; check magnitude is sane.
        let c = EnuConverter::new(48.0, 11.0, 0.0);
        let e = c.to_enu(48.0 + 1.0 / 3600.0, 11.0, 0.0);
        assert!((e[1] - 30.8).abs() < 0.5, "≈30.8 m north, got {}", e[1]);
    }

    #[test]
    fn round_trip_to_enu_and_back() {
        let c = EnuConverter::new(37.7749, -122.4194, 12.0);
        let (lat, lon, alt) = (37.7800, -122.4100, 55.0);
        let enu = c.to_enu(lat, lon, alt);
        let geo = c.from_enu(enu[0], enu[1], enu[2]);
        assert!((geo[0] - lat).abs() < 1e-7, "lat round-trip: {} vs {}", geo[0], lat);
        assert!((geo[1] - lon).abs() < 1e-7, "lon round-trip: {} vs {}", geo[1], lon);
        assert!((geo[2] - alt).abs() < 1e-3, "alt round-trip: {} vs {}", geo[2], alt);
    }

    #[test]
    fn round_trip_at_high_latitudes() {
        for lat in [60.0, 80.0, 89.0] {
            let c = EnuConverter::new(lat, 0.0, 50.0);
            let (tgt_lat, tgt_lon, tgt_alt) = (lat + 0.001, 0.001, 60.0);
            let enu = c.to_enu(tgt_lat, tgt_lon, tgt_alt);
            let geo = c.from_enu(enu[0], enu[1], enu[2]);
            assert!(
                (geo[0] - tgt_lat).abs() < 1e-5,
                "high-lat round-trip at {lat} deg: {} vs {}",
                geo[0],
                tgt_lat
            );
            assert!(
                (geo[1] - tgt_lon).abs() < 1e-5,
                "lon at {lat} deg"
            );
        }
    }

    #[test]
    fn from_enu_converges_in_few_iterations() {
        let c = EnuConverter::new(37.7749, -122.4194, 12.0);
        let enu = c.to_enu(37.7800, -122.4100, 55.0);
        let geo = c.from_enu(enu[0], enu[1], enu[2]);
        assert!(
            (geo[0] - 37.7800).abs() < 1e-7,
            "converged lat: {} vs 37.78",
            geo[0]
        );
    }
}