cartesian_tree/
rotation.rs

1use crate::CartesianTreeError;
2use nalgebra::{Quaternion, UnitQuaternion, Vector3};
3
4/// Minimum norm below which a quaternion is considered degenerate.
5pub(crate) const MIN_QUATERNION_NORM: f64 = 1.0e-9;
6
7/// Unified representation for rotations, allowing different input formats.
8#[derive(Clone, Copy, Debug)]
9pub enum Rotation {
10    /// Quaternion representation (x, y, z, w).
11    Quaternion(UnitQuaternion<f64>),
12    /// Roll-Pitch-Yaw (Euler angles in radians, ZYX convention).
13    Rpy(Vector3<f64>),
14}
15
16impl Rotation {
17    /// Creates a Rotation from a quaternion (x, y, z, w).
18    ///
19    /// The quaternion does not need to be normalized; it is normalized internally.
20    ///
21    /// # Errors
22    /// Returns a [`CartesianTreeError`] if:
23    /// - The quaternion's norm is too close to zero to normalize.
24    pub fn from_quaternion(x: f64, y: f64, z: f64, w: f64) -> Result<Self, CartesianTreeError> {
25        UnitQuaternion::try_new(Quaternion::new(w, x, y, z), MIN_QUATERNION_NORM)
26            .map(Self::Quaternion)
27            .ok_or(CartesianTreeError::InvalidQuaternion(x, y, z, w))
28    }
29
30    /// Creates a Rotation from RPY angles in radians (roll, pitch, yaw).
31    #[must_use]
32    pub const fn from_rpy(roll: f64, pitch: f64, yaw: f64) -> Self {
33        Self::Rpy(Vector3::new(roll, pitch, yaw))
34    }
35
36    /// Creates the identity rotation using the identity quaternion.
37    #[must_use]
38    pub fn identity() -> Self {
39        Self::Quaternion(UnitQuaternion::identity())
40    }
41
42    /// Converts this rotation to a `UnitQuaternion`.
43    #[must_use]
44    pub fn as_quaternion(&self) -> UnitQuaternion<f64> {
45        match self {
46            Self::Quaternion(q) => *q,
47            Self::Rpy(rpy) => UnitQuaternion::from_euler_angles(rpy.x, rpy.y, rpy.z),
48        }
49    }
50
51    /// Converts to RPY (roll, pitch, yaw) in radians.
52    #[must_use]
53    pub fn as_rpy(&self) -> Vector3<f64> {
54        match self {
55            Self::Quaternion(q) => {
56                let (roll, pitch, yaw) = UnitQuaternion::euler_angles(q);
57                Vector3::new(roll, pitch, yaw)
58            }
59            Self::Rpy(rpy) => *rpy,
60        }
61    }
62}
63
64impl From<UnitQuaternion<f64>> for Rotation {
65    fn from(q: UnitQuaternion<f64>) -> Self {
66        Self::Quaternion(q)
67    }
68}
69
70#[cfg(test)]
71mod tests {
72    use super::*;
73    use approx::assert_relative_eq;
74
75    #[test]
76    fn from_quaternion_normalizes_input() {
77        let rotation = Rotation::from_quaternion(0.0, 0.0, 2.0, 0.0).unwrap();
78        let q = rotation.as_quaternion();
79        assert_relative_eq!(q.k, 1.0, epsilon = 1e-12);
80        assert_relative_eq!(q.w, 0.0, epsilon = 1e-12);
81    }
82
83    #[test]
84    fn from_quaternion_rejects_zero_norm() {
85        assert!(matches!(
86            Rotation::from_quaternion(0.0, 0.0, 0.0, 0.0),
87            Err(CartesianTreeError::InvalidQuaternion(..))
88        ));
89    }
90}
cartesian_tree/rotation.rs

cartesian_tree/
rotation.rs
