sim_lib_view_spatial/
world.rs

1//! World-anchor resolution for Viture spatial panels.
2
3use std::collections::BTreeMap;
4
5use sim_kernel::{Error, Expr, Result, Symbol};
6use sim_lib_scene::{AnchorSpace, Transform3};
7
8use crate::PanelPlacement;
9
10/// Namespace shared with XR tracking-status sample symbols.
11pub const XR_TRACKING_STATUS_NAMESPACE: &str = "stream/xr-tracking";
12
13/// Namespace for world-anchor fallback reason symbols.
14pub const WORLD_ANCHOR_REASON_NAMESPACE: &str = "world-anchor";
15
16/// VIO stability state used by the spatial world-anchor resolver.
17#[derive(Clone, Copy, Debug, PartialEq, Eq)]
18pub enum VioTrackingStatus {
19    /// Six degree-of-freedom tracking is stable enough for world-locked panels.
20    Stable6Dof,
21    /// Tracking is present but not stable enough to keep panels world locked.
22    Limited,
23    /// Tracking is unavailable.
24    Lost,
25}
26
27impl VioTrackingStatus {
28    /// Returns whether this status can keep world anchors locked.
29    pub fn is_stable(self) -> bool {
30        matches!(self, Self::Stable6Dof)
31    }
32
33    /// Encodes the status as the shared XR tracking symbol.
34    pub fn to_symbol(self) -> Symbol {
35        match self {
36            Self::Stable6Dof => Symbol::qualified(XR_TRACKING_STATUS_NAMESPACE, "tracked"),
37            Self::Limited => Symbol::qualified(XR_TRACKING_STATUS_NAMESPACE, "limited"),
38            Self::Lost => Symbol::qualified(XR_TRACKING_STATUS_NAMESPACE, "lost"),
39        }
40    }
41
42    /// Decodes the shared XR tracking symbol into resolver status.
43    pub fn from_symbol(symbol: &Symbol) -> Result<Self> {
44        match (symbol.namespace.as_deref(), symbol.name.as_ref()) {
45            (Some(XR_TRACKING_STATUS_NAMESPACE), "tracked") => Ok(Self::Stable6Dof),
46            (Some(XR_TRACKING_STATUS_NAMESPACE), "limited") => Ok(Self::Limited),
47            (Some(XR_TRACKING_STATUS_NAMESPACE), "lost") => Ok(Self::Lost),
48            _ => Err(Error::HostError(format!(
49                "unknown Viture VIO tracking status {symbol}"
50            ))),
51        }
52    }
53
54    /// Encodes the status as portable expression data.
55    pub fn to_expr(self) -> Expr {
56        Expr::Symbol(self.to_symbol())
57    }
58
59    /// Decodes portable expression data into resolver status.
60    pub fn from_expr(expr: &Expr) -> Result<Self> {
61        let Expr::Symbol(symbol) = expr else {
62            return Err(Error::HostError(
63                "Viture VIO tracking status must be a symbol".to_owned(),
64            ));
65        };
66        Self::from_symbol(symbol)
67    }
68}
69
70/// One observed world anchor or plane that can support a panel placement.
71#[derive(Clone, Debug, PartialEq)]
72pub struct WorldAnchorObservation {
73    /// Stable anchor id observed by VIO.
74    pub anchor: Symbol,
75    /// World-space transform for the observed anchor.
76    pub transform: Transform3,
77}
78
79impl WorldAnchorObservation {
80    /// Builds an observed world anchor.
81    pub fn new(anchor: Symbol, transform: Transform3) -> Self {
82        Self { anchor, transform }
83    }
84}
85
86/// Result of resolving a panel placement against world observations and VIO state.
87#[derive(Clone, Debug, PartialEq)]
88pub enum AnchorResolution {
89    /// Placement remains pinned to a stable world anchor.
90    World {
91        /// Stable anchor id used for the resolution.
92        anchor: Symbol,
93        /// Resolved world-space transform.
94        transform: Transform3,
95    },
96    /// Placement is rendered head locked until world tracking is safe again.
97    HeadLocked {
98        /// Stable anchor id requested by the placement.
99        anchor: Symbol,
100        /// Head-relative transform used while degraded.
101        transform: Transform3,
102        /// Reason for the degradation.
103        reason: Symbol,
104    },
105}
106
107impl AnchorResolution {
108    /// Returns the anchor id involved in this resolution.
109    pub fn anchor(&self) -> &Symbol {
110        match self {
111            Self::World { anchor, .. } | Self::HeadLocked { anchor, .. } => anchor,
112        }
113    }
114
115    /// Returns the transform to render.
116    pub fn transform(&self) -> &Transform3 {
117        match self {
118            Self::World { transform, .. } | Self::HeadLocked { transform, .. } => transform,
119        }
120    }
121
122    /// Returns the resolved coordinate space.
123    pub fn anchor_space(&self) -> AnchorSpace {
124        match self {
125            Self::World { .. } => AnchorSpace::World,
126            Self::HeadLocked { .. } => AnchorSpace::Head,
127        }
128    }
129
130    /// Returns the fallback reason when the panel is head locked.
131    pub fn reason(&self) -> Option<&Symbol> {
132        match self {
133            Self::World { .. } => None,
134            Self::HeadLocked { reason, .. } => Some(reason),
135        }
136    }
137}
138
139/// Resolver for observed world anchors.
140#[derive(Clone, Debug, Default, PartialEq)]
141pub struct WorldAnchorResolver {
142    anchors: BTreeMap<Symbol, Transform3>,
143}
144
145impl WorldAnchorResolver {
146    /// Builds a resolver from observed anchors.
147    pub fn new(observed: impl IntoIterator<Item = WorldAnchorObservation>) -> Self {
148        let anchors = observed
149            .into_iter()
150            .map(|item| (item.anchor, item.transform))
151            .collect();
152        Self { anchors }
153    }
154
155    /// Records or replaces one observed anchor transform.
156    pub fn observe(&mut self, observation: WorldAnchorObservation) {
157        self.anchors
158            .insert(observation.anchor, observation.transform);
159    }
160
161    /// Returns the observed transform for `anchor`.
162    pub fn observed_transform(&self, anchor: &Symbol) -> Option<&Transform3> {
163        self.anchors.get(anchor)
164    }
165
166    /// Resolves one panel placement for the current VIO status.
167    pub fn resolve(
168        &self,
169        placement: &PanelPlacement,
170        status: VioTrackingStatus,
171    ) -> AnchorResolution {
172        let anchor = placement_anchor(placement);
173        if placement.space != AnchorSpace::World {
174            return head_locked(placement, anchor, reason("non-world-anchor"));
175        }
176        if !status.is_stable() {
177            return head_locked(placement, anchor, tracking_reason(status));
178        }
179        let Some(observed) = self.anchors.get(&anchor) else {
180            return head_locked(placement, anchor, reason("missing-world-anchor"));
181        };
182        AnchorResolution::World {
183            anchor,
184            transform: compose_transforms(observed, &placement.transform),
185        }
186    }
187}
188
189/// Resolves one panel placement against observed anchors and VIO status.
190pub fn resolve_world_anchor(
191    placement: &PanelPlacement,
192    status: VioTrackingStatus,
193    resolver: &WorldAnchorResolver,
194) -> AnchorResolution {
195    resolver.resolve(placement, status)
196}
197
198fn placement_anchor(placement: &PanelPlacement) -> Symbol {
199    placement
200        .world_anchor
201        .clone()
202        .unwrap_or_else(|| placement.panel_id.clone())
203}
204
205fn head_locked(placement: &PanelPlacement, anchor: Symbol, reason: Symbol) -> AnchorResolution {
206    AnchorResolution::HeadLocked {
207        anchor,
208        transform: placement.transform.clone(),
209        reason,
210    }
211}
212
213fn tracking_reason(status: VioTrackingStatus) -> Symbol {
214    match status {
215        VioTrackingStatus::Stable6Dof => reason("stable"),
216        VioTrackingStatus::Limited => reason("unstable-vio"),
217        VioTrackingStatus::Lost => reason("lost-vio"),
218    }
219}
220
221fn reason(name: &'static str) -> Symbol {
222    Symbol::qualified(WORLD_ANCHOR_REASON_NAMESPACE, name)
223}
224
225fn compose_transforms(anchor: &Transform3, local: &Transform3) -> Transform3 {
226    let translated = rotate_vector(
227        normalize_quat(anchor.rotate_xyzw),
228        [
229            local.translate_m[0] * anchor.scale[0],
230            local.translate_m[1] * anchor.scale[1],
231            local.translate_m[2] * anchor.scale[2],
232        ],
233    );
234    Transform3::new(
235        [
236            anchor.translate_m[0] + translated[0],
237            anchor.translate_m[1] + translated[1],
238            anchor.translate_m[2] + translated[2],
239        ],
240        normalize_quat(quat_mul(anchor.rotate_xyzw, local.rotate_xyzw)),
241        [
242            anchor.scale[0] * local.scale[0],
243            anchor.scale[1] * local.scale[1],
244            anchor.scale[2] * local.scale[2],
245        ],
246    )
247}
248
249fn quat_mul(left: [f64; 4], right: [f64; 4]) -> [f64; 4] {
250    let [x1, y1, z1, w1] = normalize_quat(left);
251    let [x2, y2, z2, w2] = normalize_quat(right);
252    [
253        w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
254        w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
255        w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2,
256        w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
257    ]
258}
259
260fn normalize_quat(quat: [f64; 4]) -> [f64; 4] {
261    let len =
262        (quat[0] * quat[0] + quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3]).sqrt();
263    if len == 0.0 {
264        [0.0, 0.0, 0.0, 1.0]
265    } else {
266        [quat[0] / len, quat[1] / len, quat[2] / len, quat[3] / len]
267    }
268}
269
270fn rotate_vector(quat: [f64; 4], vector: [f64; 3]) -> [f64; 3] {
271    let qv = [quat[0], quat[1], quat[2]];
272    let uv = cross(qv, vector);
273    let uuv = cross(qv, uv);
274    [
275        vector[0] + 2.0 * (quat[3] * uv[0] + uuv[0]),
276        vector[1] + 2.0 * (quat[3] * uv[1] + uuv[1]),
277        vector[2] + 2.0 * (quat[3] * uv[2] + uuv[2]),
278    ]
279}
280
281fn cross(left: [f64; 3], right: [f64; 3]) -> [f64; 3] {
282    [
283        left[1] * right[2] - left[2] * right[1],
284        left[2] * right[0] - left[0] * right[2],
285        left[0] * right[1] - left[1] * right[0],
286    ]
287}
sim_lib_view_spatial/world.rs

sim_lib_view_spatial/
world.rs
