cartesian_tree/
rotation.rs

1use nalgebra::{Quaternion, UnitQuaternion, Vector3};
2
3/// Unified representation for rotations, allowing different input formats.
4#[derive(Clone, Copy, Debug)]
5pub enum Rotation {
6    /// Quaternion representation (x, y, z, w).
7    Quaternion(UnitQuaternion<f64>),
8    /// Roll-Pitch-Yaw (Euler angles in radians, ZYX convention).
9    Rpy(Vector3<f64>),
10}
11
12impl Rotation {
13    /// Creates a Rotation from a quaternion (x, y, z, w).
14    #[must_use]
15    pub fn from_quaternion(x: f64, y: f64, z: f64, w: f64) -> Self {
16        Self::Quaternion(UnitQuaternion::new_normalize(Quaternion::new(w, x, y, z)))
17    }
18
19    /// Creates a Rotation from RPY angles in radians (roll, pitch, yaw).
20    #[must_use]
21    pub const fn from_rpy(roll: f64, pitch: f64, yaw: f64) -> Self {
22        Self::Rpy(Vector3::new(roll, pitch, yaw))
23    }
24
25    /// Creates the identity rotation using the identity quaternion.
26    #[must_use]
27    pub fn identity() -> Self {
28        Self::Quaternion(UnitQuaternion::identity())
29    }
30
31    /// Converts this rotation to a `UnitQuaternion`.
32    #[must_use]
33    pub fn as_quaternion(&self) -> UnitQuaternion<f64> {
34        match self {
35            Self::Quaternion(q) => *q,
36            Self::Rpy(rpy) => UnitQuaternion::from_euler_angles(rpy.x, rpy.y, rpy.z),
37        }
38    }
39
40    /// Converts to RPY (roll, pitch, yaw) in radians.
41    #[must_use]
42    pub fn as_rpy(&self) -> Vector3<f64> {
43        match self {
44            Self::Quaternion(q) => {
45                let (roll, pitch, yaw) = UnitQuaternion::euler_angles(q);
46                Vector3::new(roll, pitch, yaw)
47            }
48            Self::Rpy(rpy) => *rpy,
49        }
50    }
51}
52
53impl From<UnitQuaternion<f64>> for Rotation {
54    fn from(q: UnitQuaternion<f64>) -> Self {
55        Self::Quaternion(q)
56    }
57}
cartesian_tree/rotation.rs

cartesian_tree/
rotation.rs
