bb_geometry/linear_algebra/rotation3x3/
mod.rs

1use crate::linear_algebra::matrix3x3::Matrix3x3;
2
3impl Matrix3x3 {
4    pub fn rotation_from_euler_angles(psi: f32, theta: f32, phi: f32) -> Matrix3x3 {
5        let cos_psi = f32::cos(psi);
6        let sin_psi = f32::sin(psi);
7        let cos_theta = f32::cos(theta);
8        let sin_theta = f32::sin(theta);
9        let cos_phi = f32::cos(phi);
10        let sin_phi = f32::sin(phi);
11        Matrix3x3::new([
12            [
13                cos_psi * cos_theta * cos_phi + -sin_psi * sin_phi,
14                -sin_psi * cos_theta * cos_phi - cos_psi * sin_phi,
15                cos_phi * sin_theta,
16            ],
17            [
18                cos_psi * sin_phi * cos_theta + sin_psi * cos_phi,
19                -sin_psi * sin_phi * cos_theta + cos_psi * cos_phi,
20                sin_phi * sin_theta,
21            ],
22            [-cos_psi * sin_theta, sin_psi * sin_theta, cos_theta],
23        ])
24    }
25
26    pub fn is_orthogonal(&self) -> bool {
27        (self * self.transpose() - Matrix3x3::identity()).is_almost_zero()
28    }
29}
30
31#[cfg(test)]
32mod tests {
33    use super::*;
34    use crate::linear_algebra::vector3::{E_X, E_Y, E_Z};
35    use std::f32::consts::PI;
36    #[test]
37    fn rotations_are_orthogonal() {
38        let r = Matrix3x3::rotation_from_euler_angles(0.1, 0.2, 0.3);
39
40        assert!(r.is_orthogonal());
41    }
42
43    #[test]
44    fn directions_tests() {
45        // given
46        let r = Matrix3x3::rotation_from_euler_angles(0.0, PI / 2.0, PI / 2.0);
47
48        // when & then
49        assert!((r * E_X + E_Z).is_almost_zero());
50        assert!((r * E_Y + E_X).is_almost_zero());
51        assert!((r * E_Z - E_Y).is_almost_zero());
52
53        // given
54        let r = Matrix3x3::rotation_from_euler_angles(PI / 2.0, PI / 2.0, PI / 2.0);
55
56        // when & then
57        assert!((r * E_X + E_X).is_almost_zero());
58        assert!((r * E_Y - E_Z).is_almost_zero());
59        assert!((r * E_Z - E_Y).is_almost_zero());
60    }
61
62    #[test]
63    fn compare_rotations() {
64        // given
65        let r1 = Matrix3x3::rotation_from_euler_angles(0.0, PI / 2.0, 0.0);
66        let r2 = Matrix3x3::rotation_from_euler_angles(0.0, -PI / 2.0, 0.0).transpose();
67
68        // when & then
69        assert_eq!(r1, r2);
70    }
71}
bb_geometry/linear_algebra/rotation3x3/mod.rs

bb_geometry/linear_algebra/rotation3x3/
mod.rs
