# Plozone
> 3D Geofencing · Hole Scanning · Adaptive Density Partitioning
> RTK-grade accuracy · Octree-based · Sub-centimeter support
**`plozone`** — Rust crate for high-precision 3D spatial zone management.
Real world, game maps, robotics, autonomous vehicles, IoT — one API.
```toml
[dependencies]
plozone = "0.1"
```
[](https://crates.io/crates/plozone)
[](https://docs.rs/plozone)
[](LICENSE)
-----
## Name
**Plozone** = **Plo**t + **zone**.
Plot as in spatial plotting / mapping. Zone as in the core primitive.
Pronounced: *ploh-zone*.
-----
## Architecture Overview
```
Input: RTK GNSS XYZ + Timestamp
│
▼
┌──────────────────────────────────────────────┐
│ ZoneStore │
│ ┌─────────────┐ ┌───────────────────────┐ │
│ │ enum Zone │ │ trait ZoneShape │ │
│ │ (wire-safe) │ │ (custom / dynamic) │ │
│ └──────┬──────┘ └──────────┬────────────┘ │
│ └──────────┬──────────┘ │
│ Box<dyn ZoneShape> │
│ R-tree indexed (ENU) │
└─────────────────────┬────────────────────────┘
│
┌──────────────┼──────────────┐
▼ ▼ ▼
Point-in-Zone Hole Scanner Density Query
(pure ENU geo) (octree scan) (coarse/fine)
```
**Two-layer design:**
- `enum Zone` — built-in shapes, fully `Serialize/Deserialize`, safe to send over wire
- `trait ZoneShape` — implement for any custom shape (dynamic radius, entity-following, closure-based)
- `Zone` implements `ZoneShape` automatically
- `ZoneStore` stores `Box<dyn ZoneShape>` internally — works with both
-----
## Cargo.toml
```toml
[package]
name = "plozone"
version = "0.1.0"
edition = "2024"
[features]
default = []
net = ["dep:tokio", "dep:tokio-tungstenite", "dep:futures-util",
"dep:postcard", "dep:lz4_flex"]
gnss = ["dep:ublox", "dep:serialport"]
lidar = ["dep:serialport", "dep:rplidar-rs"]
parallel = ["dep:rayon"]
full = ["net", "gnss", "lidar", "parallel"]
[dependencies]
serde = { version = "1", features = ["derive"] }
rstar = "0.13"
parry3d = "0.28"
# Optional — net feature
tokio = { version = "1.51", features = ["full"], optional = true }
tokio-tungstenite = { version = "0.24", optional = true }
futures-util = { version = "0.3", optional = true }
postcard = { version = "1.1", features = ["alloc"], optional = true }
lz4_flex = { version = "0.11", optional = true }
zstd = { version = "0.13", optional = true }
# Optional — parallel feature
rayon = { version = "1", optional = true }
```
> `serde_json` deliberately excluded from defaults. Use `serde` + `postcard`
> for wire, or add `serde_json` in your own binary if you need JSON output.
> **2026 compatibility notes:**
>
> - `parry3d 0.21+` switched mesh input from `nalgebra` → `glam` — `ConvexHull` zones use `glam::Vec3`
> - `nalgebra 0.34` — latest stable, used for EKF / IMU / pose math
> - `tokio 1.51` — current LTS (MSRV 1.71, supported until March 2027)
> - `postcard 1.1` — added `nalgebra-v0_33` / `core-num-saturating` features
> - Rust stable: **1.95.0** (May 2026), edition **2024** (stable since 1.85.0)
-----
## 1. Core Trait — ZoneShape
```rust
/// The single interface all zone types must implement.
/// All coordinates passed to these methods are in ENU meters
/// (pre-converted by EnuConverter — no geodetic math inside).
pub trait ZoneShape: Send + Sync {
/// Returns true if the ENU point lies inside this zone.
fn contains_enu(&self, p: [f64; 3]) -> bool;
/// Axis-aligned bounding box in ENU meters: [min_x, min_y, min_z, max_x, max_y, max_z]
/// Used by the R-tree for fast candidate pruning.
fn aabb_enu(&self) -> [f64; 6];
}
```
-----
## 2. Built-in Shapes — enum Zone (Serializable)
```rust
/// Serializable built-in zone shapes.
/// Implements ZoneShape after conversion to ENU via zone_to_shape().
/// Safe to send over the wire, store in DB, include in ZoneDiff.
#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub enum Zone {
/// Axis-aligned bounding box
Aabb {
min: [f64; 3], // [lat, lon, alt_m]
max: [f64; 3],
},
/// Vertical cylinder
Cylinder {
center: [f64; 2], // [lat, lon]
radius_m: f64,
z_min: f64,
z_max: f64,
},
/// 2D polygon ring extruded along Z — most common for buildings/floors
ExtrudedPolygon {
ring: Vec<[f64; 2]>, // closed lat/lon ring
z_min: f64,
z_max: f64,
},
/// Arbitrary convex polyhedron
ConvexHull {
vertices: Vec<[f64; 3]>, // [lat, lon, alt_m]
},
}
```
### 2.1 Internal ENU shapes (converted once at insert)
```rust
// These live inside ZoneStore — not public API.
// Geodetic conversion happens once at add_zone(); queries are pure ENU arithmetic.
struct AabbEnu { min: [f64; 3], max: [f64; 3] }
struct CylinderEnu { cx: f64, cy: f64, r2: f64, z_min: f64, z_max: f64 }
struct PolygonEnu { ring: Vec<[f64; 2]>, z_min: f64, z_max: f64 }
// parry3d 0.21+ uses glam::Vec3 for mesh input (breaking change from nalgebra)
struct ConvexEnu { pts: Vec<glam::Vec3> }
impl ZoneShape for AabbEnu {
fn contains_enu(&self, p: [f64; 3]) -> bool {
(0..3).all(|i| p[i] >= self.min[i] && p[i] <= self.max[i])
}
fn aabb_enu(&self) -> [f64; 6] {
[self.min[0], self.min[1], self.min[2],
self.max[0], self.max[1], self.max[2]]
}
}
impl ZoneShape for CylinderEnu {
fn contains_enu(&self, p: [f64; 3]) -> bool {
let dx = p[0] - self.cx;
let dy = p[1] - self.cy;
dx*dx + dy*dy <= self.r2
&& p[2] >= self.z_min && p[2] <= self.z_max
}
fn aabb_enu(&self) -> [f64; 6] {
let r = self.r2.sqrt();
[self.cx-r, self.cy-r, self.z_min,
self.cx+r, self.cy+r, self.z_max]
}
}
impl ZoneShape for PolygonEnu {
fn contains_enu(&self, p: [f64; 3]) -> bool {
// Z check first — cheapest gate
if p[2] < self.z_min || p[2] > self.z_max { return false; }
point_in_ring_enu(p[0], p[1], &self.ring)
}
fn aabb_enu(&self) -> [f64; 6] {
let min_x = self.ring.iter().map(|v| v[0]).fold(f64::MAX, f64::min);
let min_y = self.ring.iter().map(|v| v[1]).fold(f64::MAX, f64::min);
let max_x = self.ring.iter().map(|v| v[0]).fold(f64::MIN, f64::max);
let max_y = self.ring.iter().map(|v| v[1]).fold(f64::MIN, f64::max);
[min_x, min_y, self.z_min, max_x, max_y, self.z_max]
}
}
impl ZoneShape for ConvexEnu {
fn contains_enu(&self, p: [f64; 3]) -> bool {
use parry3d::{math::{Point, Isometry}, query::PointQuery};
// PointQuery::contains_point is correct and unambiguous.
// Ray-cast was unreliable: a ray from inside a convex hull exits exactly once,
// making edge/vertex cases ambiguous. PointQuery handles this properly.
if let Ok(hull) = parry3d::shape::ConvexPolyhedron::from_convex_hull(&self.pts) {
let pt = Point::new(p[0] as f32, p[1] as f32, p[2] as f32);
hull.contains_point(&Isometry::identity(), &pt)
} else { false }
}
fn aabb_enu(&self) -> [f64; 6] {
let (mut lo, mut hi) = ([f64::MAX; 3], [f64::MIN; 3]);
for v in &self.pts {
let arr = [v.x as f64, v.y as f64, v.z as f64];
for i in 0..3 {
lo[i] = lo[i].min(arr[i]);
hi[i] = hi[i].max(arr[i]);
}
}
[lo[0], lo[1], lo[2], hi[0], hi[1], hi[2]]
}
}
/// Jordan curve theorem — pure ENU geometry, no conversion.
/// FIX: previous version incorrectly called conv.to_enu() on already-ENU coords.
fn point_in_ring_enu(px: f64, py: f64, ring: &[[f64; 2]]) -> bool {
let n = ring.len();
let mut inside = false;
let mut j = n - 1;
for i in 0..n {
let (xi, yi) = (ring[i][0], ring[i][1]);
let (xj, yj) = (ring[j][0], ring[j][1]);
if ((yi > py) != (yj > py))
&& (px < (xj - xi) * (py - yi) / (yj - yi) + xi)
{
inside = !inside;
}
j = i;
}
inside
}
/// Convert Zone (geodetic) → Box<dyn ZoneShape> (ENU, query-ready).
/// Called once at insert time; all subsequent queries are pure arithmetic.
pub fn zone_to_shape(zone: &Zone, conv: &EnuConverter) -> Box<dyn ZoneShape> {
match zone {
Zone::Aabb { min, max } => Box::new(AabbEnu {
min: conv.to_enu(min[0], min[1], min[2]),
max: conv.to_enu(max[0], max[1], max[2]),
}),
Zone::Cylinder { center, radius_m, z_min, z_max } => {
let c = conv.to_enu(center[0], center[1], 0.0);
Box::new(CylinderEnu {
cx: c[0], cy: c[1],
r2: radius_m * radius_m,
z_min: *z_min, z_max: *z_max,
})
}
Zone::ExtrudedPolygon { ring, z_min, z_max } => {
let enu_ring = ring.iter()
.map(|v| { let e = conv.to_enu(v[0], v[1], 0.0); [e[0], e[1]] })
.collect();
Box::new(PolygonEnu { ring: enu_ring, z_min: *z_min, z_max: *z_max })
}
Zone::ConvexHull { vertices } => {
// parry3d 0.21+: use glam::Vec3 for mesh vertices
let pts = vertices.iter()
.map(|v| { let e = conv.to_enu(v[0], v[1], v[2]);
glam::Vec3::new(e[0] as f32, e[1] as f32, e[2] as f32) })
.collect();
Box::new(ConvexEnu { pts })
}
}
}
```
-----
## 3. Custom Zone Shapes — trait ZoneShape
Dev implements `ZoneShape` for any logic. These are **local-only** — cannot be serialized over the wire unless dev provides their own encoding.
```rust
// ── Dynamic shapes ─────────────────────────────────────────────────────
/// Zone that shrinks over time (battle royale safe zone, etc.)
pub struct ShrinkingZone {
pub cx: f64,
pub cy: f64,
pub z_min: f64,
pub z_max: f64,
/// Updated externally (e.g. game tick thread)
pub radius: Arc<std::sync::atomic::AtomicU32>, // meters × 1000 (fixed-point)
}
impl ShrinkingZone {
pub fn set_radius(&self, r: f64) {
self.radius.store((r * 1000.0) as u32, Ordering::Relaxed);
}
fn r(&self) -> f64 { self.radius.load(Ordering::Relaxed) as f64 / 1000.0 }
}
impl ZoneShape for ShrinkingZone {
fn contains_enu(&self, p: [f64; 3]) -> bool {
let r = self.r();
let dx = p[0] - self.cx; let dy = p[1] - self.cy;
dx*dx + dy*dy <= r*r && p[2] >= self.z_min && p[2] <= self.z_max
}
fn aabb_enu(&self) -> [f64; 6] {
let r = self.r();
[self.cx-r, self.cy-r, self.z_min,
self.cx+r, self.cy+r, self.z_max]
}
}
/// Zone attached to a moving entity
pub struct FollowZone {
pub pos: Arc<RwLock<[f64; 3]>>, // ENU, updated by entity
pub radius_m: f64,
pub half_h: f64,
}
impl ZoneShape for FollowZone {
fn contains_enu(&self, p: [f64; 3]) -> bool {
let t = self.pos.read().unwrap();
let dx = p[0]-t[0]; let dy = p[1]-t[1];
dx*dx + dy*dy <= self.radius_m * self.radius_m
&& (p[2]-t[2]).abs() <= self.half_h
}
fn aabb_enu(&self) -> [f64; 6] {
let t = self.pos.read().unwrap();
let r = self.radius_m; let h = self.half_h;
[t[0]-r, t[1]-r, t[2]-h, t[0]+r, t[1]+r, t[2]+h]
}
}
/// Composite — union of multiple shapes
pub struct UnionZone { pub parts: Vec<Box<dyn ZoneShape>> }
impl ZoneShape for UnionZone {
fn contains_enu(&self, p: [f64; 3]) -> bool {
self.parts.iter().any(|z| z.contains_enu(p))
}
fn aabb_enu(&self) -> [f64; 6] {
self.parts.iter().map(|z| z.aabb_enu())
.fold([f64::MAX,f64::MAX,f64::MAX,f64::MIN,f64::MIN,f64::MIN], |a,b| {
[a[0].min(b[0]), a[1].min(b[1]), a[2].min(b[2]),
a[3].max(b[3]), a[4].max(b[4]), a[5].max(b[5])]
})
}
}
/// Closure-based — for prototyping. LOCAL ONLY — not serializable.
/// Cannot be used with the `net` feature's ZoneDiff wire protocol.
pub struct LambdaZone<F: Fn([f64;3]) -> bool + Send + Sync> {
pub f: F,
pub aabb: [f64; 6], // dev provides bounding box manually
}
impl<F: Fn([f64;3]) -> bool + Send + Sync> ZoneShape for LambdaZone<F> {
fn contains_enu(&self, p: [f64; 3]) -> bool { (self.f)(p) }
fn aabb_enu(&self) -> [f64; 6] { self.aabb }
}
```
-----
## 4. Coordinate Converter
```rust
pub struct EnuConverter {
pub origin_lat: f64,
pub origin_lon: f64,
pub origin_alt: f64,
}
impl EnuConverter {
// WGS84 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); // e²
pub fn new(lat: f64, lon: f64, alt: f64) -> Self {
Self { origin_lat: lat, origin_lon: lon, origin_alt: alt }
}
/// Geodetic (deg) + 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 meters (proper ECEF→ENU, accurate to mm at 100 km range)
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];
// Rotation matrix: ECEF delta → ENU
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 meters → Geodetic (approximate inverse via ECEF)
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 Bowring / Vermeille)
let p2 = x * x + y * y;
let lon = y.atan2(x).to_degrees();
let mut lat_r = (z / (p2.sqrt() * (1.0 - Self::E2))).atan();
for _ in 0..5 {
let n = Self::A / (1.0 - Self::E2 * lat_r.sin().powi(2)).sqrt();
lat_r = ((z + Self::E2 * n * lat_r.sin()) / p2.sqrt()).atan();
}
let n = Self::A / (1.0 - Self::E2 * lat_r.sin().powi(2)).sqrt();
let alt = p2.sqrt() / lat_r.cos() - n;
[lat_r.to_degrees(), lon, alt]
}
}
```
-----
## 5. ZoneStore — R-tree Indexed, Works with Both Zone and ZoneShape
```rust
use rstar::{RTree, RTreeObject, AABB};
// Placeholder shape for R-tree removal (only id is compared, shape is unused)
struct DummyShape;
impl ZoneShape for DummyShape {
fn contains_enu(&self, _: [f64; 3]) -> bool { false }
fn aabb_enu(&self) -> [f64; 6] { [0.0; 6] }
}
// Internal record — stores ENU shape + original id
struct ZoneRecord {
pub id: u32,
pub shape: Box<dyn ZoneShape>,
pub aabb: [f64; 6],
}
// PartialEq by id only — lets rstar::remove() work without full shape equality
impl PartialEq for ZoneRecord { fn eq(&self, o: &Self) -> bool { self.id == o.id } }
impl Eq for ZoneRecord {}
impl RTreeObject for ZoneRecord {
type Envelope = AABB<[f64; 3]>;
fn envelope(&self) -> Self::Envelope {
AABB::from_corners(
[self.aabb[0], self.aabb[1], self.aabb[2]],
[self.aabb[3], self.aabb[4], self.aabb[5]],
)
}
}
pub struct ZoneStore {
index: RTree<ZoneRecord>,
id_to_aabb: std::collections::HashMap<u32, [f64; 6]>, // side index for O(log n) remove
}
impl ZoneStore {
/// Build from serializable ZoneEntry list (loaded from wire / DB).
pub fn from_entries(entries: &[ZoneEntry], conv: &EnuConverter) -> Self {
let mut id_to_aabb = std::collections::HashMap::new();
let records: Vec<ZoneRecord> = entries.iter().map(|e| {
let shape = zone_to_shape(&e.zone, conv);
let aabb = shape.aabb_enu();
id_to_aabb.insert(e.id, aabb);
ZoneRecord { id: e.id, shape, aabb }
}).collect();
Self { index: RTree::bulk_load(records), id_to_aabb }
}
/// Add a built-in Zone (geodetic).
pub fn add_zone(&mut self, id: u32, zone: &Zone, conv: &EnuConverter) {
let shape = zone_to_shape(zone, conv);
let aabb = shape.aabb_enu();
self.id_to_aabb.insert(id, aabb);
self.index.insert(ZoneRecord { id, shape, aabb });
}
/// Add a custom ZoneShape (ENU, local-only).
pub fn add_custom(&mut self, id: u32, shape: Box<dyn ZoneShape>) {
let aabb = shape.aabb_enu();
self.id_to_aabb.insert(id, aabb);
self.index.insert(ZoneRecord { id, shape, aabb });
}
/// Remove a zone by id — O(log n) using R-tree + PartialEq-by-id.
/// No full rebuild required.
pub fn remove(&mut self, id: u32) {
let Some(&aabb) = self.id_to_aabb.get(&id) else { return; };
// DummyShape: shape doesn't matter — PartialEq compares id only.
let dummy = ZoneRecord { id, shape: Box::new(DummyShape), aabb };
self.index.remove(&dummy);
self.id_to_aabb.remove(&id);
}
/// All zone IDs containing ENU point p.
/// R-tree prunes by AABB first → O(log N + k) full tests.
pub fn query_enu(&self, p: [f64; 3]) -> Vec<u32> {
self.index
.locate_all_at_point(&p)
.filter(|r| r.shape.contains_enu(p))
.map(|r| r.id)
.collect()
}
/// Convenience: geodetic input.
pub fn query_geodetic(&self, lat: f64, lon: f64, alt: f64, conv: &EnuConverter) -> Vec<u32> {
self.query_enu(conv.to_enu(lat, lon, alt))
}
/// All zone IDs whose AABB overlaps a geodetic bounding box.
/// Both min/max in geodetic → converted to ENU internally.
pub fn query_region(
&self,
min_geo: [f64; 3],
max_geo: [f64; 3],
conv: &EnuConverter,
) -> Vec<u32> {
let lo = conv.to_enu(min_geo[0], min_geo[1], min_geo[2]);
let hi = conv.to_enu(max_geo[0], max_geo[1], max_geo[2]);
self.index
.locate_in_envelope(&AABB::from_corners(lo, hi))
.map(|r| r.id)
.collect()
}
pub fn len(&self) -> usize { self.index.size() }
/// All zone IDs (for iterating every zone, e.g. scan-all).
pub fn ids(&self) -> Vec<u32> {
self.index.iter().map(|r| r.id).collect()
}
/// Does `zone_id` contain the ENU point? (single-zone test)
/// Used by the hole scanner without exposing the internal index.
pub fn zone_contains(&self, zone_id: u32, p: [f64; 3]) -> bool {
self.index
.locate_all_at_point(&p)
.any(|r| r.id == zone_id && r.shape.contains_enu(p))
}
/// AABB of a zone in ENU meters, if it exists.
pub fn zone_aabb(&self, zone_id: u32) -> Option<[f64; 6]> {
self.index.iter().find(|r| r.id == zone_id).map(|r| r.aabb)
}
}
/// Serializable zone entry — used for wire protocol and storage.
#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub struct ZoneEntry {
pub id: u32, // consistent everywhere
pub zone: Zone,
}
```
-----
## 6. Octree — Adaptive Density Partitioning
```
Depth → Voxel size (1000 m world)
3 → ~62 m district / macro
6 → ~7.8 m building block
9 → ~0.97 m room / floor
12 → ~12 cm object level
15 → ~1.5 cm RTK fine
17 → ~3.8 mm precision engineering
```
```rust
const MAX_DEPTH: u8 = 17;
pub struct OctreeNode {
pub center: [f64; 3],
pub half_len: f64,
pub depth: u8,
pub points: Vec<[f64; 3]>,
pub children: Option<Box<[OctreeNode; 8]>>,
}
impl OctreeNode {
pub fn new(center: [f64; 3], half_len: f64) -> Self {
Self { center, half_len, depth: 0, points: vec![], children: None }
}
fn with_depth(center: [f64; 3], half_len: f64, depth: u8) -> Self {
Self { center, half_len, depth, points: vec![], children: None }
}
pub fn insert(&mut self, p: [f64; 3], threshold: usize) {
if !self.contains(p) { return; }
if self.depth >= MAX_DEPTH
|| (self.children.is_none() && self.points.len() < threshold)
{
self.points.push(p);
return;
}
if self.children.is_none() {
self.subdivide();
let old: Vec<_> = self.points.drain(..).collect();
for op in old { self.insert(op, threshold); }
}
if let Some(ch) = &mut self.children {
for c in ch.iter_mut() {
if c.contains(p) { c.insert(p, threshold); break; }
}
}
}
pub fn insert_batch(&mut self, pts: &[[f64; 3]], threshold: usize) {
for &p in pts { self.insert(p, threshold); }
}
fn contains(&self, p: [f64; 3]) -> bool {
let h = self.half_len;
(0..3).all(|i| p[i] >= self.center[i]-h && p[i] < self.center[i]+h)
}
fn subdivide(&mut self) {
let h = self.half_len / 2.0;
let [cx, cy, cz] = self.center;
let d = self.depth + 1;
let offs = [[-h,-h,-h],[h,-h,-h],[-h,h,-h],[h,h,-h],
[-h,-h, h],[h,-h, h],[-h,h, h],[h,h, h]];
self.children = Some(Box::new(
std::array::from_fn(|i| {
let [dx,dy,dz] = offs[i];
OctreeNode::with_depth([cx+dx,cy+dy,cz+dz], h, d)
})
));
}
pub fn nodes_at_depth(&self, target: u8) -> Vec<&OctreeNode> {
if self.depth == target { return vec![self]; }
if let Some(ch) = &self.children {
ch.iter().flat_map(|c| c.nodes_at_depth(target)).collect()
} else { vec![] }
}
pub fn range_query(&self, min: [f64; 3], max: [f64; 3]) -> Vec<&[f64; 3]> {
let h = self.half_len;
let overlaps = (0..3).all(|i|
self.center[i]+h > min[i] && self.center[i]-h < max[i]);
if !overlaps { return vec![]; }
if self.children.is_none() {
return self.points.iter()
.filter(|p| (0..3).all(|i| p[i] >= min[i] && p[i] <= max[i]))
.collect();
}
self.children.as_ref().unwrap().iter()
.flat_map(|c| c.range_query(min, max)).collect()
}
}
/// Depth needed for max_error_m resolution in a world of world_size_m.
pub fn depth_for_accuracy(max_error_m: f64, world_size_m: f64) -> u8 {
let mut size = world_size_m;
let mut d = 0u8;
while size > max_error_m && d < MAX_DEPTH { size /= 2.0; d += 1; }
d
}
```
-----
## 7. Hole Scanner
A **hole** is an octree node that falls inside a zone but has no points — unsampled volume.
```rust
#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub struct Hole {
pub center: [f64; 3], // ENU meters
pub size_m: f64,
pub depth: u8,
}
pub fn scan_holes(
octree: &OctreeNode,
store: &ZoneStore,
zone_id: u32,
depth: u8,
) -> Vec<Hole> {
// Virtual-grid traversal over the zone's AABB.
// FIX: empty octree regions never subdivide, so we cannot rely on
// nodes_at_depth() to surface holes — instead we walk a regular grid
// at the target resolution and test occupancy against the octree.
let Some(aabb) = store.zone_aabb(zone_id) else { return vec![]; };
let voxel = octree.half_len * 2.0 / (1u64 << depth) as f64;
if voxel <= 0.0 { return vec![]; }
let mut holes = vec![];
let (mut x, x_end) = (aabb[0], aabb[3]);
while x < x_end {
let (mut y, y_end) = (aabb[1], aabb[4]);
while y < y_end {
let (mut z, z_end) = (aabb[2], aabb[5]);
while z < z_end {
let center = [x + voxel/2.0, y + voxel/2.0, z + voxel/2.0];
// Inside the zone?
if store.zone_contains(zone_id, center) {
// Occupied? (any octree point inside this voxel)
let occupied = !octree.range_query(
[x, y, z],
[x + voxel, y + voxel, z + voxel],
).is_empty();
if !occupied {
holes.push(Hole { center, size_m: voxel, depth });
}
}
z += voxel;
}
y += voxel;
}
x += voxel;
}
holes
}
/// Total number of virtual voxels covering a zone at `depth` (for coverage %).
pub fn voxel_count_in_zone(octree: &OctreeNode, store: &ZoneStore, zone_id: u32, depth: u8) -> usize {
let Some(aabb) = store.zone_aabb(zone_id) else { return 0; };
let voxel = octree.half_len * 2.0 / (1u64 << depth) as f64;
if voxel <= 0.0 { return 0; }
let mut count = 0;
let (mut x, xe) = (aabb[0], aabb[3]);
while x < xe {
let (mut y, ye) = (aabb[1], aabb[4]);
while y < ye {
let (mut z, ze) = (aabb[2], aabb[5]);
while z < ze {
let c = [x + voxel/2.0, y + voxel/2.0, z + voxel/2.0];
if store.zone_contains(zone_id, c) { count += 1; }
z += voxel;
}
y += voxel;
}
x += voxel;
}
count
}
pub fn scan_holes_multiscale(
octree: &OctreeNode,
store: &ZoneStore,
zone_id: u32,
depths: &[u8],
) -> Vec<(u8, Vec<Hole>)> {
depths.iter().map(|&d| (d, scan_holes(octree, store, zone_id, d))).collect()
}
```
-----
## 8. Scan Modes
```rust
pub enum ScanMode {
Coarse { depth: u8 },
Precise { depth: u8 },
Adaptive { coarse_depth: u8, refine_depth: u8, min_hole_frac: f64 },
}
pub struct ScanResult {
pub holes: Vec<Hole>,
pub coverage_pct: f64,
pub depth_used: u8,
pub node_count: usize,
}
pub fn run_scan(
octree: &OctreeNode,
store: &ZoneStore,
zone_id: u32,
mode: &ScanMode,
) -> ScanResult {
let scan = |depth: u8| -> ScanResult {
let holes = scan_holes(octree, store, zone_id, depth);
// total = all voxels covering the zone (occupied + empty),
// so coverage = fraction of voxels that are NOT holes.
let total = voxel_count_in_zone(octree, store, zone_id, depth);
ScanResult {
coverage_pct: 100.0 * (1.0 - holes.len() as f64 / total.max(1) as f64),
node_count: total, depth_used: depth, holes,
}
};
match mode {
ScanMode::Coarse { depth } | ScanMode::Precise { depth } => scan(*depth),
ScanMode::Adaptive { coarse_depth, refine_depth, min_hole_frac } => {
let coarse = scan(*coarse_depth);
let frac = coarse.holes.len() as f64 / coarse.node_count.max(1) as f64;
if frac >= *min_hole_frac { scan(*refine_depth) } else { coarse }
}
}
}
```
-----
## 9. LiDAR Integration
### 9.1 Pose
```rust
// feature = "lidar"
use nalgebra::{UnitQuaternion, Vector3};
pub struct Pose {
pub position: [f64; 3],
pub orientation: UnitQuaternion<f64>,
}
impl Pose {
pub fn level(position: [f64; 3]) -> Self {
Self { position, orientation: UnitQuaternion::identity() }
}
pub fn transform(&self, p: [f32; 3]) -> [f64; 3] {
let ps = Vector3::new(p[0] as f64, p[1] as f64, p[2] as f64);
let pw = self.orientation * ps;
[self.position[0]+pw.x, self.position[1]+pw.y, self.position[2]+pw.z]
}
}
pub struct ScanFrame {
pub points: Vec<[f32; 3]>,
pub pose: Pose,
pub ts_ns: u64,
}
pub fn ingest_scan(octree: &mut OctreeNode, frame: &ScanFrame, threshold: usize) {
let world: Vec<[f64; 3]> = frame.points.iter()
.map(|&p| frame.pose.transform(p))
.collect();
octree.insert_batch(&world, threshold);
}
```
### 9.2 RPLidar driver
```rust
// feature = "lidar"
use rplidar_drv::RplidarDevice;
pub fn open_rplidar(path: &str) -> RplidarDevice<Box<dyn serialport::SerialPort>> {
let port = serialport::new(path, 115_200)
.timeout(std::time::Duration::from_millis(100))
.open().unwrap();
RplidarDevice::with_stream(port)
}
pub fn grab_scan(dev: &mut RplidarDevice<Box<dyn serialport::SerialPort>>) -> Vec<[f32; 3]> {
dev.grab_scan().unwrap_or_default().iter()
.filter(|pt| pt.dist_mm_q2 > 0)
.map(|pt| {
let d = (pt.dist_mm_q2 as f32) / 4000.0;
let a = (pt.angle_z_q14 as f32) * std::f32::consts::PI / (180.0 * 64.0);
[d * a.cos(), d * a.sin(), 0.0]
})
.collect()
}
```
-----
## 10. Realtime Pipeline
```rust
// feature = "net" or manual threading
use std::sync::{Arc, RwLock};
use crossbeam_channel::{bounded, Sender, Receiver};
pub type SharedOctree = Arc<RwLock<OctreeNode>>;
pub type SharedStore = Arc<RwLock<ZoneStore>>;
pub struct PipelineConfig {
pub channel_cap: usize, // frame buffer depth
pub threshold: usize, // octree subdivision threshold
pub scan_every: usize, // run hole scan every N frames
pub mode: ScanMode,
}
pub fn start_pipeline(
config: PipelineConfig,
octree: SharedOctree,
store: SharedStore,
conv: Arc<EnuConverter>,
frame_src: Receiver<ScanFrame>, // caller provides frames
result_tx: Sender<(u32, ScanResult)>, // caller receives results
running: Arc<std::sync::atomic::AtomicBool>,
) {
use std::sync::atomic::Ordering;
let mut tick = 0usize;
while running.load(Ordering::Relaxed) {
let frame = match frame_src.recv_timeout(std::time::Duration::from_millis(50)) {
Ok(f) => f,
Err(_) => continue,
};
// Write lock — held only during insert
{
let mut tree = octree.write().unwrap();
ingest_scan(&mut tree, &frame, config.threshold);
}
tick += 1;
if tick % config.scan_every == 0 {
let tree = octree.read().unwrap();
let store = store.read().unwrap();
// Scan all zones — parallel if feature = "parallel"
#[cfg(feature = "parallel")]
{
use rayon::prelude::*;
store.ids().par_iter().for_each(|&id| {
let result = run_scan(&tree, &store, id, &config.mode);
let _ = result_tx.try_send((id, result));
});
}
#[cfg(not(feature = "parallel"))]
{
for id in store.ids() {
let result = run_scan(&tree, &store, id, &config.mode);
let _ = result_tx.try_send((id, result));
}
}
}
}
}
```
-----
## 11. Wire Protocol — Compressed Binary, Bidirectional
### 11.1 Compression strategy
|raw postcard |baseline |< 64 bytes (no compress) |
|postcard + lz4 |~55% of raw |realtime diffs |
|postcard + zstd|~35% of raw |snapshot on connect (one-time)|
```rust
// feature = "net"
use postcard::{to_allocvec, from_bytes};
use lz4_flex::{compress_prepend_size, decompress_size_prepended};
const LZ4_THRESHOLD: usize = 64;
pub fn encode<T: serde::Serialize>(msg: &T) -> Vec<u8> {
let raw = to_allocvec(msg).unwrap();
if raw.len() < LZ4_THRESHOLD {
let mut out = Vec::with_capacity(raw.len() + 1);
out.push(0x00u8); // uncompressed tag
out.extend_from_slice(&raw);
out
} else {
let compressed = compress_prepend_size(&raw);
let mut out = Vec::with_capacity(compressed.len() + 1);
out.push(0x01u8); // lz4 tag
out.extend_from_slice(&compressed);
out
}
}
pub fn decode<T: for<'de> serde::Deserialize<'de>>(bytes: &[u8]) -> Option<T> {
if bytes.is_empty() { return None; }
let payload = match bytes[0] {
0x00 => bytes[1..].to_vec(),
0x01 => decompress_size_prepended(&bytes[1..]).ok()?,
_ => return None,
};
from_bytes::<T>(&payload).ok()
}
pub fn encode_snapshot(entries: &[ZoneEntry]) -> Vec<u8> {
let raw = to_allocvec(entries).unwrap();
zstd::encode_all(raw.as_slice(), 3).unwrap()
}
pub fn decode_snapshot(bytes: &[u8]) -> Vec<ZoneEntry> {
let raw = zstd::decode_all(bytes).unwrap();
from_bytes(&raw).unwrap()
}
```
### 11.2 Message types
```rust
// ── Client → Server ────────────────────────────────────────────────────
#[derive(serde::Serialize, serde::Deserialize, Debug, Clone)]
pub enum ClientMsg {
/// Full position — sent on connect or after dead-reckoning drift too large
FullPos {
entity_id: u32,
pos: [f32; 3], // ENU meters, f32 = ~0.1 mm at 1 km range
ts_ms: u32,
},
/// Delta — i16 millimeters from last known position.
/// Range: ±32.7 m per tick. Falls back to FullPos if entity moves faster.
DeltaPos {
entity_id: u32, // u32 everywhere — see EntityEvent / FullPos
dx: i16, // mm
dy: i16,
dz: i16,
dt_ms: u8,
},
/// Entity is stationary — server dead-reckons, no position sent
Stationary {
entity_id: u32,
duration_ms: u16,
},
RequestSnapshot,
Ack { seq: u16 },
Ping { seq: u16 },
}
// ── Server → Client ────────────────────────────────────────────────────
#[derive(serde::Serialize, serde::Deserialize, Debug, Clone)]
pub enum ServerMsg {
/// Batched zone changes — always batched, never per-tick singles
ZoneBatch { seq: u16, diffs: Vec<ZoneDiff> },
/// Zone crossing event for this entity
EntityEvent {
entity_id: u32,
event: ZoneEvent,
zone_id: u32,
ts_ms: u32,
},
/// Hole scan result
ScanResult {
zone_id: u32,
coverage_pct: u16, // 0–10000 = 0.00%–100.00% (fixed-point × 100)
holes: Vec<CompactHole>,
},
Pong { seq: u16, server_ts_ms: u32 },
}
#[derive(serde::Serialize, serde::Deserialize, Debug, Clone)]
pub enum ZoneDiff {
Add(ZoneEntry), // ZoneEntry uses enum Zone — serializable
Remove { id: u32 },
Modify { id: u32, zone: Zone },
ScanReady { zone_id: u32 },
}
#[derive(serde::Serialize, serde::Deserialize, Debug, Clone)]
pub enum ZoneEvent { Enter, Exit, Dwell { ms: u32 } }
/// 20 bytes flat, heap-free
#[derive(serde::Serialize, serde::Deserialize, Debug, Clone, Copy)]
pub struct CompactHole {
pub cx: f32, pub cy: f32, pub cz: f32,
pub size_m: f32,
pub depth: u8,
_pad: [u8; 3],
}
```
### 11.3 Packet sizes
```
FullPos (f32×3 + ts) → postcard ~14 bytes, uncompressed (< threshold)
DeltaPos (u32 id + i16×3 + u8) → postcard ~11 bytes, uncompressed
Stationary → postcard ~4 bytes, uncompressed
ZoneDiff::Remove → postcard ~5 bytes, uncompressed
ZoneDiff::Modify (polygon) → postcard ~120 bytes → lz4 ~80 bytes
ScanResult (50 holes) → postcard ~1050 bytes → lz4 ~420 bytes
Snapshot 1k zones (zstd) → ~18 KB (one-time on connect)
```
-----
## 12. Bidirectional WebSocket Server
```rust
// feature = "net"
use tokio::net::TcpListener;
use tokio_tungstenite::{accept_async, tungstenite::Message};
use futures_util::{StreamExt, SinkExt};
use std::collections::HashMap;
use std::sync::{Arc, RwLock};
use tokio::sync::mpsc::Sender;
pub struct ZoneServer {
pub store: SharedStore,
pub octree: SharedOctree,
pub conv: Arc<EnuConverter>,
// Subscribers stored in struct — not passed per-method
subscribers: Arc<RwLock<HashMap<u32, Sender<Vec<u8>>>>>,
seq: Arc<std::sync::atomic::AtomicU32>,
}
impl ZoneServer {
pub fn new(store: SharedStore, octree: SharedOctree, conv: Arc<EnuConverter>) -> Self {
Self {
store, octree, conv,
subscribers: Arc::new(RwLock::new(HashMap::new())),
seq: Arc::new(std::sync::atomic::AtomicU32::new(0)),
}
}
fn next_seq(&self) -> u16 {
self.seq.fetch_add(1, std::sync::atomic::Ordering::Relaxed) as u16
}
/// Broadcast a batch of diffs to all connected clients.
pub fn broadcast(&self, diffs: Vec<ZoneDiff>) {
let msg = encode(&ServerMsg::ZoneBatch { seq: self.next_seq(), diffs });
for tx in self.subscribers.read().unwrap().values() {
let _ = tx.try_send(msg.clone());
}
}
/// Add zone and immediately notify all clients.
pub fn add_zone_broadcast(&self, entry: ZoneEntry) {
let diff = ZoneDiff::Add(entry.clone());
self.store.write().unwrap().add_zone(entry.id, &entry.zone, &self.conv);
self.broadcast(vec![diff]);
}
pub async fn listen(self: Arc<Self>, addr: &str) {
let listener = TcpListener::bind(addr).await.unwrap();
println!("ZoneServer listening on {addr}");
while let Ok((stream, _)) = listener.accept().await {
let srv = self.clone();
tokio::spawn(async move { srv.handle(stream).await; });
}
}
async fn handle(self: Arc<Self>, stream: tokio::net::TcpStream) {
let ws = accept_async(stream).await.unwrap();
let (mut ws_tx, mut ws_rx) = ws.split();
let entity_id = rand::random::<u32>();
let (out_tx, mut out_rx) = tokio::sync::mpsc::channel::<Vec<u8>>(256);
self.subscribers.write().unwrap().insert(entity_id, out_tx);
// Outbound pump
tokio::spawn(async move {
while let Some(bytes) = out_rx.recv().await {
let _ = ws_tx.send(Message::Binary(bytes)).await;
}
});
// Entity state for dead-reckoning
let mut last_pos: [f64; 3] = [0.0; 3];
let mut last_vel: [f64; 3] = [0.0; 3];
let mut last_ts: u32 = 0;
let mut active_zones: Vec<u32> = vec![];
while let Some(Ok(Message::Binary(bytes))) = ws_rx.next().await {
let Some(msg) = decode::<ClientMsg>(&bytes) else { continue };
let (enu, ts) = match msg {
ClientMsg::FullPos { pos, ts_ms, .. } => {
let e = [pos[0] as f64, pos[1] as f64, pos[2] as f64];
// Update velocity estimate for dead-reckoning
let dt = (ts_ms.wrapping_sub(last_ts)) as f64 / 1000.0;
if dt > 0.0 {
last_vel = std::array::from_fn(|i| (e[i]-last_pos[i]) / dt);
}
last_ts = ts_ms;
last_pos = e;
(e, ts_ms)
}
ClientMsg::DeltaPos { dx, dy, dz, dt_ms, .. } => {
let e = [
last_pos[0] + dx as f64 / 1000.0,
last_pos[1] + dy as f64 / 1000.0,
last_pos[2] + dz as f64 / 1000.0,
];
let ts = last_ts.wrapping_add(dt_ms as u32);
last_pos = e;
last_ts = ts;
(e, ts)
}
ClientMsg::Stationary { duration_ms, .. } => {
// Dead-reckon: assume velocity = 0 (entity is stopped)
let ts = last_ts.wrapping_add(duration_ms as u32);
last_ts = ts;
(last_pos, ts)
}
ClientMsg::RequestSnapshot => {
let entries: Vec<ZoneEntry> = {
let s = self.store.read().unwrap();
// Rebuild entries from store for snapshot
// (store keeps ZoneEntry cache in production)
vec![]
};
let bytes = encode_snapshot(&entries);
let _ = self.subscribers.read().unwrap()
.get(&entity_id).unwrap().try_send(bytes);
continue;
}
ClientMsg::Ping { seq } => {
let pong = encode(&ServerMsg::Pong {
seq,
server_ts_ms: epoch_ms() as u32,
});
let _ = self.subscribers.read().unwrap()
.get(&entity_id).unwrap().try_send(pong);
continue;
}
_ => continue,
};
// Zone crossing detection
let current = self.store.read().unwrap().query_enu(enu);
let subs = self.subscribers.read().unwrap();
let tx = subs.get(&entity_id).unwrap();
for &id in ¤t {
if !active_zones.contains(&id) {
let _ = tx.try_send(encode(&ServerMsg::EntityEvent {
entity_id, event: ZoneEvent::Enter, zone_id: id, ts_ms: ts,
}));
}
}
for &id in &active_zones {
if !current.contains(&id) {
let _ = tx.try_send(encode(&ServerMsg::EntityEvent {
entity_id, event: ZoneEvent::Exit, zone_id: id, ts_ms: ts,
}));
}
}
active_zones = current;
}
self.subscribers.write().unwrap().remove(&entity_id);
}
}
fn epoch_ms() -> u64 {
std::time::SystemTime::now()
.duration_since(std::time::UNIX_EPOCH).unwrap()
.as_millis() as u64
}
```
-----
## 13. Client — Adaptive Rate + Dead Reckoning
```rust
// feature = "net"
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum NetworkTier { Wifi, LTE, G3, G2Edge, Offline }
impl NetworkTier {
pub fn from_rtt(avg_rtt_ms: u32, drop_rate: f64) -> Self {
if drop_rate > 0.1 { return Self::G2Edge; }
match avg_rtt_ms {
0..=50 => Self::Wifi,
0..=100 => Self::LTE,
0..=300 => Self::G3,
0..=1000 => Self::G2Edge,
_ => Self::Offline,
}
}
pub fn tick_ms(&self) -> u64 {
match self { Self::Wifi=>50, Self::LTE=>100, Self::G3=>200,
Self::G2Edge=>1000, Self::Offline=>u64::MAX }
}
/// Correction threshold — only send update if dead-reckoning error exceeds this
pub fn correction_threshold_m(&self) -> f64 {
match self { Self::Wifi=>0.05, Self::LTE=>0.20, Self::G3=>0.50,
Self::G2Edge=>2.0, Self::Offline=>f64::MAX }
}
}
pub struct ZoneClient {
pub store: ZoneStore,
pub conv: EnuConverter,
pub entity_id: u32,
pub tier: NetworkTier,
last_sent_pos: [f32; 3],
last_sent_ts: u32,
server_vel: [f64; 3], // server's last known velocity for this entity
pending_diffs: Vec<u8>, // buffer when offline
pub on_enter: Box<dyn Fn(u32) + Send>,
pub on_exit: Box<dyn Fn(u32) + Send>,
pub on_holes: Box<dyn Fn(u32, Vec<CompactHole>) + Send>,
}
impl ZoneClient {
/// Build position message — delta if possible, full if too far or first send
pub fn build_pos_msg(&mut self, pos: [f32; 3], ts_ms: u32) -> ClientMsg {
let dx = ((pos[0]-self.last_sent_pos[0]) * 1000.0).round() as i32;
let dy = ((pos[1]-self.last_sent_pos[1]) * 1000.0).round() as i32;
let dz = ((pos[2]-self.last_sent_pos[2]) * 1000.0).round() as i32;
let dt = ts_ms.wrapping_sub(self.last_sent_ts);
// Fits in i16 mm range and time fits in u8?
if dx.abs() <= i16::MAX as i32 && dy.abs() <= i16::MAX as i32
&& dz.abs() <= i16::MAX as i32 && dt <= u8::MAX as u32
{
self.last_sent_pos = pos;
self.last_sent_ts = ts_ms;
ClientMsg::DeltaPos {
entity_id: self.entity_id,
dx: dx as i16, dy: dy as i16, dz: dz as i16,
dt_ms: dt as u8,
}
} else {
self.last_sent_pos = pos;
self.last_sent_ts = ts_ms;
ClientMsg::FullPos { entity_id: self.entity_id, pos, ts_ms }
}
}
/// Dead-reckoning check — should we send an update?
pub fn needs_update(&self, actual: [f32; 3], ts_ms: u32) -> bool {
let dt = ts_ms.wrapping_sub(self.last_sent_ts) as f64 / 1000.0;
let predicted = std::array::from_fn::<f64, 3, _>(|i|
self.last_sent_pos[i] as f64 + self.server_vel[i] * dt
);
let err = (0..3).map(|i| (actual[i] as f64 - predicted[i]).powi(2))
.sum::<f64>().sqrt();
err > self.tier.correction_threshold_m()
}
pub fn apply_server_msg(&mut self, msg: ServerMsg) {
match msg {
ServerMsg::ZoneBatch { diffs, .. } => {
for diff in diffs {
match diff {
ZoneDiff::Add(e) => self.store.add_zone(e.id, &e.zone, &self.conv),
ZoneDiff::Remove { id } => self.store.remove(id),
ZoneDiff::Modify { id, zone } => {
self.store.remove(id);
self.store.add_zone(id, &zone, &self.conv);
}
_ => {}
}
}
}
ServerMsg::EntityEvent { zone_id, event, .. } => match event {
ZoneEvent::Enter => (self.on_enter)(zone_id),
ZoneEvent::Exit => (self.on_exit)(zone_id),
_ => {}
},
ServerMsg::ScanResult { zone_id, holes, .. } => {
(self.on_holes)(zone_id, holes);
}
_ => {}
}
}
}
```
-----
## 14. Crate API Surface
### 14.1 Module tree
```
plozone
├── coord
│ └── EnuConverter
│
├── zone
│ ├── trait ZoneShape ← implement for custom shapes
│ ├── enum Zone ← built-in, serializable, wire-safe
│ ├── ZoneEntry ← id: u32 + Zone, fully serializable
│ ├── zone_to_shape() ← Zone → Box<dyn ZoneShape>
│ ├── ShrinkingZone ← dynamic radius
│ ├── FollowZone ← entity-attached
│ ├── UnionZone ← composite
│ └── LambdaZone ← closure (LOCAL ONLY, not serializable)
│
├── store
│ ├── ZoneStore ← R-tree, holds Box<dyn ZoneShape>
│ └── ZoneDiff ← uses enum Zone (wire-safe)
│
├── octree
│ ├── OctreeNode
│ └── depth_for_accuracy()
│
├── scan
│ ├── Hole / CompactHole
│ ├── scan_holes()
│ ├── scan_holes_multiscale()
│ ├── run_scan()
│ └── ScanMode / ScanResult
│
├── net ← feature = "net"
│ ├── ClientMsg / ServerMsg
│ ├── ZoneEvent
│ ├── ZoneServer
│ ├── ZoneClient
│ ├── NetworkTier
│ ├── encode() / decode()
│ └── encode_snapshot() / decode_snapshot()
│
├── pipeline ← feature = "net" or "lidar"
│ ├── PipelineConfig
│ └── start_pipeline()
│
└── lidar ← feature = "lidar"
├── Pose
├── ScanFrame
├── open_rplidar()
├── grab_scan()
└── ingest_scan()
```
### 14.2 Feature flags
```toml
# Minimal — zone query only, no networking, no hardware
plozone = { version = "0.1", default-features = false }
# Game server
plozone = { version = "0.1", features = ["net", "parallel"] }
# Robot / drone
plozone = { version = "0.1", features = ["net", "lidar", "gnss", "parallel"] }
# Mobile / embedded — just zone math, minimal deps
plozone = { version = "0.1", default-features = false, features = ["net"] }
```
### 14.3 Usage examples
```rust
// ── Minimal: zone query only ──────────────────────────────────────────
let conv = EnuConverter::new(10.7626, 106.6601, 0.0);
let mut store = ZoneStore::from_entries(&[
ZoneEntry::new(1, Zone::Cylinder {
center: [10.7626, 106.6601], radius_m: 50.0, z_min: 0.0, z_max: 20.0
}),
], &conv);
// Add custom dynamic zone
let radius = Arc::new(AtomicU32::new(100_000)); // 100 m
store.add_custom(2, Box::new(ShrinkingZone {
cx: 0.0, cy: 0.0, z_min: 0.0, z_max: 50.0, radius: radius.clone()
}));
// Query — works with both Zone and ZoneShape seamlessly
let hits = store.query_geodetic(10.7627, 106.6601, 5.0, &conv);
// → [1, 2]
// ── Game server with realtime zones ──────────────────────────────────
let server = Arc::new(ZoneServer::new(store, octree, conv));
let srv2 = server.clone();
// Shrink zone 1 every second
tokio::spawn(async move {
loop {
tokio::time::sleep(Duration::from_secs(1)).await;
// Modify zone on server — clients auto-notified
srv2.broadcast(vec![ZoneDiff::Modify {
id: 1,
zone: Zone::Cylinder { radius_m: new_radius, .. },
}]);
}
});
server.listen("0.0.0.0:9000").await;
```
-----
## 15. Accuracy Reference
|3 |~62 m |District / macro |
|6 |~7.8 m |Building block |
|9 |~0.97 m |Room / floor |
|12 |~12 cm |Object level |
|15 |~1.5 cm |RTK fine |
|17 |~3.8 mm |Precision engineering|
-----
## 16. Performance Budget
|FullPos encode |~0.4 µs |15 bytes |
|DeltaPos encode |~0.3 µs |10 bytes |
|Stationary encode |~0.2 µs |5 bytes |
|Zone event encode |~0.5 µs |12 bytes |
|ZoneDiff encode (lz4) |~1.2 µs |~80 bytes|
|Snapshot 1k zones (zstd) |~15 ms once|~18 KB |
|Point query (R-tree, 10k zones) |~50 µs |— |
|Hole scan depth 12 |~2 ms |— |
|Parallel scan 100 zones / 8 cores|~5 ms |— |
|Octree batch insert 1k pts |~500 µs |— |
**Position at 20 Hz, FullPos:** ~300 bytes/sec per entity
**Position at 10 Hz, DeltaPos:** ~100 bytes/sec per entity
**Dead-reckoning, correction only:** ~5–20 bytes/sec per entity
**10k entities @ LTE tier:** ~1 MB/sec server inbound
-----
## 17. Global-Scale Mapping
### 17.1 Problem with single ENU origin
A single `EnuConverter` accumulates error beyond ~100 km because it approximates Earth as a flat plane. At 100 km from origin, horizontal error reaches ~1 m. At 500 km, ~25 m.
**Solution: Tiled world** — split the globe into tiles, each with its own ENU origin. Cross-tile queries handled by querying adjacent tiles.
```rust
/// Web-map-style tile key (zoom/x/y — here used as lat/lon grid)
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, serde::Serialize, serde::Deserialize)]
pub struct TileKey {
pub zoom: u8, // 0 = whole world, 1 = hemispheres, …, 14 = ~2.4 km tiles
pub tx: i32,
pub ty: i32,
}
impl TileKey {
/// Tile size in degrees at this zoom level
pub fn deg_size(zoom: u8) -> f64 { 360.0 / (1u64 << zoom) as f64 }
/// Which tile does a geodetic point fall into?
pub fn from_geodetic(lat: f64, lon: f64, zoom: u8) -> Self {
let s = Self::deg_size(zoom);
Self {
zoom,
tx: ((lon + 180.0) / s).floor() as i32,
ty: ((lat + 90.0) / s).floor() as i32,
}
}
/// ENU origin for this tile (south-west corner, sea level)
pub fn origin(&self) -> (f64, f64) {
let s = Self::deg_size(self.zoom);
let lat = self.ty as f64 * s - 90.0;
let lon = self.tx as f64 * s - 180.0;
(lat, lon)
}
/// 8 neighbors + self (for cross-tile queries near boundaries)
pub fn neighbors(&self) -> [TileKey; 9] {
let mut out = [*self; 9];
let mut i = 0;
for dy in -1i32..=1 { for dx in -1i32..=1 {
out[i] = TileKey { zoom: self.zoom, tx: self.tx+dx, ty: self.ty+dy };
i += 1;
}}
out
}
}
/// One tile = one ZoneStore + one OctreeNode, each with local ENU origin.
pub struct WorldTile {
pub key: TileKey,
pub conv: EnuConverter,
pub store: ZoneStore,
pub octree: OctreeNode,
}
impl WorldTile {
pub fn new(key: TileKey) -> Self {
let (lat, lon) = key.origin();
Self {
key,
conv: EnuConverter::new(lat, lon, 0.0),
store: ZoneStore::from_entries(&[], &EnuConverter::new(lat, lon, 0.0)),
octree: OctreeNode::new([0.0; 3], 50_000.0), // 100 km tile
}
}
}
/// Global world — HashMap of tiles, lazy-initialized
pub struct TiledWorld {
pub zoom: u8,
pub tiles: std::collections::HashMap<TileKey, WorldTile>,
}
impl TiledWorld {
pub fn new(zoom: u8) -> Self {
// zoom=14 → ~2.4 km tiles (ENU error < 1mm within tile)
// zoom=12 → ~9.8 km tiles (ENU error < 1cm within tile)
// zoom=10 → ~39 km tiles (ENU error < 5cm within tile)
Self { zoom, tiles: Default::default() }
}
fn tile_for(&mut self, lat: f64, lon: f64) -> &mut WorldTile {
let key = TileKey::from_geodetic(lat, lon, self.zoom);
self.tiles.entry(key).or_insert_with(|| WorldTile::new(key))
}
/// Insert a point into the correct tile's octree
pub fn insert_point(&mut self, lat: f64, lon: f64, alt: f64) {
let tile = self.tile_for(lat, lon);
let enu = tile.conv.to_enu(lat, lon, alt);
tile.octree.insert(enu, 8);
}
/// Query zones across tile boundaries (checks self + 8 neighbors)
pub fn query_geodetic(&mut self, lat: f64, lon: f64, alt: f64) -> Vec<(TileKey, Vec<u32>)> {
let key = TileKey::from_geodetic(lat, lon, self.zoom);
let neighbors = key.neighbors();
let mut results = vec![];
for nk in neighbors {
if let Some(tile) = self.tiles.get(&nk) {
let enu = tile.conv.to_enu(lat, lon, alt);
let hits = tile.store.query_enu(enu);
if !hits.is_empty() {
results.push((nk, hits));
}
}
}
results
}
/// Add a zone — inserts into ALL tiles the zone's geodetic AABB spans.
pub fn add_zone(&mut self, entry: ZoneEntry) {
let tiles_to_fill = self.tiles_for_zone(&entry.zone);
for key in tiles_to_fill {
self.ensure_tile(key);
let tile = self.tiles.get_mut(&key).unwrap();
tile.store.add_zone(entry.id, &entry.zone, &tile.conv.clone());
}
}
/// All tile keys overlapping the zone's geodetic AABB.
fn tiles_for_zone(&self, zone: &Zone) -> Vec<TileKey> {
let (min_lat, min_lon, max_lat, max_lon) = zone_geodetic_bbox(zone);
let deg = TileKey::deg_size(self.zoom);
let tx0 = ((min_lon + 180.0) / deg).floor() as i32;
let ty0 = ((min_lat + 90.0) / deg).floor() as i32;
let tx1 = ((max_lon + 180.0) / deg).ceil() as i32;
let ty1 = ((max_lat + 90.0) / deg).ceil() as i32;
let mut keys = vec![];
for ty in ty0..=ty1 { for tx in tx0..=tx1 {
keys.push(TileKey { zoom: self.zoom, tx, ty });
}}
keys
}
fn ensure_tile(&mut self, key: TileKey) {
self.tiles.entry(key).or_insert_with(|| WorldTile::new(key));
}
}
/// Geodetic bounding box of any Zone: (min_lat, min_lon, max_lat, max_lon)
fn zone_geodetic_bbox(zone: &Zone) -> (f64, f64, f64, f64) {
match zone {
Zone::Aabb { min, max } => (min[0], min[1], max[0], max[1]),
Zone::Cylinder { center, radius_m, .. } => {
// approximate: 1 deg lat ≈ 111_111 m
let dlat = radius_m / 111_111.0;
let dlon = radius_m / (111_111.0 * center[0].to_radians().cos());
(center[0]-dlat, center[1]-dlon, center[0]+dlat, center[1]+dlon)
}
Zone::ExtrudedPolygon { ring, .. } => {
let lats: Vec<f64> = ring.iter().map(|v| v[0]).collect();
let lons: Vec<f64> = ring.iter().map(|v| v[1]).collect();
(lats.iter().cloned().fold(f64::MAX,f64::min),
lons.iter().cloned().fold(f64::MAX,f64::min),
lats.iter().cloned().fold(f64::MIN,f64::max),
lons.iter().cloned().fold(f64::MIN,f64::max))
}
Zone::ConvexHull { vertices } => {
let lats: Vec<f64> = vertices.iter().map(|v| v[0]).collect();
let lons: Vec<f64> = vertices.iter().map(|v| v[1]).collect();
(lats.iter().cloned().fold(f64::MAX,f64::min),
lons.iter().cloned().fold(f64::MAX,f64::min),
lats.iter().cloned().fold(f64::MIN,f64::max),
lons.iter().cloned().fold(f64::MIN,f64::max))
}
}
}
fn zone_center_geodetic(zone: &Zone) -> (f64, f64) {
match zone {
Zone::Cylinder { center, .. } => (center[0], center[1]),
Zone::Aabb { min, max } => ((min[0]+max[0])/2.0, (min[1]+max[1])/2.0),
Zone::ExtrudedPolygon { ring, .. } => {
let lat = ring.iter().map(|v| v[0]).sum::<f64>() / ring.len() as f64;
let lon = ring.iter().map(|v| v[1]).sum::<f64>() / ring.len() as f64;
(lat, lon)
}
Zone::ConvexHull { vertices } => {
let lat = vertices.iter().map(|v| v[0]).sum::<f64>() / vertices.len() as f64;
let lon = vertices.iter().map(|v| v[1]).sum::<f64>() / vertices.len() as f64;
(lat, lon)
}
}
}
```
### 17.2 ENU accuracy by zoom level
|10 |~39 km |~5 cm |Country-level zone management|
|12 |~9.8 km |~3 mm |City district tracking |
|14 |~2.4 km |~0.2 mm |Building / robot / drone |
|16 |~600 m |< 0.01 mm |RTK precision, indoor |
-----
## 18. Voxel Pathfinding on Octree
> In-scope because the octree already encodes occupancy — points = obstacle,
> empty nodes inside a zone = free space. A* over octree leaf nodes
> is a natural fit, no separate data structure needed.
```rust
/// Feature: pathfinding
/// A* over octree voxels at a fixed depth.
/// Occupied = node has points. Free = node is empty and inside zone.
use std::collections::BinaryHeap;
use std::cmp::Ordering;
#[derive(Clone, Debug)]
pub struct PathNode {
pub center: [f64; 3], // ENU meters
pub depth: u8,
}
#[derive(Clone)]
struct AStarState {
f: f64, // f = g + h
g: f64, // cost from start
center: [f64; 3],
}
impl PartialEq for AStarState { fn eq(&self, o: &Self) -> bool { self.f == o.f } }
impl Eq for AStarState {}
impl PartialOrd for AStarState { fn partial_cmp(&self, o: &Self) -> Option<Ordering> { Some(self.cmp(o)) } }
impl Ord for AStarState {
// Min-heap: negate f so BinaryHeap (max-heap) pops smallest f
fn cmp(&self, o: &Self) -> Ordering {
o.f.partial_cmp(&self.f).unwrap_or(Ordering::Equal)
}
}
/// Find a path from `start` to `goal` (ENU meters) through free voxels
/// inside the given zone at `depth` resolution.
///
/// Returns None if no path exists (zone fully blocked or disconnected).
pub fn find_path(
octree: &OctreeNode,
store: &ZoneStore,
zone_id: u32,
start: [f64; 3],
goal: [f64; 3],
depth: u8,
) -> Option<Vec<PathNode>> {
let voxel_size = octree.half_len * 2.0 / (1u64 << depth) as f64;
if voxel_size <= 0.0 { return None; }
// Collect FREE voxels = inside the zone AND empty in the octree.
// Virtual-grid traversal over the zone AABB (same reasoning as the hole
// scanner: empty octree regions never subdivide, so we cannot enumerate
// free space from existing nodes — we walk a regular grid instead).
let Some(aabb) = store.zone_aabb(zone_id) else { return None; };
let mut free: std::collections::HashMap<OrdF64x3, [f64; 3]> = Default::default();
let (mut x, xe) = (aabb[0], aabb[3]);
while x < xe {
let (mut y, ye) = (aabb[1], aabb[4]);
while y < ye {
let (mut z, ze) = (aabb[2], aabb[5]);
while z < ze {
let c = [x + voxel_size/2.0, y + voxel_size/2.0, z + voxel_size/2.0];
let empty = octree.range_query(
[x, y, z],
[x + voxel_size, y + voxel_size, z + voxel_size],
).is_empty();
if empty && store.zone_contains(zone_id, c) {
free.insert(OrdF64x3::from(c), c);
}
z += voxel_size;
}
y += voxel_size;
}
x += voxel_size;
}
if free.is_empty() { return None; }
// Snap to the SAME grid the free map uses: center = aabb_min + voxel/2 + k*voxel.
// (A naive round-to-multiple-of-voxel grid would be offset by voxel/2 and never
// match the free-voxel keys.)
let snap = |p: [f64; 3]| -> [f64; 3] {
std::array::from_fn(|i| {
let k = ((p[i] - aabb[i] - voxel_size/2.0) / voxel_size).round();
aabb[i] + voxel_size/2.0 + k * voxel_size
})
};
// Snap, then fall back to nearest free voxel if the snapped cell is occupied.
let nearest_free = |p: [f64; 3]| -> Option<[f64; 3]> {
let s = snap(p);
if free.contains_key(&OrdF64x3::from(s)) { return Some(s); }
free.values()
.min_by(|a, b| {
let da = (0..3).map(|i| (a[i]-p[i]).powi(2)).sum::<f64>();
let db = (0..3).map(|i| (b[i]-p[i]).powi(2)).sum::<f64>();
da.partial_cmp(&db).unwrap()
})
.copied()
};
let start_v = nearest_free(start)?;
let goal_v = nearest_free(goal)?;
// A* over voxel grid
let h = |p: [f64; 3]| -> f64 {
// 3D Euclidean heuristic (admissible)
(0..3).map(|i| (p[i]-goal_v[i]).powi(2)).sum::<f64>().sqrt()
};
let mut open = BinaryHeap::new();
let mut came = std::collections::HashMap::<OrdF64x3, [f64; 3]>::new();
let mut g_cost = std::collections::HashMap::<OrdF64x3, f64>::new();
open.push(AStarState { f: h(start_v), g: 0.0, center: start_v });
g_cost.insert(OrdF64x3::from(start_v), 0.0);
// 26-connectivity neighbors (face + edge + corner adjacent voxels)
let neighbor_offsets: Vec<[i32; 3]> = {
let mut v = vec![];
for dz in -1i32..=1 { for dy in -1i32..=1 { for dx in -1i32..=1 {
if dx != 0 || dy != 0 || dz != 0 { v.push([dx,dy,dz]); }
}}}
v
};
while let Some(AStarState { g, center, .. }) = open.pop() {
if OrdF64x3::from(center) == OrdF64x3::from(goal_v) {
// Reconstruct path
let mut path = vec![PathNode { center: goal_v, depth }];
let mut cur = goal_v;
while let Some(&prev) = came.get(&OrdF64x3::from(cur)) {
path.push(PathNode { center: prev, depth });
cur = prev;
if OrdF64x3::from(cur) == OrdF64x3::from(start_v) { break; }
}
path.reverse();
return Some(path);
}
for off in &neighbor_offsets {
let nc = std::array::from_fn(|i|
center[i] + off[i] as f64 * voxel_size);
// Skip if not a free voxel
if !free.contains_key(&OrdF64x3::from(nc)) { continue; }
let step_cost = (0..3).map(|i| (off[i] as f64 * voxel_size).powi(2))
.sum::<f64>().sqrt();
let ng = g + step_cost;
if ng < *g_cost.get(&OrdF64x3::from(nc)).unwrap_or(&f64::MAX) {
g_cost.insert(OrdF64x3::from(nc), ng);
came.insert(OrdF64x3::from(nc), center);
open.push(AStarState { f: ng + h(nc), g: ng, center: nc });
}
}
}
None // no path found
}
/// Hashable wrapper for [f64; 3] voxel centers (rounded to mm precision)
#[derive(PartialEq, Eq, Hash, Clone, Copy)]
struct OrdF64x3([i64; 3]);
impl From<[f64; 3]> for OrdF64x3 {
fn from(p: [f64; 3]) -> Self {
// Round to 0.1 mm — avoids float equality issues
Self(std::array::from_fn(|i| (p[i] * 10_000.0).round() as i64))
}
}
```
**Usage:**
```rust
// depth 9 → ~1m voxels — good for room-level navigation
// depth 12 → ~12cm — robot corridor navigation
let path = find_path(&octree, &store, zone_id,
[0.0, 0.0, 1.5], // start ENU
[20.0, 15.0, 1.5], // goal ENU
9,
);
if let Some(waypoints) = path {
for wp in &waypoints {
println!("→ {:?}", wp.center);
}
}
```
> **Scope note:** This is voxel A* — suitable for robots, drones, indoor navigation.
> For large open terrain, NavMesh (e.g. `recast-rs`) gives smoother paths.
> The two complement each other: use voxel A* inside zones, NavMesh outside.
-----
## 19. Terrain Mesh — Marching Cubes
Convert the octree point cloud into a surface mesh. Occupied voxels = solid, empty = air. Marching cubes extracts the isosurface between them.
```rust
/// Feature: terrain
/// Output: triangle mesh as (vertices, indices)
pub struct TriMesh {
pub vertices: Vec<[f32; 3]>,
pub indices: Vec<[u32; 3]>,
}
/// Build a terrain mesh from octree point density at given depth.
/// depth 9 → ~1m resolution, depth 12 → ~12cm resolution
///
/// Uses the `isosurface` crate (feature = "terrain") for the 256-case
/// Marching Cubes table — embed it as follows:
///
/// ```toml
/// isosurface = { version = "0.3", optional = true } # feature = "terrain"
/// ```
///
/// ```rust
/// // With the isosurface crate (feature = "terrain"):
/// use isosurface::{marching_cubes::MarchingCubes, source::Source};
///
/// pub struct OctreeDensity<'a> { octree: &'a OctreeNode, depth: u8 }
///
/// impl<'a> Source for OctreeDensity<'a> {
/// fn sample(&self, x: f32, y: f32, z: f32) -> f32 {
/// let voxel = self.octree.half_len * 2.0 / (1u64 << self.depth) as f64;
/// let pts = self.octree.range_query(
/// [x as f64 - voxel, y as f64 - voxel, z as f64 - voxel],
/// [x as f64 + voxel, y as f64 + voxel, z as f64 + voxel],
/// );
/// (pts.len() as f32 / 8.0).min(1.0)
/// }
/// fn dims(&self) -> [usize; 3] {
/// let n = (1usize << self.depth).min(512);
/// [n, n, n]
/// }
/// }
///
/// pub fn marching_cubes(octree: &OctreeNode, depth: u8) -> TriMesh {
/// let src = OctreeDensity { octree, depth };
/// let mut mc = MarchingCubes::new();
/// mc.extract(&src, 0.5) // isolevel 0.5
/// }
/// ```
///
/// Without the crate: export raw voxel density as `.ply` or `.obj` and
/// run Meshlab / Open3D offline for surface reconstruction.
pub fn marching_cubes_export_density(octree: &OctreeNode, depth: u8) -> Vec<([f32;3], f32)> {
// Returns (center_xyz, density) per occupied voxel — feed into any isosurface impl.
octree.nodes_at_depth(depth).into_iter()
.filter(|n| !n.points.is_empty())
.map(|n| {
let c = [n.center[0] as f32, n.center[1] as f32, n.center[2] as f32];
let d = (n.points.len() as f32 / 8.0).min(1.0);
(c, d)
})
.collect()
}
/// Export mesh to OBJ format (load in Blender, MeshLab, etc.)
pub fn to_obj(mesh: &TriMesh) -> String {
let mut out = String::new();
for v in &mesh.vertices {
out.push_str(&format!("v {} {} {}\n", v[0], v[1], v[2]));
}
for tri in &mesh.indices {
out.push_str(&format!("f {} {} {}\n", tri[0]+1, tri[1]+1, tri[2]+1));
}
out
}
/// Export to binary PLY (compact, standard for point cloud tools)
pub fn to_ply(mesh: &TriMesh) -> Vec<u8> {
let header = format!(
"ply\nformat binary_little_endian 1.0\n\
element vertex {}\nproperty float x\nproperty float y\nproperty float z\n\
element face {}\nproperty list uchar uint vertex_indices\nend_header\n",
mesh.vertices.len(), mesh.indices.len()
);
let mut out = header.into_bytes();
for v in &mesh.vertices {
for &f in v { out.extend_from_slice(&f.to_le_bytes()); }
}
for tri in &mesh.indices {
out.push(3u8);
for &i in tri { out.extend_from_slice(&i.to_le_bytes()); }
}
out
}
```
**Cargo addition:**
```toml
isosurface = { version = "0.3", optional = true } # feature = "terrain"
```
-----
## 20. no_std / Bare Metal / RTOS
### 20.1 Feature flag
```toml
[features]
std = ["serde/std"] # default for desktop/server
no_std = [] # for embedded, RTOS, bare metal
[dependencies]
serde = { version = "1", default-features = false, features = ["derive", "alloc"] }
```
```rust
// lib.rs
#![cfg_attr(not(feature = "std"), no_std)]
extern crate alloc; // Vec, Box, HashMap via alloc crate
use alloc::vec::Vec;
use alloc::boxed::Box;
use alloc::collections::BTreeMap; // no_std HashMap alternative
```
### 20.2 What works without std
|`coord::EnuConverter` |✅ |Pure f64 math |
|`zone::Zone` (enum) |✅ |Serde alloc |
|`zone::ZoneShape` (trait) |✅ |Box<dyn> needs alloc |
|`octree::OctreeNode` |✅ |Vec needs alloc |
|`scan::scan_holes` |✅ | |
|`store::ZoneStore` (R-tree)|⚠️ |rstar needs std; use BTreeMap fallback|
|`net::*` |❌ |tokio requires std |
|`lidar::*` |❌ |serialport requires std |
### 20.3 Minimal no_std ZoneStore (BTreeMap fallback)
```rust
/// no_std ZoneStore — linear scan, no R-tree.
/// Suitable for embedded with < ~500 zones.
#[cfg(not(feature = "std"))]
pub struct ZoneStoreEmbed {
zones: alloc::vec::Vec<(u32, alloc::boxed::Box<dyn ZoneShape>)>,
}
#[cfg(not(feature = "std"))]
impl ZoneStoreEmbed {
pub fn new() -> Self { Self { zones: alloc::vec::Vec::new() } }
pub fn add(&mut self, id: u32, shape: alloc::boxed::Box<dyn ZoneShape>) {
self.zones.push((id, shape));
}
pub fn query_enu(&self, p: [f64; 3]) -> alloc::vec::Vec<u32> {
self.zones.iter()
.filter(|(_, s)| s.contains_enu(p))
.map(|(id, _)| *id)
.collect()
}
}
```
### 20.4 Target platforms
```toml
# ARM Cortex-M (STM32, nRF52)
[target.thumbv7em-none-eabihf]
# compile: cargo build --target thumbv7em-none-eabihf --no-default-features --features no_std
# ESP32-S3
[target.xtensa-esp32s3-none-elf]
# RISC-V (ESP32-C3, SiFive)
[target.riscv32imac-unknown-none-elf]
# Zephyr RTOS (via cargo-zephyr)
# FreeRTOS (via freertos-rust)
```
-----
## 21. Async, Low Latency & Scale
### 21.1 QUIC transport — connection survives network change
WebSocket over TCP drops when switching WiFi → 4G. QUIC maintains the session across network changes — critical for mobile robots and phones.
```toml
quinn = { version = "0.12", optional = true } # feature = "quic"
wtransport = { version = "0.4", optional = true } # browser-compatible WebTransport
```
```rust
// feature = "quic"
use quinn::{Endpoint, ServerConfig, Connection};
pub async fn quic_server(addr: &str, server: Arc<ZoneServer>) {
let endpoint = Endpoint::server(ServerConfig::with_single_cert(
vec![cert], key).unwrap(), addr.parse().unwrap()
).unwrap();
while let Some(conn) = endpoint.accept().await {
let conn = conn.await.unwrap();
let srv = server.clone();
tokio::spawn(async move {
// QUIC streams — ordered, reliable, multiplexed
while let Ok((send, recv)) = conn.accept_bi().await {
let srv = srv.clone();
tokio::spawn(async move { srv.handle_quic(send, recv).await; });
}
});
}
}
```
**QUIC advantages over WebSocket for this crate:**
|Survives network change |❌ |✅ |
|Head-of-line blocking |❌ |✅ (per-stream) |
|0-RTT reconnect |❌ |✅ |
|UDP-based (lower overhead)|❌ |✅ |
|Browser support |✅ |✅ (WebTransport)|
### 21.2 Sharded ZoneServer — horizontal scale
Single `RwLock<ZoneStore>` is a bottleneck above ~50k concurrent entities. Solution: shard by entity ID.
```rust
const SHARDS: usize = 16; // power of 2, one per CPU core
pub struct ShardedZoneServer {
shards: [Arc<ZoneServer>; SHARDS],
}
impl ShardedZoneServer {
pub fn shard_for(&self, entity_id: u32) -> &Arc<ZoneServer> {
&self.shards[(entity_id as usize) & (SHARDS - 1)]
}
pub fn handle_update(&self, entity_id: u32, pos: [f64; 3]) {
// Each entity always routes to the same shard — no cross-shard locking
self.shard_for(entity_id).process_position(entity_id, pos);
}
/// Broadcast a zone diff to ALL shards (zone changes are global)
pub fn broadcast_all(&self, diffs: Vec<ZoneDiff>) {
for shard in &self.shards {
shard.broadcast(diffs.clone());
}
}
}
// Scale estimate:
// 1 shard → ~5k entities @ < 1ms p99 latency
// 16 shards → ~80k entities @ < 1ms p99 latency
// 64 shards → ~300k entities (need multi-process / cluster beyond this)
```
### 21.3 io_uring backend — ultra-low latency on Linux
```toml
tokio = { version = "1", features = ["full", "io-uring"] }
# or
monoio = "0.2" # io_uring-native async runtime, lower latency than tokio
```
```rust
// monoio: thread-per-core, no cross-thread sync, ~30% lower latency than tokio
#[monoio::main(driver = "io_uring")]
async fn main() {
run_server("0.0.0.0:9000", store, octree, conv).await;
}
```
### 21.4 Lock-free position updates
For extreme throughput, replace `RwLock<[f64;3]>` with atomic position:
```rust
use std::sync::atomic::{AtomicU64, Ordering};
/// Lock-free XYZ storage — encode f32 × 2 per u64
pub struct AtomicPos {
xy: AtomicU64, // x: hi 32 bits, y: lo 32 bits (f32 bits)
z: AtomicU64, // z: hi 32 bits, unused: lo 32 bits
}
impl AtomicPos {
pub fn store(&self, x: f32, y: f32, z: f32) {
let xy = ((x.to_bits() as u64) << 32) | y.to_bits() as u64;
self.xy.store(xy, Ordering::Release);
self.z.store((z.to_bits() as u64) << 32, Ordering::Release);
}
pub fn load(&self) -> [f32; 3] {
let xy = self.xy.load(Ordering::Acquire);
let z = self.z.load(Ordering::Acquire);
[
f32::from_bits((xy >> 32) as u32),
f32::from_bits(xy as u32),
f32::from_bits((z >> 32) as u32),
]
}
}
// FollowZone now uses AtomicPos instead of Arc<RwLock<[f64;3]>>
// → zone query with zero locking overhead
```
### 21.5 Scale summary
|Single process, tokio |~5k |~100k |< 5 ms |1 server |
|Sharded (16), tokio |~80k |~100k |< 2 ms |1 server |
|Sharded (64), io_uring|~300k |~500k |< 1 ms |1 server |
|Multi-process cluster |~5M+ |unlimited|< 5 ms |Load balancer + N servers|
> Zone state sync across servers requires an external pub/sub bus
> (Redis Streams, NATS, or custom QUIC multicast) to propagate `ZoneDiff`.
-----
## 22. Updated Crate API — Full Feature Matrix
```toml
[features]
default = []
std = ["serde/std", "dep:rstar"] # desktop/server (recommended)
no_std = [] # embedded / RTOS
net = ["std", "dep:tokio", "dep:tokio-tungstenite",
"dep:futures-util", "dep:postcard", "dep:lz4_flex"]
quic = ["net", "dep:quinn"]
gnss = ["std", "dep:ublox", "dep:serialport"]
lidar = ["std", "dep:serialport", "dep:rplidar-rs"]
parallel = ["std", "dep:rayon"]
terrain = ["std", "dep:isosurface"]
pathfinding = ["std"] # built-in, no extra deps
tiled = ["std"] # global-scale TiledWorld
full = ["net", "quic", "gnss", "lidar",
"parallel", "terrain", "pathfinding", "tiled"]
```
### What each feature unlocks
|*(default)* |`ZoneShape` trait, `EnuConverter`, `OctreeNode` — pure no_std-ready core|
|`std` |`ZoneStore` (R-tree), `Zone` enum, `ZoneDiff`, serde |
|`no_std` |`ZoneStoreEmbed` (linear scan), alloc-based collections |
|`net` |`ZoneServer`, `ZoneClient`, `encode/decode`, WebSocket, dead-reckoning |
|`quic` |QUIC transport, connection migration, 0-RTT reconnect |
|`gnss` |RTK u-blox F9P driver, NTRIP client |
|`lidar` |RPLidar driver, `ScanFrame`, `ingest_scan` |
|`parallel` |Rayon parallel zone scan |
|`terrain` |`marching_cubes`, `to_obj`, `to_ply` |
|`pathfinding`|3D voxel A* on octree |
|`tiled` |`TiledWorld`, global-scale multi-ENU-origin |
-----
## 23. Game Map — Custom Coordinate Systems
### 23.1 CoordSystem trait
Real world uses lat/lon → `EnuConverter`. Game maps use arbitrary XYZ — no geodetic at all. A `CoordSystem` trait abstracts both so the rest of the crate works unchanged.
```rust
/// Abstraction for both real-world and game coordinate systems.
/// ZoneStore, OctreeNode, pathfinding — all operate in internal metric XYZ.
/// CoordSystem converts from "user space" (game units / geodetic) to that space.
pub trait CoordSystem: Send + Sync + Clone {
fn to_internal(&self, pos: [f64; 3]) -> [f64; 3];
fn from_internal(&self, pos: [f64; 3]) -> [f64; 3];
}
// EnuConverter already implements CoordSystem (real world)
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]) }
}
/// Game world where 1 unit = 1 meter, Z-up — passthrough, zero cost
#[derive(Clone)]
pub struct DirectCartesian;
impl CoordSystem for DirectCartesian {
fn to_internal(&self, p: [f64; 3]) -> [f64; 3] { p }
fn from_internal(&self, p: [f64; 3]) -> [f64; 3] { p }
}
/// Unreal Engine: 1 unit = 1 cm, Z-up → scale = 0.01
/// Custom origin offset if world doesn't start at 0
#[derive(Clone)]
pub struct ScaledCartesian {
pub scale_to_meters: f64,
pub origin: [f64; 3],
}
impl CoordSystem for ScaledCartesian {
fn to_internal(&self, p: [f64; 3]) -> [f64; 3] {
std::array::from_fn(|i| (p[i] - self.origin[i]) * self.scale_to_meters)
}
fn from_internal(&self, p: [f64; 3]) -> [f64; 3] {
std::array::from_fn(|i| p[i] / self.scale_to_meters + self.origin[i])
}
}
/// Unity: Y-up → remap to Z-up internal
#[derive(Clone)]
pub struct YUpCartesian { pub scale: f64 }
impl CoordSystem for YUpCartesian {
fn to_internal(&self, p: [f64; 3]) -> [f64; 3] {
[p[0]*self.scale, p[2]*self.scale, p[1]*self.scale]
}
fn from_internal(&self, p: [f64; 3]) -> [f64; 3] {
[p[0]/self.scale, p[2]/self.scale, p[1]/self.scale]
}
}
/// 2D game (top-down / side-scroller) — Z is a fixed layer constant
#[derive(Clone)]
pub struct Cartesian2D { pub scale: f64, pub layer: f64 }
impl CoordSystem for Cartesian2D {
fn to_internal(&self, p: [f64; 3]) -> [f64; 3] {
[p[0]*self.scale, p[1]*self.scale, self.layer]
}
fn from_internal(&self, p: [f64; 3]) -> [f64; 3] {
[p[0]/self.scale, p[1]/self.scale, 0.0]
}
}
```
2D zones: set `z_min = f64::NEG_INFINITY`, `z_max = f64::INFINITY` → Z check always passes, pure 2D polygon query.
```rust
let zone_2d = Zone::ExtrudedPolygon {
ring: vec![[0.,0.],[100.,0.],[100.,100.],[0.,100.],[0.,0.]],
z_min: f64::NEG_INFINITY,
z_max: f64::INFINITY,
};
```
-----
### 23.2 Multi-Instance World
```rust
/// One instance = one isolated "room" running the same map template.
/// All instances share zone definitions; each has its own ZoneStore + Octree.
pub struct GameInstance {
pub instance_id: u32,
pub store: ZoneStore,
pub octree: OctreeNode,
pub coord: Box<dyn CoordSystem>,
}
pub struct GameWorld {
instances: std::collections::HashMap<u32, GameInstance>,
zone_templates: Vec<ZoneEntry>, // shared definitions
}
impl GameWorld {
pub fn new(templates: Vec<ZoneEntry>) -> Self {
Self { instances: Default::default(), zone_templates: templates }
}
pub fn spawn_instance(&mut self, id: u32, coord: Box<dyn CoordSystem>, world_half: f64) {
let store = ZoneStore::from_entries(&self.zone_templates, coord.as_ref());
self.instances.insert(id, GameInstance {
instance_id: id,
store,
octree: OctreeNode::new([0.0; 3], world_half),
coord,
});
}
pub fn despawn_instance(&mut self, id: u32) { self.instances.remove(&id); }
pub fn query(&self, instance_id: u32, pos: [f64; 3]) -> Vec<u32> {
let inst = self.instances.get(&instance_id).unwrap();
inst.store.query_enu(inst.coord.to_internal(pos))
}
pub fn add_zone_to_instance(&mut self, instance_id: u32, entry: ZoneEntry) {
let inst = self.instances.get_mut(&instance_id).unwrap();
inst.store.add_zone(entry.id, &entry.zone, inst.coord.as_ref());
}
}
```
-----
### 23.3 Layered Map (Multi-floor)
```rust
/// Multiple floors stacked on same XY — MMO dungeons, buildings, platformers
pub struct LayeredMap {
pub layers: std::collections::HashMap<i32, ZoneStore>,
pub coord: ScaledCartesian,
pub floor_height: f64, // meters per floor
}
impl LayeredMap {
pub fn floor_of(&self, z_game: f64) -> i32 {
let z_m = z_game * self.coord.scale_to_meters;
(z_m / self.floor_height).floor() as i32
}
pub fn add_zone(&mut self, floor: i32, entry: ZoneEntry) {
self.layers
.entry(floor)
.or_insert_with(|| ZoneStore::from_entries(&[], &self.coord))
.add_zone(entry.id, &entry.zone, &self.coord);
}
/// Query current floor + adjacent floors (handles boundary cases)
pub fn query(&self, pos: [f64; 3]) -> Vec<(i32, Vec<u32>)> {
let floor = self.floor_of(pos[2]);
let p = self.coord.to_internal(pos);
[floor - 1, floor, floor + 1].iter()
.filter_map(|&f| {
let hits = self.layers.get(&f)?.query_enu(p);
if hits.is_empty() { None } else { Some((f, hits)) }
})
.collect()
}
}
```
-----
### 23.4 Chunked World (Minecraft-style)
```rust
#[derive(Hash, PartialEq, Eq, Clone, Copy, Debug)]
pub struct ChunkKey { pub cx: i32, pub cy: i32, pub cz: i32 }
impl ChunkKey {
pub fn from_pos(pos: [f64; 3], chunk_size: f64) -> Self {
Self {
cx: (pos[0] / chunk_size).floor() as i32,
cy: (pos[1] / chunk_size).floor() as i32,
cz: (pos[2] / chunk_size).floor() as i32,
}
}
pub fn neighbors(&self) -> Vec<ChunkKey> {
let mut v = vec![];
for dz in -1i32..=1 { for dy in -1i32..=1 { for dx in -1i32..=1 {
v.push(ChunkKey { cx: self.cx+dx, cy: self.cy+dy, cz: self.cz+dz });
}}}
v
}
}
pub struct ChunkedGameWorld {
pub chunk_size: f64,
pub coord: Box<dyn CoordSystem>,
pub max_chunks: usize, // LRU eviction cap
chunks: std::collections::HashMap<ChunkKey, GameChunk>,
access_order: std::collections::VecDeque<ChunkKey>,
}
pub struct GameChunk {
pub store: ZoneStore,
pub octree: OctreeNode,
}
impl ChunkedGameWorld {
fn ensure_loaded(&mut self, key: ChunkKey) {
if self.chunks.contains_key(&key) { return; }
// LRU evict
if self.chunks.len() >= self.max_chunks {
if let Some(old) = self.access_order.pop_front() {
self.chunks.remove(&old);
}
}
let h = self.chunk_size / 2.0;
let origin = std::array::from_fn(|i| {
[key.cx, key.cy, key.cz][i] as f64 * self.chunk_size + h
});
self.chunks.insert(key, GameChunk {
store: ZoneStore::from_entries(&[], self.coord.as_ref()),
octree: OctreeNode::new(origin, h),
});
self.access_order.push_back(key);
}
pub fn query(&mut self, pos: [f64; 3]) -> Vec<u32> {
let p = self.coord.to_internal(pos);
let key = ChunkKey::from_pos(p, self.chunk_size);
key.neighbors().into_iter()
.flat_map(|k| {
self.ensure_loaded(k);
self.chunks.get(&k).unwrap().store.query_enu(p)
})
.collect()
}
pub fn insert_point(&mut self, pos: [f64; 3]) {
let p = self.coord.to_internal(pos);
let key = ChunkKey::from_pos(p, self.chunk_size);
self.ensure_loaded(key);
self.chunks.get_mut(&key).unwrap().octree.insert(p, 8);
}
}
```
-----
### 23.5 Portal System
```rust
/// Trigger zone → teleport to another instance / coordinate space
#[derive(Debug, Clone)]
pub struct Portal {
pub id: u32,
pub trigger_zone: Zone,
pub dest_instance: u32,
pub dest_pos: [f64; 3], // position in destination game space
pub dest_yaw_deg: f32, // facing direction after teleport
}
pub struct PortalSystem {
portals: Vec<Portal>,
portal_store: ZoneStore, // R-tree indexed trigger zones
}
impl PortalSystem {
pub fn build(portals: Vec<Portal>, coord: &impl CoordSystem) -> Self {
let entries: Vec<ZoneEntry> = portals.iter()
.map(|p| ZoneEntry::new(p.id, p.trigger_zone.clone()))
.collect();
Self {
portal_store: ZoneStore::from_entries(&entries, coord),
portals,
}
}
/// Returns triggered portal if entity is inside one
pub fn check(&self, pos: [f64; 3], coord: &impl CoordSystem) -> Option<&Portal> {
let p = coord.to_internal(pos);
let hits = self.portal_store.query_enu(p);
hits.first().and_then(|&id| self.portals.iter().find(|p| p.id == id))
}
}
// Usage:
// if let Some(portal) = portal_system.check(entity_pos, &coord) {
// world.teleport(entity_id, portal.dest_instance, portal.dest_pos, portal.dest_yaw_deg);
// }
```
-----
### 23.6 Bevy ECS Integration
```rust
// feature = "bevy"
use bevy::prelude::*;
pub struct Spatial3dPlugin<C: CoordSystem + 'static> {
pub coord: C,
pub world_half: f64,
}
impl<C: CoordSystem + 'static> Plugin for Spatial3dPlugin<C> {
fn build(&self, app: &mut App) {
let store = ZoneStore::from_entries(&[], &self.coord);
let octree = OctreeNode::new([0.0; 3], self.world_half);
app
.insert_resource(SharedZoneStore(Arc::new(RwLock::new(store))))
.insert_resource(SharedOctreeRes(Arc::new(RwLock::new(octree))))
.add_event::<ZoneEnterEvent>()
.add_event::<ZoneExitEvent>()
.add_systems(Update, zone_tracking_system::<C>);
}
}
#[derive(Resource)] pub struct SharedZoneStore(pub Arc<RwLock<ZoneStore>>);
#[derive(Resource)] pub struct SharedOctreeRes(pub Arc<RwLock<OctreeNode>>);
#[derive(Component, Default)]
pub struct ZoneTracker { pub active: Vec<u32> }
#[derive(Event)] pub struct ZoneEnterEvent { pub entity: Entity, pub zone_id: u32 }
#[derive(Event)] pub struct ZoneExitEvent { pub entity: Entity, pub zone_id: u32 }
fn zone_tracking_system<C: CoordSystem + Resource>(
store: Res<SharedZoneStore>,
mut query: Query<(Entity, &Transform, &mut ZoneTracker)>,
mut enter: EventWriter<ZoneEnterEvent>,
mut exit: EventWriter<ZoneExitEvent>,
) {
let store = store.0.read().unwrap();
for (entity, tf, mut tracker) in &mut query {
let t = tf.translation;
// Bevy is Y-up: remap to internal Z-up
let p = [t.x as f64, t.z as f64, t.y as f64];
let current: Vec<u32> = store.query_enu(p);
for &id in ¤t {
if !tracker.active.contains(&id) {
enter.send(ZoneEnterEvent { entity, zone_id: id });
}
}
for &id in &tracker.active {
if !current.contains(&id) {
exit.send(ZoneExitEvent { entity, zone_id: id });
}
}
tracker.active = current;
}
}
```
**Usage in Bevy app:**
```rust
App::new()
.add_plugins(DefaultPlugins)
.add_plugins(Spatial3dPlugin {
coord: DirectCartesian, // or YUpCartesian, ScaledCartesian...
world_half: 5000.0,
})
.run();
// React to zone events anywhere in the ECS:
fn my_system(mut events: EventReader<ZoneEnterEvent>) {
for ev in events.read() {
println!("Entity {:?} entered zone {}", ev.entity, ev.zone_id);
}
}
```
-----
### 23.7 Game vs Real World — summary
|Coordinate system |`EnuConverter`|`DirectCartesian` / `ScaledCartesian` / `YUpCartesian`|
|Global coverage |`TiledWorld` |`ChunkedGameWorld` |
|Multiple spaces |— |`GameWorld` (instances) |
|Floor stacking |— |`LayeredMap` |
|Space transition |— |`PortalSystem` |
|Engine integration|CLI / server |Bevy plugin |
|2D support |Not needed |`Cartesian2D` + Z=∞ zones |
|Unit scale |meters (fixed)|Any — `scale_to_meters` |
Dev picks a `CoordSystem`, everything else — zone query, hole scan, octree, pathfinding, realtime sync, terrain mesh — **works without modification**.
-----
## 24. Autonomous Vehicle Support
### 24.1 Architecture overview
```
Sensors Crate modules Outputs
─────── ───────────── ───────
RTK GNSS (cm) ─────────▶ EnuConverter │
IMU ─────────▶ ImuFusion (dead reckon) ──────▶ VehiclePose
LiDAR 360° ─────────▶ ingest_scan → OctreeNode │
Cameras ─────────▶ FrustumZone (FOV check) │
Radar ─────────▶ RadarSectorZone │
│ │
ZoneStore (R-tree) │
├── ODD zones ──────────▶ ODD violation alert
├── Behavior zones ──────────▶ speed limit, yield
├── SafetyEnvelope ──────────▶ collision check
├── PredictionZone ──────────▶ risk score
└── SensorFovZone ──────────▶ blind spot map
│
LaneGraph (HD Map)
├── lane_pathfind() ──────────▶ route waypoints
└── OccupancyGrid ──────────▶ local obstacle map
│
V2X Server (QUIC) ──────────▶ broadcast to RSU / fleet
```
-----
### 24.2 Vehicle Pose — RTK + IMU fusion
```rust
use nalgebra::{UnitQuaternion, Vector3, Matrix6, Vector6};
/// Full 6-DOF vehicle state
#[derive(Debug, Clone)]
pub struct VehiclePose {
pub position: [f64; 3], // ENU meters (RTK-grade)
pub orientation: UnitQuaternion<f64>, // heading + pitch + roll
pub velocity: [f64; 3], // m/s ENU
pub ts_ns: u64,
}
/// Extended Kalman Filter: fuses RTK position + IMU acceleration/gyro
/// State vector: [x, y, z, vx, vy, vz] (6D)
pub struct ImuFusion {
pub state: Vector6<f64>, // [pos(3), vel(3)]
pub cov: Matrix6<f64>, // state covariance
pub last_ts: u64, // nanoseconds
pub has_rtk: bool,
}
impl ImuFusion {
pub fn new(initial_pos: [f64; 3]) -> Self {
let mut state = Vector6::zeros();
state[0] = initial_pos[0];
state[1] = initial_pos[1];
state[2] = initial_pos[2];
Self {
state,
cov: Matrix6::identity() * 1.0,
last_ts: 0,
has_rtk: false,
}
}
/// Predict step — called every IMU tick (~100–1000 Hz)
/// accel: m/s² body-frame, gyro: rad/s (used externally for orientation)
pub fn predict(&mut self, accel_enu: [f64; 3], ts_ns: u64) {
if self.last_ts == 0 { self.last_ts = ts_ns; return; }
let dt = (ts_ns - self.last_ts) as f64 / 1e9;
self.last_ts = ts_ns;
// State transition: pos += vel*dt, vel += accel*dt
self.state[0] += self.state[3] * dt + 0.5 * accel_enu[0] * dt * dt;
self.state[1] += self.state[4] * dt + 0.5 * accel_enu[1] * dt * dt;
self.state[2] += self.state[5] * dt + 0.5 * accel_enu[2] * dt * dt;
self.state[3] += accel_enu[0] * dt;
self.state[4] += accel_enu[1] * dt;
self.state[5] += accel_enu[2] * dt;
// Process noise (tunable — higher = trust IMU less)
let q = 0.1 * dt;
for i in 0..6 { self.cov[(i,i)] += q; }
}
/// Update step — called when RTK fix available (~10 Hz)
pub fn update_rtk(&mut self, pos: [f64; 3], h_acc: f64) {
// Kalman gain: K = P * H^T * (H * P * H^T + R)^-1
// H = [I3|0] (observe position only)
let r = h_acc * h_acc; // RTK measurement noise variance
for i in 0..3 {
let k = self.cov[(i,i)] / (self.cov[(i,i)] + r);
let innovation = pos[i] - self.state[i];
self.state[i] += k * innovation;
self.cov[(i,i)] *= 1.0 - k;
}
self.has_rtk = true;
}
pub fn pose(&self, orientation: UnitQuaternion<f64>, ts_ns: u64) -> VehiclePose {
VehiclePose {
position: [self.state[0], self.state[1], self.state[2]],
velocity: [self.state[3], self.state[4], self.state[5]],
orientation,
ts_ns,
}
}
/// Dead-reckoning quality: covariance trace (lower = more confident)
pub fn uncertainty_m(&self) -> f64 {
(self.cov[(0,0)] + self.cov[(1,1)] + self.cov[(2,2)]).sqrt()
}
}
```
-----
### 24.3 Sensor FOV Zones
Each sensor’s field of view is a `ZoneShape`. Obstacles inside the FOV are detectable; outside = blind spot.
```rust
/// LiDAR 360° horizontal scan — torus/cylinder FOV
pub struct LidarFovZone {
pub pose: Arc<RwLock<VehiclePose>>,
pub min_range: f64, // dead zone (m)
pub max_range: f64, // max detection range (m)
pub z_min: f64, // vertical FOV bottom offset from sensor (m)
pub z_max: f64, // vertical FOV top offset
}
impl ZoneShape for LidarFovZone {
fn contains_enu(&self, p: [f64; 3]) -> bool {
let pose = self.pose.read().unwrap();
let dx = p[0] - pose.position[0];
let dy = p[1] - pose.position[1];
let dz = p[2] - pose.position[2];
let r2 = dx*dx + dy*dy;
r2 >= self.min_range * self.min_range
&& r2 <= self.max_range * self.max_range
&& dz >= self.z_min
&& dz <= self.z_max
}
fn aabb_enu(&self) -> [f64; 6] {
let pose = self.pose.read().unwrap();
let r = self.max_range;
let p = pose.position;
[p[0]-r, p[1]-r, p[2]+self.z_min,
p[0]+r, p[1]+r, p[2]+self.z_max]
}
}
/// Camera frustum — truncated pyramid in vehicle-forward direction
pub struct CameraFrustumZone {
pub pose: Arc<RwLock<VehiclePose>>,
pub near_m: f64, // near plane distance
pub far_m: f64, // far plane (max detection range)
pub hfov_rad: f64, // horizontal field of view
pub vfov_rad: f64, // vertical field of view
}
impl ZoneShape for CameraFrustumZone {
fn contains_enu(&self, p: [f64; 3]) -> bool {
let pose = self.pose.read().unwrap();
// Transform point to vehicle body frame
let dp = Vector3::new(
p[0] - pose.position[0],
p[1] - pose.position[1],
p[2] - pose.position[2],
);
let body = pose.orientation.inverse() * dp;
let fwd = body[0]; // forward axis
let left = body[1];
let up = body[2];
if fwd < self.near_m || fwd > self.far_m { return false; }
// Angular check within FOV cone
let h_angle = (left / fwd).atan().abs();
let v_angle = (up / fwd).atan().abs();
h_angle <= self.hfov_rad / 2.0 && v_angle <= self.vfov_rad / 2.0
}
fn aabb_enu(&self) -> [f64; 6] {
let pose = self.pose.read().unwrap();
let r = self.far_m;
let p = pose.position;
[p[0]-r, p[1]-r, p[2]-r, p[0]+r, p[1]+r, p[2]+r] // conservative AABB
}
}
/// Radar sector — wide angle, long range, primarily forward
pub struct RadarSectorZone {
pub pose: Arc<RwLock<VehiclePose>>,
pub range_m: f64,
pub half_angle: f64, // half of total azimuth (radians)
}
impl ZoneShape for RadarSectorZone {
fn contains_enu(&self, p: [f64; 3]) -> bool {
let pose = self.pose.read().unwrap();
let dp = Vector3::new(
p[0] - pose.position[0],
p[1] - pose.position[1],
p[2] - pose.position[2],
);
let body = pose.orientation.inverse() * dp;
let dist = (body[0]*body[0] + body[1]*body[1]).sqrt();
if dist > self.range_m { return false; }
let angle = body[1].atan2(body[0]).abs();
angle <= self.half_angle
}
fn aabb_enu(&self) -> [f64; 6] {
let pose = self.pose.read().unwrap();
let r = self.range_m;
let p = pose.position;
[p[0]-r, p[1]-r, p[2]-2., p[0]+r, p[1]+r, p[2]+2.]
}
}
```
-----
### 24.4 Safety Envelope
Dynamic buffer zone around the vehicle — larger at high speed, shaped by velocity vector.
```rust
/// Safety envelope — ellipsoid that stretches forward proportional to speed.
/// At 0 km/h: sphere r=0.5m.
/// At 100 km/h: front buffer ~30m, side ~2m, rear ~5m.
pub struct SafetyEnvelope {
pub pose: Arc<RwLock<VehiclePose>>,
pub width_m: f64, // vehicle half-width
pub base_front_m: f64, // min front buffer (m)
pub base_rear_m: f64, // min rear buffer (m)
pub speed_factor: f64, // extra front meters per m/s of speed
}
impl SafetyEnvelope {
fn dims(&self, pose: &VehiclePose) -> (f64, f64, f64) {
let speed = (0..3).map(|i| pose.velocity[i].powi(2)).sum::<f64>().sqrt();
let front = self.base_front_m + speed * self.speed_factor;
let rear = self.base_rear_m;
let side = self.width_m;
(front, rear, side)
}
}
impl ZoneShape for SafetyEnvelope {
fn contains_enu(&self, p: [f64; 3]) -> bool {
let pose = self.pose.read().unwrap();
let (front, rear, side) = self.dims(&pose);
let dp = Vector3::new(
p[0] - pose.position[0],
p[1] - pose.position[1],
p[2] - pose.position[2],
);
let body = pose.orientation.inverse() * dp;
// Asymmetric: different front/rear extent
let norm_fwd = if body[0] >= 0.0 { body[0] / front } else { body[0].abs() / rear };
let norm_lat = body[1].abs() / side;
let norm_up = body[2].abs() / (side * 0.5);
// Inside envelope = unit ellipsoid
norm_fwd * norm_fwd + norm_lat * norm_lat + norm_up * norm_up <= 1.0
}
fn aabb_enu(&self) -> [f64; 6] {
let pose = self.pose.read().unwrap();
let (front,_,s) = self.dims(&pose);
let p = pose.position;
let r = front.max(s);
[p[0]-r, p[1]-r, p[2]-s, p[0]+r, p[1]+r, p[2]+s]
}
}
/// Check if any LiDAR point falls inside safety envelope
pub fn safety_check(
envelope: &SafetyEnvelope,
octree: &OctreeNode,
pose: &VehiclePose,
range_m: f64,
) -> bool {
let p = pose.position;
let pts = octree.range_query(
[p[0]-range_m, p[1]-range_m, p[2]-range_m],
[p[0]+range_m, p[1]+range_m, p[2]+range_m],
);
pts.iter().any(|&&pt| envelope.contains_enu(pt))
}
```
-----
### 24.5 Prediction Zone — Other Vehicle Trajectory
Predict where another vehicle could be in the next N seconds. Shape is an uncertainty cone that grows over time.
```rust
/// Gaussian uncertainty cone for predicted vehicle position.
/// At t=0: tight ellipse around current pos.
/// At t=T: wider ellipse along predicted trajectory.
pub struct PredictionZone {
pub origin: [f64; 3], // current position ENU
pub velocity: [f64; 3], // current velocity m/s
pub horizon_s: f64, // prediction horizon (seconds)
pub sigma_lat: f64, // lateral uncertainty (m)
pub sigma_fwd: f64, // forward uncertainty growth per second
}
impl PredictionZone {
/// Risk score for a query point — 0.0 (safe) to 1.0 (high risk)
pub fn risk_at(&self, p: [f64; 3], t_s: f64) -> f64 {
if t_s > self.horizon_s { return 0.0; }
// Predicted center at time t
let center = std::array::from_fn(|i|
self.origin[i] + self.velocity[i] * t_s
);
// Direction of travel (forward axis)
let speed = (0..3).map(|i| self.velocity[i].powi(2)).sum::<f64>().sqrt();
let fwd = if speed > 0.01 {
std::array::from_fn(|i| self.velocity[i] / speed)
} else { [1.0, 0.0, 0.0] };
let dp = std::array::from_fn::<f64,3,_>(|i| p[i] - center[i]);
let dp_fwd = (0..3).map(|i| dp[i] * fwd[i]).sum::<f64>();
let dp_lat = ((0..3).map(|i| dp[i].powi(2)).sum::<f64>()
- dp_fwd * dp_fwd).sqrt();
let sigma_fwd_t = self.sigma_fwd * t_s + 1.0;
let exponent = (dp_fwd / sigma_fwd_t).powi(2)
+ (dp_lat / self.sigma_lat).powi(2);
(-0.5 * exponent).exp() // Gaussian probability density (unnormalized)
}
/// Max risk across the entire horizon (sampled at dt intervals)
pub fn max_risk(&self, p: [f64; 3], dt_s: f64) -> f64 {
let steps = (self.horizon_s / dt_s).ceil() as usize;
(0..=steps)
.map(|i| self.risk_at(p, i as f64 * dt_s))
.fold(0.0f64, f64::max)
}
}
impl ZoneShape for PredictionZone {
fn contains_enu(&self, p: [f64; 3]) -> bool {
self.max_risk(p, 0.1) > 0.05 // threshold tunable
}
fn aabb_enu(&self) -> [f64; 6] {
let max_fwd = self.velocity.iter()
.map(|v| v.abs() * self.horizon_s)
.sum::<f64>();
let r = max_fwd + self.sigma_lat * 3.0;
let p = self.origin;
[p[0]-r, p[1]-r, p[2]-2., p[0]+r, p[1]+r, p[2]+2.]
}
}
```
-----
### 24.6 Behavior Zones — ODD + Traffic Rules
```rust
/// Zone metadata for traffic rule enforcement
#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub struct BehaviorZone {
pub entry: ZoneEntry,
pub behavior: TrafficBehavior,
}
#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub enum TrafficBehavior {
SpeedLimit { max_mps: f32 },
SchoolZone { max_mps: f32, active_hours: (u8, u8) },
YieldZone,
StopZone,
NoPassing,
Roundabout { direction: TurnDir },
OddBoundary, // vehicle must not exit this zone
ConstructionZone { max_mps: f32 },
PedestrianPriority,
}
#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub enum TurnDir { Clockwise, CounterClockwise }
pub struct BehaviorZoneStore {
pub zones: Vec<BehaviorZone>,
pub store: ZoneStore,
}
impl BehaviorZoneStore {
pub fn build(zones: Vec<BehaviorZone>, conv: &impl CoordSystem) -> Self {
let entries: Vec<ZoneEntry> = zones.iter()
.map(|b| b.entry.clone()).collect();
Self {
store: ZoneStore::from_entries(&entries, conv),
zones,
}
}
/// Active behaviors at current vehicle position
pub fn active_behaviors(&self, pos: [f64; 3], conv: &impl CoordSystem)
-> Vec<&TrafficBehavior>
{
let p = conv.to_internal(pos);
let hits = self.store.query_enu(p);
hits.iter()
.filter_map(|&id| self.zones.iter().find(|z| z.entry.id == id))
.map(|z| &z.behavior)
.collect()
}
/// Effective speed limit at position (minimum across all active zones)
pub fn speed_limit_mps(&self, pos: [f64; 3], conv: &impl CoordSystem) -> f32 {
self.active_behaviors(pos, conv)
.iter()
.filter_map(|b| match b {
TrafficBehavior::SpeedLimit { max_mps } => Some(*max_mps),
TrafficBehavior::SchoolZone { max_mps, .. } => Some(*max_mps),
TrafficBehavior::ConstructionZone { max_mps } => Some(*max_mps),
_ => None,
})
.fold(f32::MAX, f32::min)
}
/// ODD violation check — vehicle must stay within ODD boundary zones
pub fn odd_violated(&self, pos: [f64; 3], conv: &impl CoordSystem) -> bool {
let has_odd_zones = self.zones.iter()
.any(|z| matches!(z.behavior, TrafficBehavior::OddBoundary));
if !has_odd_zones { return false; }
let p = conv.to_internal(pos);
let hits = self.store.query_enu(p);
let in_odd = hits.iter().any(|&id|
self.zones.iter()
.find(|z| z.entry.id == id)
.map(|z| matches!(z.behavior, TrafficBehavior::OddBoundary))
.unwrap_or(false)
);
!in_odd // violation = not inside any ODD zone
}
}
```
-----
### 24.7 HD Map — Lane Graph
```rust
/// Node = waypoint on a lane centerline
#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub struct LaneNode {
pub id: u32,
pub pos: [f64; 3], // ENU meters
pub lane_id: u32,
pub speed_limit: f32, // m/s
}
/// Directed edge = drivable connection between two nodes
#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub struct LaneEdge {
pub from: u32,
pub to: u32,
pub kind: EdgeKind,
pub cost: f64, // precomputed: distance * penalty
}
#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub enum EdgeKind {
Forward, // same lane, straight
LaneChange, // lateral move, higher cost
Intersection, // through intersection
UTurn, // highest cost
}
impl EdgeKind {
pub fn penalty(&self) -> f64 {
match self {
Self::Forward => 1.0,
Self::LaneChange => 2.5,
Self::Intersection => 1.5,
Self::UTurn => 5.0,
}
}
}
// NOTE: kiddo's API changed between major versions (3.x → 4.x → 5.x):
// type params, `add`, and the distance-metric path all differ. The code below
// targets kiddo 4/5-style usage and is illustrative — pin the exact version
// (e.g. `kiddo = "5"`) and adjust `KdTree<...>` params / `SquaredEuclidean`
// import to match. A plain Vec + linear scan also works for < ~1k lane nodes.
pub struct HdMap {
pub nodes: Vec<LaneNode>,
pub edges: Vec<LaneEdge>,
/// Spatial index — nearest lane node to a position (kiddo 5.x form)
node_index: kiddo::KdTree<f64, 3>,
}
impl HdMap {
pub fn build(nodes: Vec<LaneNode>, edges: Vec<LaneEdge>) -> Self {
let mut index = kiddo::KdTree::new();
for n in &nodes {
index.add(&n.pos, n.id); // kiddo 5.x: add(&[f64; 3], u64) — cast id if needed
}
Self { nodes, edges, node_index: index }
}
/// Nearest lane node to an ENU position
pub fn nearest_node(&self, pos: [f64; 3]) -> Option<&LaneNode> {
// kiddo 5.x: nearest_one returns NearestNeighbour { distance, item }
let nn = self.node_index.nearest_one::<kiddo::SquaredEuclidean>(&pos);
self.nodes.iter().find(|n| n.id == nn.item)
}
/// A* lane-level pathfinding
pub fn find_route(&self, start_pos: [f64; 3], goal_pos: [f64; 3]) -> Option<Vec<&LaneNode>> {
use std::collections::{BinaryHeap, HashMap};
use std::cmp::Ordering;
#[derive(Clone)]
struct State { f: f64, g: f64, id: u32 }
impl PartialEq for State { fn eq(&self, o: &Self) -> bool { self.f == o.f } }
impl Eq for State {}
impl PartialOrd for State { fn partial_cmp(&self, o: &Self) -> Option<Ordering> { Some(self.cmp(o)) } }
impl Ord for State {
fn cmp(&self, o: &Self) -> Ordering {
o.f.partial_cmp(&self.f).unwrap_or(Ordering::Equal)
}
}
let start = self.nearest_node(start_pos)?.id;
let goal = self.nearest_node(goal_pos)?.id;
let goal_pos = self.nodes.iter().find(|n| n.id == goal)?.pos;
// Heuristic: straight-line distance to goal. Returns 0.0 if node missing
// (admissible — never overestimates).
let h = |id: u32| -> f64 {
match self.nodes.iter().find(|n| n.id == id) {
Some(n) => (0..3).map(|i| (n.pos[i] - goal_pos[i]).powi(2))
.sum::<f64>().sqrt(),
None => 0.0,
}
};
let mut open = BinaryHeap::new();
let mut came = HashMap::<u32, u32>::new();
let mut g_cost = HashMap::<u32, f64>::new();
open.push(State { f: h(start), g: 0.0, id: start });
g_cost.insert(start, 0.0);
while let Some(State { g, id, .. }) = open.pop() {
if id == goal {
// Reconstruct
let mut path = vec![id];
let mut cur = id;
while let Some(&prev) = came.get(&cur) {
path.push(prev); cur = prev;
}
path.reverse();
return Some(path.iter()
.filter_map(|&i| self.nodes.iter().find(|n| n.id == i))
.collect());
}
for edge in self.edges.iter().filter(|e| e.from == id) {
let ng = g + edge.cost * edge.kind.penalty();
if ng < *g_cost.get(&edge.to).unwrap_or(&f64::MAX) {
g_cost.insert(edge.to, ng);
came.insert(edge.to, id);
open.push(State { f: ng + h(edge.to), g: ng, id: edge.to });
}
}
}
None
}
}
```
-----
### 24.8 V2X — Vehicle-to-Everything broadcast
```rust
// V2X over QUIC — broadcast pose + safety envelope state to:
// RSU (Road-Side Units), other vehicles, traffic management system
#[derive(serde::Serialize, serde::Deserialize, Debug, Clone)]
pub struct V2xBeacon {
pub vehicle_id: u32,
pub pos: [f32; 3], // ENU meters (f32 = 0.1mm precision)
pub vel: [f32; 3], // m/s
pub heading_rad: f32,
pub speed_mps: f32,
pub ts_ms: u32,
pub safety_alert: Option<SafetyAlert>,
}
#[derive(serde::Serialize, serde::Deserialize, Debug, Clone)]
pub enum SafetyAlert {
EmergencyBrake,
ObstacleAhead { dist_m: f32 },
OddViolation,
SensorDegraded { sensor: SensorKind },
}
#[derive(serde::Serialize, serde::Deserialize, Debug, Clone)]
pub enum SensorKind { Lidar, Camera, Radar, Gnss, Imu }
/// V2X broadcaster — sends beacons at 10 Hz over QUIC
pub struct V2xBroadcaster {
pub vehicle_id: u32,
pose_rx: tokio::sync::watch::Receiver<VehiclePose>,
alert_rx: tokio::sync::mpsc::Receiver<SafetyAlert>,
}
impl V2xBroadcaster {
pub async fn run(mut self, server_addr: &str) {
// Connect via QUIC for connection-migration support
// (survives cellular handoff between towers)
loop {
tokio::time::sleep(tokio::time::Duration::from_millis(100)).await;
let pose = self.pose_rx.borrow().clone();
let alert = self.alert_rx.try_recv().ok();
let speed = (0..3).map(|i| pose.velocity[i].powi(2))
.sum::<f64>().sqrt() as f32;
let beacon = V2xBeacon {
vehicle_id: self.vehicle_id,
pos: std::array::from_fn(|i| pose.position[i] as f32),
vel: std::array::from_fn(|i| pose.velocity[i] as f32),
heading_rad: pose.orientation.euler_angles().2 as f32,
speed_mps: speed,
ts_ms: (std::time::SystemTime::now()
.duration_since(std::time::UNIX_EPOCH)
.unwrap().as_millis() & 0xFFFFFFFF) as u32,
safety_alert: alert,
};
let bytes = encode(&beacon);
// send over QUIC datagram (unreliable, lowest latency)
// quinn conn.send_datagram(bytes.into()).ok();
}
}
}
/// V2X receiver — tracks surrounding vehicles in a local ZoneStore
pub struct V2xReceiver {
pub vehicles: std::collections::HashMap<u32, (V2xBeacon, VehiclePose)>,
pub pred_store: ZoneStore, // prediction zones for all nearby vehicles
pub conv: EnuConverter,
}
impl V2xReceiver {
pub fn on_beacon(&mut self, beacon: V2xBeacon) {
// Update prediction zone for this vehicle
let pos = beacon.pos;
let vel = beacon.vel;
let zone = Zone::Cylinder {
center: [pos[0] as f64, pos[1] as f64],
radius_m: 5.0,
z_min: pos[2] as f64 - 2.0,
z_max: pos[2] as f64 + 2.0,
};
self.pred_store.add_zone(beacon.vehicle_id, &zone, &self.conv);
if let Some(alert) = &beacon.safety_alert {
// Escalate: emergency brake ahead → trigger own brake planning
match alert {
SafetyAlert::EmergencyBrake => { /* trigger decel */ }
SafetyAlert::ObstacleAhead { dist_m } => { /* update occupancy */ }
_ => {}
}
}
}
/// All vehicle IDs whose prediction zone overlaps ego vehicle envelope
pub fn collision_candidates(&self, ego_pos: [f64; 3]) -> Vec<u32> {
self.pred_store.query_geodetic(ego_pos[0], ego_pos[1], ego_pos[2], &self.conv)
}
}
```
-----
### 24.9 Full AV pipeline
```rust
pub struct AvStack {
// Sensing
pub imu: ImuFusion,
pub lidar_octree: SharedOctree,
// Environment model
pub behavior_zones: BehaviorZoneStore,
pub hd_map: HdMap,
pub v2x_rx: V2xReceiver,
// Dynamic zones (updated every tick)
pub safety_env: SafetyEnvelope,
pub sensor_fovs: ZoneStore, // lidar + camera + radar FOV zones
// Planning
pub current_route: Vec<u32>, // LaneNode IDs
}
impl AvStack {
pub async fn tick(&mut self, pose: VehiclePose) {
let enu = pose.position;
let conv = EnuConverter::new(enu[0], enu[1], 0.0); // local conv
// 1. Check safety envelope — emergency stop if violated
if safety_check(&self.safety_env, &self.lidar_octree.read().unwrap(), &pose, 30.0) {
self.emergency_stop();
return;
}
// 2. ODD check — pull over if out of domain
if self.behavior_zones.odd_violated(enu, &DirectCartesian) {
self.pull_over();
return;
}
// 3. Speed limit
let limit = self.behavior_zones.speed_limit_mps(enu, &DirectCartesian);
self.set_speed_limit(limit);
// 4. Collision candidates from V2X
let risks = self.v2x_rx.collision_candidates(enu);
if !risks.is_empty() {
self.adjust_for_vehicles(&risks);
}
// 5. Blind spot check — holes in sensor FOV = unknown region
// (use hole scanner on lidar_octree within FOV zones)
// 6. Re-route if needed
if self.current_route.is_empty() {
// request new route from HD map
}
}
fn emergency_stop(&self) { /* actuator command */ }
fn pull_over(&self) { /* safe stop procedure */ }
fn set_speed_limit(&self, _: f32) { /* speed controller */ }
fn adjust_for_vehicles(&self, _: &[u32]) { /* trajectory adjustment */ }
}
```
-----
### 24.10 AV feature additions to Cargo.toml
```toml
[features]
av = ["net", "quic", "gnss", "lidar", "parallel",
"dep:kiddo", # HD map KD-tree nearest node (kiddo 5.x)
"dep:nalgebra"] # already required — EKF, frustum math
```
-----
### 24.11 What the crate covers vs. what it does not
|RTK cm-level positioning |✅ |Section 3–4 |
|IMU dead-reckoning EKF |✅ |Section 24.2 |
|LiDAR occupancy grid |✅ |Octree |
|ODD geofencing |✅ |BehaviorZoneStore |
|Dynamic obstacle zones |✅ |SafetyEnvelope, PredictionZone |
|Sensor FOV blind spot |✅ |LidarFovZone, CameraFrustumZone |
|Traffic behavior zones |✅ |SpeedLimit, SchoolZone, Yield… |
|V2X broadcast/receive |✅ |QUIC beacons |
|HD Map lane graph |✅ |LaneNode + A* routing |
|Low-latency realtime |✅ |QUIC + io_uring, < 1ms |
|Prediction (Gaussian cone)|✅ |PredictionZone |
|Motion planning (MPC, RRT)|❌ |Out of scope — use `trajopt-rs` |
|Traffic light recognition |❌ |Vision/ML — out of scope |
|Localization without GPS |❌ |NDT/point cloud matching — use `ndt-rs`|
|Full autonomy stack |❌ |Crate is spatial middleware only |
-----
## 25. Unit Tests
```rust
// Run with: cargo test --features "std parallel"
#[cfg(test)]
mod tests {
use super::*;
fn test_conv() -> EnuConverter { EnuConverter::new(10.7626, 106.6601, 0.0) }
// ── EnuConverter ─────────────────────────────────────────────
#[test]
fn enu_roundtrip() {
let conv = test_conv();
let lat = 10.7630; let lon = 106.6605; let alt = 15.0;
let enu = conv.to_enu(lat, lon, alt);
let back = conv.from_enu(enu[0], enu[1], enu[2]);
assert!((back[0] - lat).abs() < 1e-8, "lat roundtrip");
assert!((back[1] - lon).abs() < 1e-8, "lon roundtrip");
assert!((back[2] - alt).abs() < 1e-4, "alt roundtrip");
}
#[test]
fn enu_known_distance() {
// 1 degree of latitude ≈ 111,111 m
let conv = EnuConverter::new(0.0, 0.0, 0.0);
let enu = conv.to_enu(1.0, 0.0, 0.0);
assert!((enu[1] - 111_111.0).abs() < 5.0, "1 deg lat ≈ 111 km, got {}", enu[1]);
}
// ── Zone point-in-zone ────────────────────────────────────────
#[test]
fn aabb_inside_outside() {
let conv = test_conv();
let mut s = ZoneStore::from_entries(&[ZoneEntry::new(
1, Zone::Aabb {
min: [10.762, 106.659, 0.0],
max: [10.763, 106.661, 10.0],
}
)], &conv);
assert_eq!(s.query_geodetic(10.7625, 106.660, 5.0, &conv), vec![1]);
assert!(s.query_geodetic(10.7640, 106.660, 5.0, &conv).is_empty());
}
#[test]
fn cylinder_boundary() {
let conv = test_conv();
let s = ZoneStore::from_entries(&[ZoneEntry::new(
2, Zone::Cylinder {
center: [10.7626, 106.6601], radius_m: 50.0, z_min: 0.0, z_max: 20.0,
}
)], &conv);
// Center: inside
assert_eq!(s.query_geodetic(10.7626, 106.6601, 10.0, &conv), vec![2]);
// Above z_max: outside
assert!(s.query_geodetic(10.7626, 106.6601, 25.0, &conv).is_empty());
}
#[test]
fn extruded_polygon_inside_outside() {
let conv = test_conv();
let s = ZoneStore::from_entries(&[ZoneEntry::new(
3, Zone::ExtrudedPolygon {
ring: vec![
[10.7620, 106.6595],
[10.7630, 106.6595],
[10.7630, 106.6610],
[10.7620, 106.6610],
[10.7620, 106.6595],
],
z_min: 0.0, z_max: 15.0,
}
)], &conv);
assert_eq!(s.query_geodetic(10.7625, 106.6600, 5.0, &conv), vec![3]);
assert!(s.query_geodetic(10.7615, 106.6600, 5.0, &conv).is_empty()); // outside
}
// ── ZoneStore add / remove ────────────────────────────────────
#[test]
fn add_remove_roundtrip() {
let conv = test_conv();
let mut s = ZoneStore::from_entries(&[], &conv);
s.add_zone(42, &Zone::Cylinder {
center: [10.7626, 106.6601], radius_m: 10.0, z_min: 0.0, z_max: 5.0,
}, &conv);
assert_eq!(s.len(), 1);
assert!(!s.query_geodetic(10.7626, 106.6601, 2.0, &conv).is_empty());
s.remove(42);
assert_eq!(s.len(), 0);
assert!(s.query_geodetic(10.7626, 106.6601, 2.0, &conv).is_empty());
}
// ── Octree ────────────────────────────────────────────────────
#[test]
fn octree_insert_query() {
let mut tree = OctreeNode::new([0.0; 3], 100.0);
tree.insert([1.0, 2.0, 3.0], 4);
tree.insert([50.0, 50.0, 50.0], 4);
let pts = tree.range_query([-5.0,-5.0,-5.0], [5.0,5.0,5.0]);
assert_eq!(pts.len(), 1);
}
#[test]
fn depth_for_accuracy_sanity() {
// 1000m world, 5cm accuracy → depth 14 expected
assert_eq!(depth_for_accuracy(0.05, 1000.0), 14);
// 1000m world, 1m accuracy → depth 10 expected
assert_eq!(depth_for_accuracy(1.0, 1000.0), 10);
}
// ── Hole scanner ─────────────────────────────────────────────
#[test]
fn hole_scanner_finds_empty_zone() {
let conv = test_conv();
let mut s = ZoneStore::from_entries(&[ZoneEntry::new(
1, Zone::Aabb {
min: [10.7620, 106.6595, 0.0],
max: [10.7630, 106.6605, 10.0],
}
)], &conv);
// Empty octree → entire zone is holes
let tree = OctreeNode::new([0.0;3], 500.0);
let holes = scan_holes(&tree, &s, 1, 6);
assert!(!holes.is_empty(), "expected holes in empty zone");
}
#[test]
fn hole_scanner_no_holes_when_full() {
let conv = test_conv();
let zone = Zone::Cylinder {
center: [10.7626, 106.6601], radius_m: 5.0, z_min: 0.0, z_max: 5.0,
};
let s = ZoneStore::from_entries(&[ZoneEntry::new(1, zone)], &conv);
let mut tree = OctreeNode::new([0.0;3], 500.0);
// Flood the zone with points at depth-7 resolution (~7.8m / 64 ≈ 0.12m voxels)
// Just insert at center — enough to mark the single relevant voxel occupied
let center_enu = conv.to_enu(10.7626, 106.6601, 2.5);
tree.insert(center_enu, 1);
let holes = scan_holes(&tree, &s, 1, 5);
// Center voxel occupied — remaining holes are the peripheral ones; not asserting
// exact count, just that the scanner runs without panic
let _ = holes;
}
// ── ENU accuracy (ECEF vs flat-earth) ─────────────────────────
#[test]
fn enu_accuracy_at_50km() {
// At 50 km from origin, ECEF-based ENU should be accurate to < 1 cm
let conv = EnuConverter::new(0.0, 0.0, 0.0);
// Move ~50 km north (≈ 0.45 degrees)
let enu = conv.to_enu(0.45, 0.0, 0.0);
let expected_north = 50_009.0; // approximate 50 km
assert!((enu[1] - expected_north).abs() < 50.0,
"north should be ~50km, got {}", enu[1]);
}
}
```
> Run with `cargo test --features std` for core tests,
> `cargo test --features "std net parallel"` for the full suite.
> Add `criterion` benchmarks under `benches/` for performance regression tracking.