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, 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
64impl std::ops::Mul for SE3 {
65    type Output = SE3;
66
67    fn mul(self, rhs: Self) -> Self::Output {
68        SE3(self.0 * rhs.0)
69    }
70}
71
72impl std::ops::Mul<&SE3> for &SE3 {
73    type Output = SE3;
74
75    fn mul(self, rhs: &SE3) -> Self::Output {
76        SE3(self.0 * rhs.0)
77    }
78}
79
80impl std::ops::Mul<SE3> for &SE3 {
81    type Output = SE3;
82
83    fn mul(self, rhs: SE3) -> Self::Output {
84        SE3(self.0 * rhs.0)
85    }
86}
87
88impl std::ops::Mul<&SE3> for SE3 {
89    type Output = SE3;
90
91    fn mul(self, rhs: &SE3) -> Self::Output {
92        SE3(self.0 * rhs.0)
93    }
94}
95
96impl Display for SE3 {
97    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
98        let r = self.0.rotation.matrix();
99
100        writeln!(f, "SE3: R=┌                            ┐  t=┌          ┐")?;
101        for i in 0..3 {
102            writeln!(
103                f,
104                "       │ {:>+8.5} {:>+8.5} {:>+8.5} │    │ {:>+8.5} │",
105                r[(i, 0)],
106                r[(i, 1)],
107                r[(i, 2)],
108                self.0.translation.vector[i]
109            )?;
110        }
111        writeln!(f, "       └                            ┘    └          ┘")?;
112        Ok(())
113    }
114}
115
116/// Trait for objects on which SE(3) transformations can act.
117pub trait ActSE3 {
118    /// Applies the SE(3) transformation to the object.
119    fn act(&self, se3: &SE3) -> Self;
120
121    /// Applies the inverse SE(3) transformation to the object.
122    fn act_inv(&self, se3: &SE3) -> Self;
123}
124
125impl SE3 {
126    /// Applies the SE(3) transformation to an object implementing the [`ActSE3`] trait.
127    pub fn act<T: ActSE3>(&self, obj: &T) -> T {
128        obj.act(self)
129    }
130
131    /// Applies the inverse SE(3) transformation to an object implementing the [`ActSE3`] trait.
132    pub fn act_inv<T: ActSE3>(&self, obj: &T) -> T {
133        obj.act_inv(self)
134    }
135}
dynamics_spatial/se3.rs

dynamics_spatial/
se3.rs
