CartesianTree 0.3.0

Build hierarchical Cartesian coordinate systems to easily transform poses.
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

use nalgebra::{Quaternion, UnitQuaternion, Vector3};

/// Unified representation for rotations, allowing different input formats.
#[derive(Clone, Copy, Debug)]
pub enum Rotation {
    /// Quaternion representation (x, y, z, w).
    Quaternion(UnitQuaternion<f64>),
    /// Roll-Pitch-Yaw (Euler angles in radians, ZYX convention).
    Rpy(Vector3<f64>),
}

impl Rotation {
    /// Creates a Rotation from a quaternion (x, y, z, w).
    #[must_use]
    pub fn from_quaternion(x: f64, y: f64, z: f64, w: f64) -> Self {
        Self::Quaternion(UnitQuaternion::new_normalize(Quaternion::new(w, x, y, z)))
    }

    /// Creates a Rotation from RPY angles in radians (roll, pitch, yaw).
    #[must_use]
    pub const fn from_rpy(roll: f64, pitch: f64, yaw: f64) -> Self {
        Self::Rpy(Vector3::new(roll, pitch, yaw))
    }

    /// Creates the identity rotation using the identity quaternion.
    #[must_use]
    pub fn identity() -> Self {
        Self::Quaternion(UnitQuaternion::identity())
    }

    /// Converts this rotation to a `UnitQuaternion`.
    #[must_use]
    pub fn as_quaternion(&self) -> UnitQuaternion<f64> {
        match self {
            Self::Quaternion(q) => *q,
            Self::Rpy(rpy) => UnitQuaternion::from_euler_angles(rpy.x, rpy.y, rpy.z),
        }
    }

    /// Converts to RPY (roll, pitch, yaw) in radians.
    #[must_use]
    pub fn as_rpy(&self) -> Vector3<f64> {
        match self {
            Self::Quaternion(q) => {
                let (roll, pitch, yaw) = UnitQuaternion::euler_angles(q);
                Vector3::new(roll, pitch, yaw)
            }
            Self::Rpy(rpy) => *rpy,
        }
    }
}

impl From<UnitQuaternion<f64>> for Rotation {
    fn from(q: UnitQuaternion<f64>) -> Self {
        Self::Quaternion(q)
    }
}