1use crate::cosmic::transforms::Geodetic;
6use crate::value::Value;
7use std::collections::BTreeMap;
8
9#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
11pub enum ArFrameLevel {
12 L60GeodeticAnchor,
14 L61VioWorldRoot,
16 L62HeadPose,
18 L63EyeFrustum,
20}
21
22impl ArFrameLevel {
23 pub fn as_str(&self) -> &'static str {
25 match self {
26 Self::L60GeodeticAnchor => "L_6.0",
27 Self::L61VioWorldRoot => "L_6.1",
28 Self::L62HeadPose => "L_6.2",
29 Self::L63EyeFrustum => "L_6.3",
30 }
31 }
32}
33
34#[derive(Debug, Clone, PartialEq)]
36pub struct SpatialAnchor {
37 pub anchor_id: String,
38 pub parent_frame: String,
39 pub geodetic_anchor: [f64; 3], pub enu_offset: [f32; 3], pub orientation_quat: [f32; 4], pub confidence_radius_mm: f32,
43 pub vps_mesh_signature: Vec<u8>,
44}
45
46impl SpatialAnchor {
47 pub fn new(anchor_id: &str, geodetic: [f64; 3]) -> Self {
49 Self {
50 anchor_id: anchor_id.into(),
51 parent_frame: "urn:omni:v1:physical:observable:standard:earth:wgs84".into(),
52 geodetic_anchor: geodetic,
53 enu_offset: [0.0, 0.0, 0.0],
54 orientation_quat: [1.0, 0.0, 0.0, 0.0], confidence_radius_mm: 10.0,
56 vps_mesh_signature: Vec::new(),
57 }
58 }
59
60 pub fn with_enu_offset(mut self, east: f32, north: f32, up: f32) -> Self {
62 self.enu_offset = [east, north, up];
63 self
64 }
65
66 pub fn with_orientation(mut self, w: f32, x: f32, y: f32, z: f32) -> Self {
68 self.orientation_quat = [w, x, y, z];
69 self
70 }
71
72 pub fn with_confidence(mut self, radius_mm: f32) -> Self {
74 self.confidence_radius_mm = radius_mm;
75 self
76 }
77
78 pub fn geodetic(&self) -> Geodetic {
80 Geodetic {
81 lat_deg: self.geodetic_anchor[0],
82 lon_deg: self.geodetic_anchor[1],
83 alt_m: self.geodetic_anchor[2],
84 }
85 }
86
87 pub fn is_submillimeter(&self) -> bool {
89 self.confidence_radius_mm < 1.0
90 }
91
92 pub fn is_quaternion_valid(&self) -> bool {
94 let [w, x, y, z] = self.orientation_quat;
95 let norm = (w * w + x * x + y * y + z * z).sqrt();
96 (norm - 1.0).abs() < 1e-5
97 }
98
99 pub fn to_value(&self) -> Value {
101 let mut rec = BTreeMap::new();
102 rec.insert("anchor_id".into(), Value::String(self.anchor_id.clone()));
103 rec.insert(
104 "parent_frame".into(),
105 Value::String(self.parent_frame.clone()),
106 );
107 rec.insert(
108 "geodetic_anchor".into(),
109 Value::List(
110 self.geodetic_anchor
111 .iter()
112 .map(|v| Value::F64(*v))
113 .collect(),
114 ),
115 );
116 rec.insert(
117 "enu_offset".into(),
118 Value::List(
119 self.enu_offset
120 .iter()
121 .map(|v| Value::F64(*v as f64))
122 .collect(),
123 ),
124 );
125 rec.insert(
126 "orientation_quat".into(),
127 Value::List(
128 self.orientation_quat
129 .iter()
130 .map(|v| Value::F64(*v as f64))
131 .collect(),
132 ),
133 );
134 rec.insert(
135 "confidence_radius_mm".into(),
136 Value::F64(self.confidence_radius_mm as f64),
137 );
138 Value::Record(rec)
139 }
140}
141
142#[derive(Debug, Clone, Copy, PartialEq)]
144pub struct HeadPose {
145 pub position: [f32; 3],
147 pub orientation: [f32; 4],
149 pub timestamp_ns: u64,
151}
152
153impl HeadPose {
154 pub fn identity() -> Self {
156 Self {
157 position: [0.0, 0.0, 0.0],
158 orientation: [1.0, 0.0, 0.0, 0.0],
159 timestamp_ns: 0,
160 }
161 }
162
163 pub fn translated(&self, dx: f32, dy: f32, dz: f32) -> Self {
165 Self {
166 position: [
167 self.position[0] + dx,
168 self.position[1] + dy,
169 self.position[2] + dz,
170 ],
171 orientation: self.orientation,
172 timestamp_ns: self.timestamp_ns,
173 }
174 }
175}
176
177#[derive(Debug, Clone)]
179pub struct VioWorldRoot {
180 pub init_anchor: SpatialAnchor,
182 pub pose_history: Vec<HeadPose>,
184 pub is_stable: bool,
186}
187
188impl VioWorldRoot {
189 pub fn new(anchor: SpatialAnchor) -> Self {
191 Self {
192 init_anchor: anchor,
193 pose_history: Vec::new(),
194 is_stable: false,
195 }
196 }
197
198 pub fn update_pose(&mut self, pose: HeadPose) {
200 self.pose_history.push(pose);
201 if self.pose_history.len() >= 30 {
203 self.is_stable = true;
204 }
205 }
206
207 pub fn current_pose(&self) -> Option<&HeadPose> {
209 self.pose_history.last()
210 }
211
212 pub fn check_stability(&self) -> bool {
215 if self.pose_history.len() < 2 {
216 return false;
217 }
218 if !self.init_anchor.is_submillimeter() {
220 return false;
221 }
222 let recent = &self.pose_history;
224 if recent.len() < 2 {
225 return false;
226 }
227 let last = recent[recent.len() - 1];
228 let prev = recent[recent.len() - 2];
229 let dx = last.position[0] - prev.position[0];
230 let dy = last.position[1] - prev.position[1];
231 let dz = last.position[2] - prev.position[2];
232 let drift = (dx * dx + dy * dy + dz * dz).sqrt();
233 drift < 0.0005
235 }
236}
237
238#[cfg(test)]
239mod tests {
240 use super::*;
241
242 #[test]
243 fn spatial_anchor_default() {
244 let a = SpatialAnchor::new("anchor-1", [37.8080, -122.4177, 10.0]);
245 assert_eq!(a.anchor_id, "anchor-1");
246 assert_eq!(a.enu_offset, [0.0, 0.0, 0.0]);
247 assert!(a.is_quaternion_valid()); }
249
250 #[test]
251 fn spatial_anchor_with_offset() {
252 let a = SpatialAnchor::new("anchor-1", [37.8, -122.4, 10.0]).with_enu_offset(5.0, 3.0, 1.0);
253 assert_eq!(a.enu_offset, [5.0, 3.0, 1.0]);
254 }
255
256 #[test]
257 fn spatial_anchor_submillimeter() {
258 let a = SpatialAnchor::new("precise", [37.8, -122.4, 10.0]).with_confidence(0.5);
259 assert!(a.is_submillimeter());
260 }
261
262 #[test]
263 fn spatial_anchor_not_submillimeter() {
264 let a = SpatialAnchor::new("rough", [37.8, -122.4, 10.0]).with_confidence(50.0);
265 assert!(!a.is_submillimeter());
266 }
267
268 #[test]
269 fn spatial_anchor_geodetic() {
270 let a = SpatialAnchor::new("a", [37.8, -122.4, 10.0]);
271 let g = a.geodetic();
272 assert_eq!(g.lat_deg, 37.8);
273 assert_eq!(g.lon_deg, -122.4);
274 }
275
276 #[test]
277 fn spatial_anchor_to_value() {
278 let a = SpatialAnchor::new("test", [37.8, -122.4, 10.0]);
279 let v = a.to_value();
280 match v {
281 Value::Record(r) => {
282 assert_eq!(r.get("anchor_id"), Some(&Value::String("test".into())));
283 assert!(r.contains_key("geodetic_anchor"));
284 }
285 _ => panic!("expected Record"),
286 }
287 }
288
289 #[test]
290 fn head_pose_identity() {
291 let p = HeadPose::identity();
292 assert_eq!(p.position, [0.0, 0.0, 0.0]);
293 assert_eq!(p.orientation, [1.0, 0.0, 0.0, 0.0]);
294 }
295
296 #[test]
297 fn head_pose_translated() {
298 let p = HeadPose::identity();
299 let p2 = p.translated(1.0, 2.0, 3.0);
300 assert_eq!(p2.position, [1.0, 2.0, 3.0]);
301 }
302
303 #[test]
304 fn vio_world_root_stability() {
305 let anchor = SpatialAnchor::new("root", [37.8, -122.4, 10.0]).with_confidence(0.3);
306 let mut root = VioWorldRoot::new(anchor);
307 for i in 0..30 {
309 root.update_pose(HeadPose {
310 position: [i as f32 * 0.0001, 0.0, 0.0], orientation: [1.0, 0.0, 0.0, 0.0],
312 timestamp_ns: i as u64 * 1_000_000,
313 });
314 }
315 assert!(root.is_stable);
316 assert!(root.check_stability());
317 }
318
319 #[test]
320 fn vio_world_root_unstable_with_drift() {
321 let anchor = SpatialAnchor::new("root", [37.8, -122.4, 10.0]).with_confidence(0.3);
322 let mut root = VioWorldRoot::new(anchor);
323 root.update_pose(HeadPose::identity());
324 root.update_pose(HeadPose {
326 position: [10.0, 0.0, 0.0], orientation: [1.0, 0.0, 0.0, 0.0],
328 timestamp_ns: 1_000_000,
329 });
330 assert!(!root.check_stability());
331 }
332
333 #[test]
334 fn ar_frame_level_names() {
335 assert_eq!(ArFrameLevel::L60GeodeticAnchor.as_str(), "L_6.0");
336 assert_eq!(ArFrameLevel::L63EyeFrustum.as_str(), "L_6.3");
337 }
338
339 #[test]
340 fn quaternion_validity_check() {
341 let mut a = SpatialAnchor::new("a", [0.0; 3]);
342 a.orientation_quat = [2.0, 0.0, 0.0, 0.0]; assert!(!a.is_quaternion_valid());
344 }
345}