loam_math/
euclidean_r2.rs

1use glam::Vec2;
2use serde::{Deserialize, Serialize};
3
4use crate::bivector::{Rotor, Rotor2};
5use crate::space::{IsometryGroup, Space};
6
7#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
8pub struct Iso2 {
9    pub rotation: Rotor2,
10    pub translation: Vec2,
11}
12
13impl Iso2 {
14    pub const IDENTITY: Self = Self {
15        rotation: Rotor2::IDENTITY,
16        translation: Vec2::ZERO,
17    };
18
19    pub fn from_rotation(rotation: Rotor2) -> Self {
20        Self {
21            rotation,
22            translation: Vec2::ZERO,
23        }
24    }
25
26    pub fn from_translation(translation: Vec2) -> Self {
27        Self {
28            rotation: Rotor2::IDENTITY,
29            translation,
30        }
31    }
32}
33
34impl Default for Iso2 {
35    fn default() -> Self {
36        Self::IDENTITY
37    }
38}
39
40impl Serialize for Rotor2 {
41    fn serialize<S: serde::Serializer>(&self, s: S) -> Result<S::Ok, S::Error> {
42        (self.a, self.b).serialize(s)
43    }
44}
45
46impl<'de> Deserialize<'de> for Rotor2 {
47    fn deserialize<D: serde::Deserializer<'de>>(d: D) -> Result<Self, D::Error> {
48        let (a, b) = <(f32, f32)>::deserialize(d)?;
49        Ok(Self { a, b })
50    }
51}
52
53#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
54pub struct EuclideanR2;
55
56impl Space for EuclideanR2 {
57    type Point = Vec2;
58    type Vector = Vec2;
59    type Frame = Rotor2;
60
61    fn frame_at(&self, _at: Vec2) -> Rotor2 {
62        Rotor2::IDENTITY
63    }
64
65    fn distance(&self, a: Vec2, b: Vec2) -> f32 {
66        (a - b).length()
67    }
68
69    fn exp(&self, at: Vec2, v: Vec2) -> Vec2 {
70        at + v
71    }
72
73    fn log(&self, from: Vec2, to: Vec2) -> Vec2 {
74        to - from
75    }
76
77    fn parallel_transport(&self, _from: Vec2, _to: Vec2, v: Vec2) -> Vec2 {
78        v
79    }
80
81    fn is_chart_flat(&self) -> bool {
82        true
83    }
84
85    fn chart_envelope(&self) -> f32 {
86        f32::INFINITY
87    }
88
89    fn valid_point(&self, p: Vec2) -> bool {
90        p.is_finite()
91    }
92}
93
94impl IsometryGroup for EuclideanR2 {
95    type Iso = Iso2;
96
97    fn iso_identity(&self) -> Iso2 {
98        Iso2::IDENTITY
99    }
100
101    fn iso_compose(&self, a: Iso2, b: Iso2) -> Iso2 {
102        Iso2 {
103            rotation: a.rotation * b.rotation,
104            translation: a.rotation.apply(b.translation) + a.translation,
105        }
106    }
107
108    fn iso_inverse(&self, a: Iso2) -> Iso2 {
109        let inv = a.rotation.inverse();
110        Iso2 {
111            rotation: inv,
112            translation: inv.apply(-a.translation),
113        }
114    }
115
116    fn iso_apply(&self, iso: Iso2, p: Vec2) -> Vec2 {
117        iso.rotation.apply(p) + iso.translation
118    }
119
120    fn iso_transport(&self, iso: Iso2, _at: Vec2, v: Vec2) -> Vec2 {
121        iso.rotation.apply(v)
122    }
123}
124
125#[cfg(test)]
126mod tests {
127    use super::*;
128    use crate::bivector::{Bivector, Bivector2};
129    use std::f32::consts::FRAC_PI_2;
130
131    fn assert_vec2_close(a: Vec2, b: Vec2) {
132        assert!((a - b).length() <= 1e-5, "expected {a:?} close to {b:?}");
133    }
134
135    #[test]
136    fn iso_translation_moves_origin_to_target() {
137        let s = EuclideanR2;
138        let t = Vec2::new(2.0, -1.0);
139        let iso = Iso2::from_translation(t);
140        assert_vec2_close(s.iso_apply(iso, Vec2::ZERO), t);
141    }
142
143    #[test]
144    fn iso_rotation_quarter_turn_sends_x_to_y() {
145        let s = EuclideanR2;
146        let r = Iso2::from_rotation(Bivector2(FRAC_PI_2).exp());
147        assert_vec2_close(s.iso_apply(r, Vec2::X), Vec2::Y);
148    }
149
150    #[test]
151    fn iso_transport_is_rotation_only() {
152        let s = EuclideanR2;
153        let iso = Iso2 {
154            rotation: Bivector2(FRAC_PI_2).exp(),
155            translation: Vec2::new(100.0, -50.0),
156        };
157        assert_vec2_close(s.iso_transport(iso, Vec2::ZERO, Vec2::X), Vec2::Y);
158    }
159}
loam_math/euclidean_r2.rs

loam_math/
euclidean_r2.rs
