orbita3d_kinematics/
conversion.rs

1use nalgebra::{Quaternion, Rotation3, UnitQuaternion, Vector3};
2
3/// Convert a quaternion to a rotation matrix.
4pub fn quaternion_to_rotation_matrix(qx: f64, qy: f64, qz: f64, qw: f64) -> Rotation3<f64> {
5    Rotation3::from(UnitQuaternion::from_quaternion(Quaternion::new(
6        qw, qx, qy, qz,
7    )))
8}
9
10/// Convert a rotation matrix to a quaternion.
11pub fn rotation_matrix_to_quaternion(rot: Rotation3<f64>) -> [f64; 4] {
12    let q = UnitQuaternion::from_rotation_matrix(&rot);
13    [q.i, q.j, q.k, q.w]
14}
15
16/// Convert intrinsic roll-pitch-yaw angles to a rotation matrix.
17pub fn intrinsic_roll_pitch_yaw_to_matrix(roll: f64, pitch: f64, yaw: f64) -> Rotation3<f64> {
18    let mx = Rotation3::from_axis_angle(&Vector3::x_axis(), roll);
19    let my = Rotation3::from_axis_angle(&Vector3::y_axis(), pitch);
20    let mz = Rotation3::from_axis_angle(&Vector3::z_axis(), yaw);
21
22    mx * my * mz
23}
24
25/// Convert a rotation martix to intrinsic roll-pitch-yaw angles.
26pub fn matrix_to_intrinsic_roll_pitch_yaw(rot: Rotation3<f64>) -> [f64; 3] {
27    let roll = -rot[(1, 2)].atan2(rot[(2, 2)]);
28    let pitch = rot[(0, 2)].atan2((1.0 - rot[(0, 2)].powf(2.0)).sqrt());
29    let yaw = -rot[(0, 1)].atan2(rot[(0, 0)]);
30
31    [roll, pitch, yaw]
32}
33
34// pub fn quaternion_to_roll_pitch_yaw(quat: [f64; 4]) -> [f64; 3] {
35//     let q = UnitQuaternion::from_quaternion(Quaternion::new(quat[3], quat[0], quat[1], quat[2]));
36//     let rpy = q.euler_angles();
37//     [rpy.0, rpy.1, rpy.2]
38// }
39
40pub fn quaternion_to_roll_pitch_yaw(quat: [f64; 4]) -> [f64; 3] {
41    let q = UnitQuaternion::from_quaternion(Quaternion::new(quat[3], quat[0], quat[1], quat[2]));
42    let mat = q.to_rotation_matrix();
43    matrix_to_intrinsic_roll_pitch_yaw(mat)
44}
45
46pub fn quaternion_to_intrinsic_roll_pitch_yaw(quat: [f64; 4]) -> [f64; 3] {
47    let q = UnitQuaternion::from_quaternion(Quaternion::new(quat[3], quat[0], quat[1], quat[2]));
48    let mat = q.to_rotation_matrix();
49    matrix_to_intrinsic_roll_pitch_yaw(mat)
50}
51
52/// Convert from `Vector3<f64>` to `\[`f64;3`\]`
53pub fn vector3_to_array(v: Vector3<f64>) -> [f64; 3] {
54    [v.x, v.y, v.z]
55}
56/// Convert from `\[`f64;3`\]` to `Vector3<f64>`
57pub fn array_to_vector3(a: [f64; 3]) -> Vector3<f64> {
58    Vector3::new(a[0], a[1], a[2])
59}
60
61#[cfg(test)]
62mod tests {
63    use rand::Rng;
64    use std::f64::consts::PI;
65
66    use super::*;
67
68    const EPSILON: f64 = 1e-6;
69
70    #[test]
71    fn rpy() {
72        let mut rng = rand::thread_rng();
73
74        let roll = rng.gen_range(-2.0 * PI..2.0 * PI).to_radians();
75        let pitch = rng.gen_range(-2.0 * PI..2.0 * PI).to_radians();
76        let yaw = rng.gen_range(-2.0 * PI..2.0 * PI).to_radians();
77
78        // [0.5058586154908705, 0.05003352447822581, 4.771802600632974]
79        // let roll=0.5058586154908705;
80        // let pitch=0.05003352447822581;
81
82        // let roll = 0.0;
83        // let pitch = 0.0;
84        // let yaw = 4.771802600632974;
85
86        let rot = intrinsic_roll_pitch_yaw_to_matrix(roll, pitch, yaw);
87        let rpy = matrix_to_intrinsic_roll_pitch_yaw(rot);
88
89        assert!((roll - rpy[0]).abs() < EPSILON);
90        assert!((pitch - rpy[1]).abs() < EPSILON);
91        assert!((yaw - rpy[2]).abs() < EPSILON);
92    }
93
94    #[test]
95    fn quat() {
96        let mut rng = rand::thread_rng();
97
98        // Use euler angles to generate a roation matrix
99        let roll = rng.gen_range(-2.0 * PI..2.0 * PI).to_radians();
100        let pitch = rng.gen_range(-2.0 * PI..2.0 * PI).to_radians();
101        let yaw = rng.gen_range(-2.0 * PI..2.0 * PI).to_radians();
102        let rot = intrinsic_roll_pitch_yaw_to_matrix(roll, pitch, yaw);
103
104        // Convert the rotation matrix to a quaternion
105        let quat = rotation_matrix_to_quaternion(rot);
106        let reconstructed = quaternion_to_rotation_matrix(quat[0], quat[1], quat[2], quat[3]);
107
108        // Check that the reconstructed rotation matrix is the same as the original
109        for i in 0..3 {
110            for j in 0..3 {
111                assert!((rot[(i, j)] - reconstructed[(i, j)]).abs() < 0.0001);
112            }
113        }
114    }
115}
orbita3d_kinematics/conversion.rs

orbita3d_kinematics/
conversion.rs
