alice/groups/
se.rs

1use alloc::vec::Vec;
2use core::marker::PhantomData;
3use crate::core::ops::{Group, Magma, Monoid, Semigroup, TopologicalGroup};
4use crate::groups::matrix_group::{GroupError, MatrixGroup};
5use crate::groups::so::So;
6use crate::topology::manifold::{Atlas, Dim, Manifold};
7use crate::core::scalar::FiniteF64;
8use crate::maps::exp_log::HasExpMap;
9
10#[derive(Debug, Clone, PartialEq, Eq)]
11pub struct Se<D: Dim> {
12    inner: MatrixGroup,
13    _dim: PhantomData<D>,
14}
15
16#[derive(Debug, Clone, PartialEq, Eq)]
17pub struct SeAlgebra<D: Dim> {
18    inner: MatrixGroup,
19    _dim: PhantomData<D>,
20}
21
22impl<D: Dim> Se<D> {
23    pub fn from_homogeneous(data: Vec<f64>, dim: D) -> Result<Self, GroupError> {
24        let n = dim.value();
25        let nh = n + 1;
26        if data.len() != nh * nh {
27            return Err(GroupError::DimensionMismatch);
28        }
29        for j in 0..n {
30            if libm::fabs(data[n * nh + j]) > 1e-10 {
31                return Err(GroupError::ConstraintViolated("bottom row must be [0..0,1]"));
32            }
33        }
34        if libm::fabs(data[n * nh + n] - 1.0) > 1e-10 {
35            return Err(GroupError::ConstraintViolated("bottom right element must be 1"));
36        }
37        let rotation_data: Vec<f64> = (0..n)
38            .flat_map(|i| (0..n).map(move |j| (i, j)))
39            .map(|(i, j)| data[i * nh + j])
40            .collect();
41        So::new(rotation_data, dim)?;
42        let inner = MatrixGroup::new(data, nh)?;
43        Ok(Self { inner, _dim: PhantomData })
44    }
45
46    pub fn from_parts(rotation: So<D>, translation: Vec<f64>, dim: D) -> Result<Self, GroupError> {
47        let n = dim.value();
48        if translation.len() != n {
49            return Err(GroupError::DimensionMismatch);
50        }
51        let nh = n + 1;
52        let mut data = alloc::vec![0.0f64; nh * nh];
53        let rot_data = rotation.data();
54        for i in 0..n {
55            for j in 0..n {
56                data[i * nh + j] = rot_data[i * n + j];
57            }
58            data[i * nh + n] = translation[i];
59        }
60        data[n * nh + n] = 1.0;
61        let inner = MatrixGroup::new(data, nh)?;
62        Ok(Self { inner, _dim: PhantomData })
63    }
64
65    pub fn identity(dim: D) -> Self {
66        Self { inner: MatrixGroup::identity(dim.value() + 1), _dim: PhantomData }
67    }
68
69    pub fn rotation(&self, dim: D) -> So<D> {
70        let n = dim.value();
71        let _nh = n + 1;
72        let data: Vec<f64> = (0..n)
73            .flat_map(|i| (0..n).map(move |j| (i, j)))
74            .map(|(i, j)| self.inner.get(i, j))
75            .collect();
76        So::new(data, dim).expect("rotation block must be valid SO element")
77    }
78
79    pub fn translation(&self, dim: D) -> Vec<f64> {
80        let n = dim.value();
81        (0..n).map(|i| self.inner.get(i, n)).collect()
82    }
83
84    pub fn to_homogeneous(&self) -> Vec<f64> { self.inner.data() }
85    pub fn n(&self) -> usize { self.inner.n() - 1 }
86}
87
88impl<D: Dim> SeAlgebra<D> {
89    pub fn from_homogeneous(data: Vec<f64>, dim: D) -> Result<Self, GroupError> {
90        let nh = dim.value() + 1;
91        if data.len() != nh * nh {
92            return Err(GroupError::DimensionMismatch);
93        }
94        let inner = MatrixGroup::new(data, nh)?;
95        Ok(Self { inner, _dim: PhantomData })
96    }
97
98    pub fn zero(dim: D) -> Self {
99        let nh = dim.value() + 1;
100        Self {
101            inner: MatrixGroup::new(alloc::vec![0.0; nh * nh], nh).unwrap(),
102            _dim: PhantomData,
103        }
104    }
105
106    pub fn data(&self) -> Vec<f64> { self.inner.data() }
107}
108
109impl<D: Dim> Magma for Se<D> {
110    fn op(&self, other: &Self) -> Self {
111        Self {
112            inner: self.inner.mul(&other.inner).expect("SE multiplication must succeed"),
113            _dim: PhantomData,
114        }
115    }
116}
117
118impl<D: Dim> Semigroup for Se<D> {}
119
120impl<D: Dim> Monoid for Se<D> {
121    fn identity() -> Self {
122        panic!("SE identity requires dimension; use Se::identity(dim)")
123    }
124}
125
126impl<D: Dim> Group for Se<D> {
127    fn inverse(&self) -> Self {
128        Self {
129            inner: self.inner.inverse().expect("SE element must be invertible"),
130            _dim: PhantomData,
131        }
132    }
133}
134
135impl<D: Dim> TopologicalGroup for Se<D> {}
136
137impl<D: Dim> Manifold for Se<D> {
138    type Scalar = FiniteF64;
139    fn dim(&self) -> usize {
140        let n = self.inner.n() - 1;
141        n * (n - 1) / 2 + n
142    }
143    fn atlas(&self) -> &Atlas<FiniteF64> { unimplemented!("SE atlas not yet constructed") }
144}
145
146impl<D: Dim> HasExpMap for Se<D> {
147    type Algebra = SeAlgebra<D>;
148    fn exp(x: &SeAlgebra<D>) -> Self {
149        Self { inner: x.inner.exp(), _dim: PhantomData }
150    }
151    fn log(&self) -> Option<SeAlgebra<D>> {
152        Some(SeAlgebra { inner: self.inner.log()?, _dim: PhantomData })
153    }
154}
alice/groups/se.rs

alice/groups/
se.rs
