liealg 0.4.1

lie group and lie algebra in rust
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78

use core::fmt::{Debug, Display};

use nalgebra::Matrix6;

use crate::{Adjoint, Real};

use super::se3;

/// Adjoint of SE3
///
/// AdjSE3 is a 6x6 matrix
/// ```ignore
/// AdjSE3 = [
///   R 0
///  tR R
/// ]
/// ```
#[derive(Debug)]
pub struct AdjSE3<T> {
    pub(crate) val: Matrix6<T>,
}

impl<T> Display for AdjSE3<T>
where
    T: Real + Display,
{
    fn fmt(&self, f: &mut core::fmt::Formatter) -> core::fmt::Result {
        core::fmt::Display::fmt(&self.val, f)
    }
}

impl<T> AdjSE3<T>
where
    T: Real,
{
    /// Create a new AdjSE3 from a slice without checking the contents
    ///
    /// # Safety
    /// use ```SE3::adjoint()``` instead if you are not sure the contents of the slice is valid
    pub fn new_unchecked(val: &[T]) -> Self {
        Self {
            val: Matrix6::from_column_slice(val),
        }
    }

    /// transpose the adjoint
    pub fn transpose(&self) -> Self {
        Self {
            val: self.val.transpose(),
        }
    }
}

impl<T> Adjoint for AdjSE3<T>
where
    T: Real,
{
    type Algebra = se3<T>;

    fn act(&self, other: &Self::Algebra) -> Self::Algebra {
        se3 {
            val: self.val * other.val,
        }
    }
}

#[cfg(test)]
mod test {
    use super::*;

    #[test]
    fn test_new() {
        let adj = AdjSE3 {
            val: Matrix6::<f64>::identity(),
        };
        assert_eq!(adj.val, Matrix6::identity());
    }
}