1pub const SRID_WGS84: u32 = 4326;
9pub const SRID_GDA2020: u32 = 7844;
10pub const SRID_WEB_MERCATOR: u32 = 3857;
11pub const SRID_MGA_ZONE_55: u32 = 7855;
12pub const SRID_MGA_ZONE_56: u32 = 7856;
13
14pub const VERTICAL_DATUM_AHD: u16 = 5711;
15pub const VERTICAL_DATUM_ELLIPSOIDAL: u16 = 4979;
16
17#[derive(Debug, Clone, Copy, PartialEq, Eq)]
19pub enum DataEpochKind {
20 MeasuredObservation,
21 HistoricalReanalysis,
22 ForecastProjection,
23 SyntheticScenario,
24}
25
26#[derive(Debug, Clone, Copy, PartialEq)]
28pub struct GeodeticCoord {
29 pub latitude_deg: f64,
30 pub longitude_deg: f64,
31 pub elevation_ahd_mm: i32,
32}
33
34impl GeodeticCoord {
35 pub const fn new(latitude_deg: f64, longitude_deg: f64, elevation_ahd_mm: i32) -> Self {
36 Self { latitude_deg, longitude_deg, elevation_ahd_mm }
37 }
38}
39
40#[derive(Debug, Clone, Copy, PartialEq)]
42pub struct TimestampedGeodeticSample {
43 pub coord: GeodeticCoord,
44 pub srid: u32,
45 pub vertical_datum: u16,
46 pub epoch_year: f32,
47 pub unix_time_nanos: u64,
48 pub epoch_kind: DataEpochKind,
49}
50
51#[derive(Debug, Clone, Copy, PartialEq, Eq)]
53pub struct VerticalDatumTransform {
54 pub from_datum: u16,
55 pub to_datum: u16,
56 pub geoid_undulation_n_mm: i32,
57}
58
59#[derive(Debug, Clone, PartialEq)]
61pub enum FrameValidationError {
62 UnsupportedSrid(u32),
63 VerticalDatumMismatch { expected: u16, found: u16 },
64 InvalidLatitude(f64),
65 InvalidLongitude(f64),
66 EpochDriftExceeded { frame_epoch: f32, sample_epoch: f32 },
67 UnmarkedForecastInObservationStream,
68}
69
70impl core::fmt::Display for FrameValidationError {
71 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
72 match self {
73 Self::UnsupportedSrid(s) => write!(f, "Unsupported SRID: {s}"),
74 Self::VerticalDatumMismatch { expected, found } => write!(f, "Datum mismatch: exp {expected}, got {found}"),
75 Self::InvalidLatitude(lat) => write!(f, "Invalid latitude [-90, +90]: {lat}"),
76 Self::InvalidLongitude(lon) => write!(f, "Invalid longitude [-180, +180]: {lon}"),
77 Self::EpochDriftExceeded { frame_epoch, sample_epoch } => write!(f, "Epoch drift: frame={frame_epoch}, sample={sample_epoch}"),
78 Self::UnmarkedForecastInObservationStream => write!(f, "Unmarked forecast projection in observation stream"),
79 }
80 }
81}
82
83impl std::error::Error for FrameValidationError {}
84
85#[derive(Debug, Clone, Copy, PartialEq)]
87pub struct GeodeticFrame {
88 pub srid: u32,
89 pub vertical_datum: u16,
90 pub epoch_year: f32, pub origin_lat_deg: f64,
92 pub origin_lon_deg: f64,
93 pub origin_elevation_mm: i32,
94}
95
96impl Default for GeodeticFrame {
97 fn default() -> Self {
98 Self {
99 srid: SRID_GDA2020,
100 vertical_datum: VERTICAL_DATUM_AHD,
101 epoch_year: 2020.0,
102 origin_lat_deg: -35.78, origin_lon_deg: 148.29,
104 origin_elevation_mm: 350_000, }
106 }
107}
108
109impl GeodeticFrame {
110 pub fn validate_sample(
112 &self,
113 sample: &TimestampedGeodeticSample,
114 allow_forecast: bool,
115 ) -> Result<(), FrameValidationError> {
116 let lat = sample.coord.latitude_deg;
117 if !(-90.0..=90.0).contains(&lat) || lat.is_nan() {
118 return Err(FrameValidationError::InvalidLatitude(lat));
119 }
120 let lon = sample.coord.longitude_deg;
121 if !(-180.0..=180.0).contains(&lon) || lon.is_nan() {
122 return Err(FrameValidationError::InvalidLongitude(lon));
123 }
124 if sample.srid != self.srid {
125 return Err(FrameValidationError::UnsupportedSrid(sample.srid));
126 }
127 if sample.vertical_datum != self.vertical_datum {
128 return Err(FrameValidationError::VerticalDatumMismatch {
129 expected: self.vertical_datum,
130 found: sample.vertical_datum,
131 });
132 }
133 if (sample.epoch_year - self.epoch_year).abs() > 5.0 {
134 return Err(FrameValidationError::EpochDriftExceeded {
135 frame_epoch: self.epoch_year,
136 sample_epoch: sample.epoch_year,
137 });
138 }
139 if !allow_forecast && sample.epoch_kind == DataEpochKind::ForecastProjection {
140 return Err(FrameValidationError::UnmarkedForecastInObservationStream);
141 }
142 Ok(())
143 }
144
145 pub fn transform_and_project_local_mm(
147 &self,
148 sample: &TimestampedGeodeticSample,
149 vertical_transform: Option<&VerticalDatumTransform>,
150 ) -> Result<(i64, i64, i32), FrameValidationError> {
151 let lat = sample.coord.latitude_deg;
152 if !(-90.0..=90.0).contains(&lat) || lat.is_nan() {
153 return Err(FrameValidationError::InvalidLatitude(lat));
154 }
155 let lon = sample.coord.longitude_deg;
156 if !(-180.0..=180.0).contains(&lon) || lon.is_nan() {
157 return Err(FrameValidationError::InvalidLongitude(lon));
158 }
159 if sample.srid != self.srid {
160 return Err(FrameValidationError::UnsupportedSrid(sample.srid));
161 }
162 if (sample.epoch_year - self.epoch_year).abs() > 5.0 {
163 return Err(FrameValidationError::EpochDriftExceeded {
164 frame_epoch: self.epoch_year,
165 sample_epoch: sample.epoch_year,
166 });
167 }
168
169 let target_elevation_mm = if sample.vertical_datum == self.vertical_datum {
170 sample.coord.elevation_ahd_mm
171 } else {
172 let tx = vertical_transform.ok_or(FrameValidationError::VerticalDatumMismatch {
173 expected: self.vertical_datum,
174 found: sample.vertical_datum,
175 })?;
176 if tx.from_datum != sample.vertical_datum || tx.to_datum != self.vertical_datum {
177 return Err(FrameValidationError::VerticalDatumMismatch {
178 expected: self.vertical_datum,
179 found: sample.vertical_datum,
180 });
181 }
182 if tx.from_datum == VERTICAL_DATUM_ELLIPSOIDAL && tx.to_datum == VERTICAL_DATUM_AHD {
183 sample.coord.elevation_ahd_mm - tx.geoid_undulation_n_mm
184 } else if tx.from_datum == VERTICAL_DATUM_AHD && tx.to_datum == VERTICAL_DATUM_ELLIPSOIDAL {
185 sample.coord.elevation_ahd_mm + tx.geoid_undulation_n_mm
186 } else {
187 sample.coord.elevation_ahd_mm - tx.geoid_undulation_n_mm
188 }
189 };
190
191 let adjusted = GeodeticCoord { latitude_deg: lat, longitude_deg: lon, elevation_ahd_mm: target_elevation_mm };
192 Ok(self.project_to_local_mm(&adjusted))
193 }
194
195 #[inline]
197 pub fn project_to_local_mm(&self, coord: &GeodeticCoord) -> (i64, i64, i32) {
198 const EARTH_RADIUS_MM: f64 = 6_378_137_000.0;
199 let lat_rad = coord.latitude_deg.to_radians();
200 let origin_lat_rad = self.origin_lat_deg.to_radians();
201 let mean_lat_rad = (lat_rad + origin_lat_rad) * 0.5;
202
203 let d_lat = (coord.latitude_deg - self.origin_lat_deg).to_radians();
204 let d_lon = (coord.longitude_deg - self.origin_lon_deg).to_radians();
205
206 let y_mm = (d_lat * EARTH_RADIUS_MM) as i64;
207 let x_mm = (d_lon * EARTH_RADIUS_MM * mean_lat_rad.cos()) as i64;
208 let z_mm = coord.elevation_ahd_mm - self.origin_elevation_mm;
209 (x_mm, y_mm, z_mm)
210 }
211
212 #[inline]
214 pub fn unproject_from_local_mm(&self, x_mm: i64, y_mm: i64) -> (f64, f64) {
215 const EARTH_RADIUS_MM: f64 = 6_378_137_000.0;
216 let origin_lat_rad = self.origin_lat_deg.to_radians();
217 let d_lat_rad = (y_mm as f64) / EARTH_RADIUS_MM;
218 let lat_deg = self.origin_lat_deg + d_lat_rad.to_degrees();
219 let mean_lat_rad = (lat_deg.to_radians() + origin_lat_rad) * 0.5;
220 let d_lon_rad = (x_mm as f64) / (EARTH_RADIUS_MM * mean_lat_rad.cos());
221 let lon_deg = self.origin_lon_deg + d_lon_rad.to_degrees();
222 (lat_deg, lon_deg)
223 }
224
225 pub fn to_dggs_cell_hash(&self, coord: &GeodeticCoord, resolution_level: u8) -> u64 {
227 let res = resolution_level.min(15) as u32;
228 let lat_norm = ((coord.latitude_deg + 90.0) / 180.0).clamp(0.0, 1.0);
229 let lon_norm = ((coord.longitude_deg + 180.0) / 360.0).clamp(0.0, 1.0);
230 let grid_size = 1u64 << res;
231 let grid_x = (lon_norm * (grid_size as f64)).floor() as u64;
232 let grid_y = (lat_norm * (grid_size as f64)).floor() as u64;
233
234 let mut hash = 0xcbf29ce484222325u64;
235 hash = (hash ^ (res as u64)).wrapping_mul(0x100000001b3);
236 hash = (hash ^ grid_x).wrapping_mul(0x100000001b3);
237 hash = (hash ^ grid_y).wrapping_mul(0x100000001b3);
238 hash
239 }
240
241 pub fn to_spatiotemporal_point(
243 &self,
244 coord: &GeodeticCoord,
245 unix_time_nanos: u64,
246 velocity_m_s: [f64; 3],
247 ) -> qualia_core_db::modalities::spatiotemporal_predicates::SpatiotemporalPoint {
248 let (x_mm, y_mm, z_mm) = self.project_to_local_mm(coord);
249 let mut pt = qualia_core_db::modalities::spatiotemporal_predicates::SpatiotemporalPoint::new(
250 (x_mm as f64) / 1000.0,
251 (y_mm as f64) / 1000.0,
252 (z_mm as f64) / 1000.0,
253 self.srid,
254 unix_time_nanos,
255 (coord.elevation_ahd_mm as f64) / 1000.0,
256 );
257 pt.velocity_m_s = velocity_m_s;
258 pt.reference_frame_id = (self.srid as u64) | ((self.vertical_datum as u64) << 32);
259 pt
260 }
261
262 pub fn evaluate_light_travel_offset_nanos(&self, coord_a: &GeodeticCoord, coord_b: &GeodeticCoord) -> u64 {
264 let pt_a = self.to_spatiotemporal_point(coord_a, 0, [0.0; 3]);
265 let pt_b = self.to_spatiotemporal_point(coord_b, 0, [0.0; 3]);
266 qualia_core_db::modalities::spatiotemporal_predicates::evaluate_light_travel_offset_nanos(&pt_a, &pt_b)
267 }
268
269 pub fn evaluate_proper_time_drift_nanos(&self, coord: &GeodeticCoord, speed_m_s: f64, duration_seconds: f64) -> f64 {
271 qualia_core_db::modalities::spatiotemporal_predicates::evaluate_proper_time_drift_nanos(
272 (coord.elevation_ahd_mm as f64) / 1000.0,
273 speed_m_s,
274 duration_seconds,
275 )
276 }
277}
278
279#[derive(Debug, Clone, Copy, PartialEq)]
282pub struct FloatingOriginFrame {
283 pub anchor_id: u64,
285 pub healpix_index: u64,
287 pub octree_cell: [i64; 3],
289 pub local_origin_m: [f64; 3],
291 pub residual_m: [f64; 3],
293 pub reanchor_count: u64,
295}
296
297impl FloatingOriginFrame {
298 pub fn new(octree_cell: [i64; 3], residual_m: [f64; 3]) -> Self {
300 let healpix_index = compute_healpix_index(octree_cell, 64);
301 let anchor_id = compute_sector_anchor_id(octree_cell, healpix_index);
302 Self {
303 anchor_id,
304 healpix_index,
305 octree_cell,
306 local_origin_m: [0.0, 0.0, 0.0],
307 residual_m,
308 reanchor_count: 0,
309 }
310 }
311}
312
313pub fn compute_healpix_index(octree_cell: [i64; 3], nside: u64) -> u64 {
315 let nside = nside.max(1);
316 let x = octree_cell[0] as f64;
317 let y = octree_cell[1] as f64;
318 let z = octree_cell[2] as f64;
319 let r = (x * x + y * y + z * z).sqrt();
320 if r == 0.0 {
321 return 0;
322 }
323 let cos_theta = (z / r).clamp(-1.0, 1.0);
324 let phi = y.atan2(x).rem_euclid(2.0 * std::f64::consts::PI);
325 let base_pix = ((phi / (2.0 * std::f64::consts::PI)) * 12.0).floor() as u64 % 12;
326 let z_band = (((1.0 - cos_theta) * 0.5) * (nside as f64)).floor() as u64 % nside;
327 let az_band = ((phi / (2.0 * std::f64::consts::PI)) * (nside as f64)).floor() as u64 % nside;
328 base_pix * nside * nside + z_band * nside + az_band
329}
330
331fn compute_sector_anchor_id(octree_cell: [i64; 3], healpix_index: u64) -> u64 {
332 let mut h: u64 = 0xcbf29ce484222325;
333 for coord in octree_cell {
334 for b in coord.to_le_bytes() {
335 h ^= b as u64;
336 h = h.wrapping_mul(0x100000001b3);
337 }
338 }
339 for b in healpix_index.to_le_bytes() {
340 h ^= b as u64;
341 h = h.wrapping_mul(0x100000001b3);
342 }
343 h
344}
345
346pub fn reanchor_origin(
351 current: &FloatingOriginFrame,
352 step_delta_m: [f64; 3],
353 sector_radius_m: f64,
354) -> FloatingOriginFrame {
355 let radius = sector_radius_m.abs().max(1.0);
356 let candidate = [
357 current.local_origin_m[0] + step_delta_m[0],
358 current.local_origin_m[1] + step_delta_m[1],
359 current.local_origin_m[2] + step_delta_m[2],
360 ];
361 let dist_sq = candidate[0] * candidate[0]
362 + candidate[1] * candidate[1]
363 + candidate[2] * candidate[2];
364 if dist_sq >= radius * radius {
365 let mut next_cell = current.octree_cell;
366 let mut moved = false;
367 for i in 0..3 {
368 let steps = (candidate[i] / radius).trunc() as i64;
369 if steps != 0 {
370 next_cell[i] = next_cell[i].saturating_add(steps);
371 moved = true;
372 }
373 }
374 if !moved {
375 let mut dom = 0;
376 if candidate[1].abs() > candidate[dom].abs() {
377 dom = 1;
378 }
379 if candidate[2].abs() > candidate[dom].abs() {
380 dom = 2;
381 }
382 let dir = if candidate[dom] >= 0.0 { 1 } else { -1 };
383 next_cell[dom] = next_cell[dom].saturating_add(dir);
384 }
385 let healpix_index = compute_healpix_index(next_cell, 64);
386 let anchor_id = compute_sector_anchor_id(next_cell, healpix_index);
387 FloatingOriginFrame {
388 anchor_id,
389 healpix_index,
390 octree_cell: next_cell,
391 local_origin_m: [0.0, 0.0, 0.0],
392 residual_m: current.residual_m,
393 reanchor_count: current.reanchor_count + 1,
394 }
395 } else {
396 FloatingOriginFrame {
397 anchor_id: current.anchor_id,
398 healpix_index: current.healpix_index,
399 octree_cell: current.octree_cell,
400 local_origin_m: candidate,
401 residual_m: current.residual_m,
402 reanchor_count: current.reanchor_count,
403 }
404 }
405}
406
407#[cfg(test)]
408mod tests {
409 use super::*;
410
411 #[test]
412 fn test_valid_gda2020_ahd_projection() {
413 let frame = GeodeticFrame::default();
414 let sample = TimestampedGeodeticSample {
415 coord: GeodeticCoord::new(-35.795, 148.315, 385_000),
416 srid: SRID_GDA2020,
417 vertical_datum: VERTICAL_DATUM_AHD,
418 epoch_year: 2020.0,
419 unix_time_nanos: 1_700_000_000_000_000_000,
420 epoch_kind: DataEpochKind::MeasuredObservation,
421 };
422 assert!(frame.validate_sample(&sample, false).is_ok());
423 let (x_mm, y_mm, z_mm) = frame.transform_and_project_local_mm(&sample, None).unwrap();
424 assert!(x_mm > 0);
425 assert!(y_mm < 0);
426 assert_eq!(z_mm, 35_000);
427 }
428
429 #[test]
430 fn test_explicit_ellipsoidal_to_ahd_transform() {
431 let frame = GeodeticFrame::default();
432 let sample = TimestampedGeodeticSample {
433 coord: GeodeticCoord::new(-35.78, 148.29, 405_000),
434 srid: SRID_GDA2020,
435 vertical_datum: VERTICAL_DATUM_ELLIPSOIDAL,
436 epoch_year: 2020.0,
437 unix_time_nanos: 1_700_000_000_000_000_000,
438 epoch_kind: DataEpochKind::MeasuredObservation,
439 };
440 let tx = VerticalDatumTransform {
441 from_datum: VERTICAL_DATUM_ELLIPSOIDAL,
442 to_datum: VERTICAL_DATUM_AHD,
443 geoid_undulation_n_mm: 20_000,
444 };
445 let (x_mm, y_mm, z_mm) = frame.transform_and_project_local_mm(&sample, Some(&tx)).unwrap();
446 assert_eq!(x_mm, 0);
447 assert_eq!(y_mm, 0);
448 assert_eq!(z_mm, 35_000);
449 }
450
451 #[test]
452 fn test_negative_rejects_mixed_vertical_datums() {
453 let frame = GeodeticFrame::default();
454 let sample = TimestampedGeodeticSample {
455 coord: GeodeticCoord::new(-35.78, 148.29, 405_000),
456 srid: SRID_GDA2020,
457 vertical_datum: VERTICAL_DATUM_ELLIPSOIDAL,
458 epoch_year: 2020.0,
459 unix_time_nanos: 1_700_000_000_000_000_000,
460 epoch_kind: DataEpochKind::MeasuredObservation,
461 };
462 let err = frame.transform_and_project_local_mm(&sample, None).unwrap_err();
463 assert_eq!(err, FrameValidationError::VerticalDatumMismatch {
464 expected: VERTICAL_DATUM_AHD,
465 found: VERTICAL_DATUM_ELLIPSOIDAL,
466 });
467 }
468
469 #[test]
470 fn test_negative_rejects_invalid_coordinates() {
471 let frame = GeodeticFrame::default();
472 let invalid_lat = TimestampedGeodeticSample {
473 coord: GeodeticCoord::new(-95.0, 148.29, 350_000),
474 srid: SRID_GDA2020,
475 vertical_datum: VERTICAL_DATUM_AHD,
476 epoch_year: 2020.0,
477 unix_time_nanos: 0,
478 epoch_kind: DataEpochKind::MeasuredObservation,
479 };
480 assert!(matches!(frame.validate_sample(&invalid_lat, false), Err(FrameValidationError::InvalidLatitude(_))));
481
482 let invalid_lon = TimestampedGeodeticSample {
483 coord: GeodeticCoord::new(-35.78, 195.0, 350_000),
484 srid: SRID_GDA2020,
485 vertical_datum: VERTICAL_DATUM_AHD,
486 epoch_year: 2020.0,
487 unix_time_nanos: 0,
488 epoch_kind: DataEpochKind::MeasuredObservation,
489 };
490 assert!(matches!(frame.validate_sample(&invalid_lon, false), Err(FrameValidationError::InvalidLongitude(_))));
491 }
492
493 #[test]
494 fn test_negative_rejects_unmarked_forecast_in_observation_stream() {
495 let frame = GeodeticFrame::default();
496 let forecast_sample = TimestampedGeodeticSample {
497 coord: GeodeticCoord::new(-35.78, 148.29, 350_000),
498 srid: SRID_GDA2020,
499 vertical_datum: VERTICAL_DATUM_AHD,
500 epoch_year: 2020.0,
501 unix_time_nanos: 1_700_000_000_000_000_000,
502 epoch_kind: DataEpochKind::ForecastProjection,
503 };
504 assert_eq!(frame.validate_sample(&forecast_sample, false), Err(FrameValidationError::UnmarkedForecastInObservationStream));
505 assert!(frame.validate_sample(&forecast_sample, true).is_ok());
506 }
507
508 #[test]
509 fn test_epoch_drift_validation() {
510 let frame = GeodeticFrame::default();
511 let drifted = TimestampedGeodeticSample {
512 coord: GeodeticCoord::new(-35.78, 148.29, 350_000),
513 srid: SRID_GDA2020,
514 vertical_datum: VERTICAL_DATUM_AHD,
515 epoch_year: 2028.5,
516 unix_time_nanos: 0,
517 epoch_kind: DataEpochKind::MeasuredObservation,
518 };
519 assert!(matches!(frame.validate_sample(&drifted, false), Err(FrameValidationError::EpochDriftExceeded { .. })));
520 }
521
522 #[test]
523 fn test_earth_to_alpha_centauri_floating_origin_reanchor() {
524 const ALPHA_CENTAURI_M: f64 = 4.1343e16;
525 let ingested_residual_m = [0.000_42, -0.000_19, 0.000_73];
526 let naive_global = std::hint::black_box(ALPHA_CENTAURI_M + ingested_residual_m[0]);
527 let naive_recovered = naive_global - ALPHA_CENTAURI_M;
528 assert!((naive_recovered - ingested_residual_m[0]).abs() > 1e-4);
529
530 let sector_radius_m = 4.1343e14;
531 let step_delta_m = [sector_radius_m, 1.0e13, -2.0e13];
532 let mut frame = FloatingOriginFrame::new([0, 0, 0], ingested_residual_m);
533 for _ in 0..100 {
534 frame = reanchor_origin(&frame, step_delta_m, sector_radius_m);
535 assert_eq!(frame.local_origin_m, [0.0, 0.0, 0.0]);
536 }
537 assert_eq!(frame.reanchor_count, 100);
538 assert_eq!(frame.octree_cell[0], 100);
539 for axis in 0..3 {
540 assert!((frame.residual_m[axis] - ingested_residual_m[axis]).abs() < 1e-12);
541 }
542 }
543}