loam_math/
euclidean.rs

1use std::borrow::Cow;
2
3use glam::{Quat, Vec3};
4use serde::{Deserialize, Serialize};
5
6use crate::space::{IsometryGroup, Space, WgslAccuracy, WgslSpace, FLAT_CHART_MAX_ARC};
7
8#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
9pub struct Iso3 {
10    pub rotation: Quat,
11    pub translation: Vec3,
12}
13
14impl Iso3 {
15    pub const IDENTITY: Self = Self {
16        rotation: Quat::IDENTITY,
17        translation: Vec3::ZERO,
18    };
19
20    pub fn from_rotation(rotation: Quat) -> Self {
21        Self {
22            rotation,
23            translation: Vec3::ZERO,
24        }
25    }
26
27    pub fn from_translation(translation: Vec3) -> Self {
28        Self {
29            rotation: Quat::IDENTITY,
30            translation,
31        }
32    }
33}
34
35#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
36pub struct EuclideanR3;
37
38impl Space for EuclideanR3 {
39    type Point = Vec3;
40    type Vector = Vec3;
41    type Frame = Quat;
42
43    fn frame_at(&self, _at: Vec3) -> Quat {
44        Quat::IDENTITY
45    }
46
47    fn distance(&self, a: Vec3, b: Vec3) -> f32 {
48        (a - b).length()
49    }
50
51    fn exp(&self, at: Vec3, v: Vec3) -> Vec3 {
52        at + v
53    }
54
55    fn log(&self, from: Vec3, to: Vec3) -> Vec3 {
56        to - from
57    }
58
59    fn parallel_transport(&self, _from: Vec3, _to: Vec3, v: Vec3) -> Vec3 {
60        v
61    }
62
63    fn is_chart_flat(&self) -> bool {
64        true
65    }
66
67    fn chart_envelope(&self) -> f32 {
68        f32::INFINITY
69    }
70
71    fn valid_point(&self, p: Vec3) -> bool {
72        p.is_finite()
73    }
74}
75
76impl IsometryGroup for EuclideanR3 {
77    type Iso = Iso3;
78
79    fn iso_identity(&self) -> Iso3 {
80        Iso3::IDENTITY
81    }
82
83    fn iso_compose(&self, a: Iso3, b: Iso3) -> Iso3 {
84        Iso3 {
85            rotation: a.rotation * b.rotation,
86            translation: a.rotation * b.translation + a.translation,
87        }
88    }
89
90    fn iso_inverse(&self, a: Iso3) -> Iso3 {
91        let inv_rot = a.rotation.inverse();
92        Iso3 {
93            rotation: inv_rot,
94            translation: inv_rot * (-a.translation),
95        }
96    }
97
98    fn iso_apply(&self, iso: Iso3, p: Vec3) -> Vec3 {
99        iso.rotation * p + iso.translation
100    }
101
102    fn iso_transport(&self, iso: Iso3, _at: Vec3, v: Vec3) -> Vec3 {
103        iso.rotation * v
104    }
105}
106
107impl WgslSpace for EuclideanR3 {
108    fn wgsl_impl(&self) -> Cow<'static, str> {
109        Cow::Owned(format!(
110            "const LOAM_MAX_ARC: f32 = {FLAT_CHART_MAX_ARC:?};{WGSL_IMPL}"
111        ))
112    }
113
114    fn wgsl_accuracy(&self) -> WgslAccuracy {
115        WgslAccuracy::Exact
116    }
117}
118
119// `from` is a WGSL reserved keyword (WGSL spec §3.2); use `p_from`/`p_to`.
120const WGSL_IMPL: &str = r#"
121// loam-math :: EuclideanR3 (v0 Space WGSL ABI)
122fn loam_distance(a: vec3<f32>, b: vec3<f32>) -> f32 { return length(a - b); }
123fn loam_origin_distance(p: vec3<f32>) -> f32 { return length(p); }
124fn loam_exp(at: vec3<f32>, v: vec3<f32>) -> vec3<f32> { return at + v; }
125fn loam_log(p_from: vec3<f32>, p_to: vec3<f32>) -> vec3<f32> { return p_to - p_from; }
126struct LoamGeodesicStep { p: vec3<f32>, v: vec3<f32> }
127fn loam_geodesic_step(p: vec3<f32>, v: vec3<f32>, s: f32) -> LoamGeodesicStep {
128    return LoamGeodesicStep(loam_exp(p, v * s), v);
129}
130"#;
131
132#[cfg(test)]
133mod tests {
134    use super::*;
135    use approx::assert_relative_eq;
136
137    fn r3() -> EuclideanR3 {
138        EuclideanR3
139    }
140
141    #[test]
142    fn parallel_transport_along_default_impl_is_identity_in_flat_space() {
143        let s = r3();
144        let v = Vec3::new(1.7, -0.3, 0.5);
145        assert_eq!(s.parallel_transport_along(&[], v), v);
146        assert_eq!(s.parallel_transport_along(&[Vec3::ZERO], v), v);
147        let path = [
148            Vec3::ZERO,
149            Vec3::new(1.0, 0.0, 0.0),
150            Vec3::new(1.0, 1.0, 0.0),
151            Vec3::new(2.0, 3.0, -1.0),
152        ];
153        let transported = s.parallel_transport_along(&path, v);
154        assert_relative_eq!(transported.x, v.x);
155        assert_relative_eq!(transported.y, v.y);
156        assert_relative_eq!(transported.z, v.z);
157    }
158
159    #[test]
160    fn iso_transport_ignores_translation() {
161        let s = r3();
162        let iso = Iso3::from_translation(Vec3::new(100.0, -50.0, 7.0));
163        let v = Vec3::new(1.0, 0.0, 0.0);
164        let transported = s.iso_transport(iso, Vec3::ZERO, v);
165        assert_relative_eq!(transported.x, v.x);
166        assert_relative_eq!(transported.y, v.y);
167        assert_relative_eq!(transported.z, v.z);
168    }
169}
loam_math/euclidean.rs

loam_math/
euclidean.rs
