Skip to main content

qualia_core_db/domains/geospatial/
reference_frame.rs

1use serde::{Deserialize, Serialize};
2
3/// Represents a local coordinate system with a floating origin and scale factor.
4/// Used to bridge the micro-verse (e.g. molecular structures) with the macro-verse (e.g. planetary scale).
5#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)]
6pub struct ReferenceFrame {
7    /// Unique identifier (e.g. q_hash of the frame URI).
8    pub id: u64,
9    /// Parent frame ID. If None, this is a top-level global frame.
10    pub parent_id: Option<u64>,
11    /// Translation relative to the parent frame [x, y, z].
12    pub translation: [f64; 3],
13    /// Rotation quaternion [w, x, y, z] relative to the parent frame.
14    pub rotation: [f64; 4],
15    /// Scale multiplier applied to everything within this frame.
16    pub scale: f64,
17}
18
19impl Default for ReferenceFrame {
20    fn default() -> Self {
21        Self {
22            id: 0,
23            parent_id: None,
24            translation: [0.0, 0.0, 0.0],
25            rotation: [1.0, 0.0, 0.0, 0.0], // Identity quaternion
26            scale: 1.0,
27        }
28    }
29}
30
31impl ReferenceFrame {
32    /// Creates a default `ReferenceFrame`.
33    pub fn new(id: u64, parent_id: Option<u64>) -> Self {
34        Self {
35            id,
36            parent_id,
37            ..Default::default()
38        }
39    }
40
41    /// Applies this frame's transform to a local point, mapping it into the parent's coordinate space.
42    pub fn transform_to_parent(&self, local_point: [f64; 3]) -> [f64; 3] {
43        let [x, y, z] = local_point;
44
45        // 1. Scale
46        let sx = x * self.scale;
47        let sy = y * self.scale;
48        let sz = z * self.scale;
49
50        // 2. Rotate (using quaternion: q * p * q^-1)
51        let [qw, qx, qy, qz] = self.rotation;
52
53        let ix = qw * sx + qy * sz - qz * sy;
54        let iy = qw * sy + qz * sx - qx * sz;
55        let iz = qw * sz + qx * sy - qy * sx;
56        let iw = -qx * sx - qy * sy - qz * sz;
57
58        let rx = ix * qw + iw * -qx + iy * -qz - iz * -qy;
59        let ry = iy * qw + iw * -qy + iz * -qx - ix * -qz;
60        let rz = iz * qw + iw * -qz + ix * -qy - iy * -qx;
61
62        // 3. Translate
63        [
64            rx + self.translation[0],
65            ry + self.translation[1],
66            rz + self.translation[2],
67        ]
68    }
69}
70
71#[cfg(test)]
72mod tests {
73    use super::*;
74
75    #[test]
76    fn test_transform_to_parent() {
77        let mut frame = ReferenceFrame::new(1, None);
78        frame.translation = [10.0, 20.0, 30.0];
79        frame.scale = 0.5;
80        // 90 degrees around Z-axis
81        let half_angle = std::f64::consts::PI / 4.0;
82        frame.rotation = [half_angle.cos(), 0.0, 0.0, half_angle.sin()];
83
84        let local_pt = [2.0, 0.0, 0.0];
85        let transformed = frame.transform_to_parent(local_pt);
86
87        // Scale -> [1.0, 0.0, 0.0]
88        // Rotate 90 deg around Z -> [0.0, 1.0, 0.0]
89        // Translate -> [10.0, 21.0, 30.0]
90
91        assert!((transformed[0] - 10.0).abs() < 1e-8);
92        assert!((transformed[1] - 21.0).abs() < 1e-8);
93        assert!((transformed[2] - 30.0).abs() < 1e-8);
94    }
95}