dynamics_spatial/
se3.rs

1//! Defines the **special Euclidean group** SE(3) and related operations.
2//!
3//! This module defines the SE(3) transformation, which combines rotation and translation in 3D space.
4//! An SE(3) transformation consists of a rotation matrix and a translation vector.
5//! Operations such as composition, inversion, and action on points are provided.
6
7use std::fmt::Display;
8
9use crate::{motion::SpatialRotation, vector3d::Vector3D};
10use nalgebra::{IsometryMatrix3, Matrix3, Matrix6, Translation3};
11
12/// SE(3) transformation represented as an isometry matrix.
13///
14/// An SE(3) transformation combines a rotation and a translation in 3D space.
15/// It combines a rotation matrix $R\in \text{SO}(3)$ and a translation vector $t \in \mathbb{R}^3$
16#[derive(Clone, Debug, Copy, PartialEq, Default)]
17pub struct SE3(pub(crate) IsometryMatrix3<f64>);
18
19impl SE3 {
20    /// Creates a new SE(3) transformation from a rotation (given as axis-angle) and a translation.
21    #[must_use]
22    pub fn new(translation: Vector3D, axis_angle: Vector3D) -> Self {
23        let rotation = SpatialRotation::from_axis_angle(&axis_angle, axis_angle.norm());
24        SE3::from_parts(
25            Vector3D::new(translation.0.x, translation.0.y, translation.0.z),
26            rotation,
27        )
28    }
29
30    /// Creates a new SE(3) transformation from a rotation and a translation.
31    #[must_use]
32    pub fn from_parts(translation: Vector3D, rotation: SpatialRotation) -> Self {
33        SE3(IsometryMatrix3::from_parts(
34            Translation3::from(translation.0),
35            rotation.0,
36        ))
37    }
38
39    /// Creates a new identity SE(3) transformation, with $R = I_3$ and $t = 0_3$.
40    #[must_use]
41    pub fn identity() -> Self {
42        SE3(IsometryMatrix3::identity())
43    }
44
45    /// Returns the inverse of the SE(3) transformation.
46    #[must_use]
47    pub fn inverse(&self) -> Self {
48        SE3(self.0.inverse())
49    }
50
51    /// Returns the translation component of the SE(3) transformation.
52    #[must_use]
53    pub fn translation(&self) -> Vector3D {
54        Vector3D(self.0.translation.vector)
55    }
56
57    /// Returns the rotation component of the SE(3) transformation.
58    #[must_use]
59    pub fn rotation(&self) -> SpatialRotation {
60        SpatialRotation(self.0.rotation)
61    }
62
63    /// Computes the action matrix of the SE(3) transformation.
64    ///
65    /// Mathematically, the action matrix is:
66    /// $$\begin{bmatrix}R & [t]_X R \\\\ 0 & R\end{bmatrix}$$
67    /// where $R$ is the rotation matrix and $[t]_X$ is the skew-symmetric matrix of the translation vector $t$.
68    pub fn action_matrix(&self) -> Matrix6<f64> {
69        // FIXME: output a custom type
70
71        let r = self.rotation();
72        let r = r.0.matrix();
73        let t = self.translation().0;
74        let mut action_matrix = Matrix6::zeros();
75        action_matrix
76            .fixed_view_mut::<3, 3>(0, 0)
77            .copy_from(&r.transpose());
78        action_matrix
79            .fixed_view_mut::<3, 3>(3, 3)
80            .copy_from(&r.transpose());
81
82        // Create skew-symmetric matrix [t]_x from translation vector
83        let skew_t = Matrix3::new(0.0, -t[2], t[1], t[2], 0.0, -t[0], -t[1], t[0], 0.0);
84
85        action_matrix
86            .fixed_view_mut::<3, 3>(3, 0)
87            .copy_from(&(skew_t * r).transpose());
88        action_matrix
89    }
90
91    /// Computes the dual action matrix of the SE(3) transformation.
92    ///
93    /// Mathematically, the action matrix is:
94    /// $$\begin{bmatrix}R & 0 \\\\ [t]_X R & R\end{bmatrix}$$
95    /// where $R$ is the rotation matrix and $[t]_X$ is the skew-symmetric matrix of the translation vector $t$.
96    pub fn dual_matrix(&self) -> Matrix6<f64> {
97        // FIXME: output a custom type
98
99        let r = self.rotation();
100        let r = r.0.matrix();
101        let t = self.translation().0;
102        let mut action_matrix = Matrix6::zeros();
103        action_matrix
104            .fixed_view_mut::<3, 3>(0, 0)
105            .copy_from(&r.transpose());
106        action_matrix
107            .fixed_view_mut::<3, 3>(3, 3)
108            .copy_from(&r.transpose());
109
110        // Create skew-symmetric matrix [t]_x from translation vector
111        let skew_t = Matrix3::new(0.0, -t[2], t[1], t[2], 0.0, -t[0], -t[1], t[0], 0.0);
112
113        action_matrix
114            .fixed_view_mut::<3, 3>(0, 3)
115            .copy_from(&(skew_t * r).transpose());
116        action_matrix
117    }
118
119    /// Computes the inverse of the action matrix of the SE(3) transformation.
120    pub fn inv_matrix(&self) -> Matrix6<f64> {
121        // FIXME: output a custom type
122
123        let r = self.rotation().0;
124        let r_inv = r.matrix();
125        let t = self.translation().0;
126
127        let mut inv_matrix = nalgebra::Matrix6::zeros();
128        inv_matrix.fixed_view_mut::<3, 3>(0, 0).copy_from(r_inv);
129        inv_matrix.fixed_view_mut::<3, 3>(3, 3).copy_from(r_inv);
130
131        // Create skew-symmetric matrix [t]_x from translation vector
132        let skew_t = Matrix3::new(0.0, -t[2], t[1], t[2], 0.0, -t[0], -t[1], t[0], 0.0);
133
134        inv_matrix
135            .fixed_view_mut::<3, 3>(3, 0)
136            .copy_from(&(-skew_t.transpose() * r_inv));
137        inv_matrix
138    }
139}
140
141impl std::ops::Mul for SE3 {
142    type Output = SE3;
143
144    fn mul(self, rhs: Self) -> Self::Output {
145        SE3(self.0 * rhs.0)
146    }
147}
148
149impl std::ops::Mul<&SE3> for &SE3 {
150    type Output = SE3;
151
152    fn mul(self, rhs: &SE3) -> Self::Output {
153        SE3(self.0 * rhs.0)
154    }
155}
156
157impl std::ops::Mul<SE3> for &SE3 {
158    type Output = SE3;
159
160    fn mul(self, rhs: SE3) -> Self::Output {
161        SE3(self.0 * rhs.0)
162    }
163}
164
165impl std::ops::Mul<&SE3> for SE3 {
166    type Output = SE3;
167
168    fn mul(self, rhs: &SE3) -> Self::Output {
169        SE3(self.0 * rhs.0)
170    }
171}
172
173impl Display for SE3 {
174    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
175        let r = self.0.rotation.matrix();
176
177        writeln!(f, "SE3: R=┌                            ┐  t=┌          ┐")?;
178        for i in 0..3 {
179            writeln!(
180                f,
181                "       │ {:>+8.5} {:>+8.5} {:>+8.5} │    │ {:>+8.5} │",
182                r[(i, 0)],
183                r[(i, 1)],
184                r[(i, 2)],
185                self.0.translation.vector[i]
186            )?;
187        }
188        writeln!(f, "       └                            ┘    └          ┘")?;
189        Ok(())
190    }
191}
192
193/// Trait for objects on which SE(3) transformations can act.
194pub trait ActSE3 {
195    /// Applies the SE(3) transformation to the object.
196    fn act(&self, se3: &SE3) -> Self;
197
198    /// Applies the inverse SE(3) transformation to the object.
199    fn act_inv(&self, se3: &SE3) -> Self;
200}
201
202impl SE3 {
203    /// Applies the SE(3) transformation to an object implementing the [`ActSE3`] trait.
204    pub fn act<T: ActSE3>(&self, obj: &T) -> T {
205        obj.act(self)
206    }
207
208    /// Applies the inverse SE(3) transformation to an object implementing the [`ActSE3`] trait.
209    pub fn act_inv<T: ActSE3>(&self, obj: &T) -> T {
210        obj.act_inv(self)
211    }
212}
dynamics_spatial/se3.rs

dynamics_spatial/
se3.rs
