bb_geometry/
rotation3d.rs

1//! This is the module covering anything related to rotations in 3d.
2//! So far this is quite limited in scope.
3
4
5use rand::Rng;
6use std::f64::consts::PI as PI;
7use crate::vector3d::Vector3D;
8use crate::ALMOST_ZERO;
9
10#[derive(Debug)]
11pub struct Rotation3D {
12    r_xx: f64,
13    r_xy: f64,
14    r_xz: f64,
15    r_yx: f64,
16    r_yy: f64,
17    r_yz: f64,
18    r_zx: f64,
19    r_zy: f64,
20    r_zz: f64,
21}
22
23impl Rotation3D {
24    /// This creates the unit rotation, which does not rotate anything.
25    ///
26    /// # Example:
27    /// ```
28    /// use bb_geometry::vector3d::Vector3D;
29    /// use bb_geometry::rotation3d::Rotation3D;
30    /// let r = Rotation3D::unit();
31    /// let v = Vector3D::random_vector();
32    ///
33    /// assert!(r.act_on(&v).almost_equals(&v));
34    /// ```
35    pub fn unit() -> Rotation3D {
36        Rotation3D {
37            r_xx: 1.0,
38            r_xy: 0.0,
39            r_xz: 0.0,
40            r_yx: 0.0,
41            r_yy: 1.0,
42            r_yz: 0.0,
43            r_zx: 0.0,
44            r_zy: 0.0,
45            r_zz: 1.0,
46        }
47    }
48
49    /// This method creates a 3d rotation using a specific parameterisation in terms of Euler angles.
50    ///
51    /// # Examples:
52    /// ```
53    /// use std::f64::consts::PI;
54    /// use bb_geometry::rotation3d::Rotation3D;
55    /// use bb_geometry::vector3d::*;
56    /// let r = Rotation3D::from_euler_angles(0.0, PI/2.0, 0.0);
57    ///
58    /// assert!(&r.act_on(&X).almost_equals(&Z.revert()));
59    /// assert!(&r.act_on(&Y).almost_equals(&Y));
60    /// assert!(&r.act_on(&Z).almost_equals(&X));
61    /// ```
62    pub fn from_euler_angles(psi: f64, theta: f64, phi: f64) -> Rotation3D {
63        let cos_psi = f64::cos(psi);
64        let sin_psi = f64::sin(psi);
65        let cos_theta = f64::cos(theta);
66        let sin_theta = f64::sin(theta);
67        let cos_phi = f64::cos(phi);
68        let sin_phi = f64::sin(phi);
69        Rotation3D {
70            r_xx: cos_psi * cos_theta * cos_phi + - sin_psi * sin_phi,
71            r_xy: - sin_psi * cos_theta * cos_phi -  cos_psi * sin_phi,
72            r_xz: cos_phi * sin_theta,
73            r_yx: cos_psi * sin_phi * cos_theta + sin_psi * cos_phi,
74            r_yy: -sin_psi * sin_phi * cos_theta + cos_psi * cos_phi,
75            r_yz: sin_phi * sin_theta,
76            r_zx: -cos_psi * sin_theta,
77            r_zy: sin_psi * sin_theta,
78            r_zz: cos_theta
79        }
80    }
81
82    fn almost_equal_to(&self, other: &Rotation3D) -> bool {
83        vec![
84            self.r_xx - other.r_xx,
85            self.r_xy - other.r_xy,
86            self.r_xz - other.r_xz,
87            self.r_yx - other.r_yx,
88            self.r_yy - other.r_yy,
89            self.r_yz - other.r_yz,
90            self.r_zx - other.r_zx,
91            self.r_zy - other.r_zy,
92            self.r_zz - other.r_zz,
93        ]
94        .iter()
95        .map(|c| c.abs())
96        .reduce(f64::max)
97        .unwrap()
98            < ALMOST_ZERO
99    }
100
101    pub fn inverse(&self) -> Rotation3D {
102        self.transpose()
103    }
104
105    pub fn transpose(&self) -> Rotation3D {
106        Rotation3D {
107            r_xx: self.r_xx,
108            r_xy: self.r_yx,
109            r_xz: self.r_zx,
110            r_yx: self.r_xy,
111            r_yy: self.r_yy,
112            r_yz: self.r_zy,
113            r_zx: self.r_xz,
114            r_zy: self.r_yz,
115            r_zz: self.r_zz,
116        }
117    }
118
119    pub fn act_on(&self, v: &Vector3D) -> Vector3D {
120        Vector3D::new(
121            self.r_xx * v.x + self.r_xy * v.y + self.r_xz * v.z ,
122            self.r_yx * v.x + self.r_yy * v.y + self.r_yz * v.z,
123            self.r_zx * v.x + self.r_zy * v.y + self.r_zz * v.z,
124        )
125    }
126
127    pub fn generate_random_rotations(n: usize) -> Vec<Rotation3D> {
128        let mut rng = rand::rng();
129        (0..n)
130            .map(|_| {
131                Rotation3D::from_euler_angles(
132                    rng.random_range(-3.14..=3.14),
133                    rng.random_range(0.0..=3.14),
134                    rng.random_range(-3.14..=3.14),
135                )
136            })
137            .collect()
138    }
139
140    pub fn components(&self) -> [[f64; 3]; 3] {
141        [
142            [self.r_xx, self.r_xy, self.r_xz],
143            [self.r_yx, self.r_yy, self.r_yz],
144            [self.r_zx, self.r_zy, self.r_zz],
145        ]
146    }
147
148}
149
150#[cfg(test)]
151mod test {
152    use crate::vector3d::{X, Y, Z};
153    use super::*;
154
155    #[test]
156    fn identity_does_not_map() {
157        // given
158        let unit = Rotation3D::unit();
159        let vectors = Vector3D::generate_random_vectors(10);
160
161        // when
162        let mapped_vectors = vectors
163            .iter()
164            .map(|v| unit.act_on(v))
165            .collect::<Vec<Vector3D>>();
166
167        // then
168        for (x, y) in vectors.iter().zip(mapped_vectors.iter()) {
169            assert!(x.almost_equals(y));
170        }
171    }
172
173    #[test]
174    fn transpose_is_inverse() {
175        // given
176        let vectors = Vector3D::generate_random_vectors(10);
177        let rotations = Rotation3D::generate_random_rotations(10);
178
179        for i in 0..10 {
180            // when
181            let inverse_rotation = rotations[i].inverse();
182            let mapped_vector = inverse_rotation.act_on(&rotations[i].act_on(&vectors[i]));
183
184            // then
185            assert!(
186                mapped_vector.almost_equals(&vectors[i]),
187                "Vector {:?} got mapped to {:?}",
188                mapped_vector,
189                vectors[i]
190            );
191        }
192    }
193
194    #[test]
195    fn directions_tests() {
196
197        // given
198        let r = Rotation3D::from_euler_angles(0.0, PI / 2.0, PI / 2.0);
199
200        // when & then
201        test_vector_equality(&r.act_on(&X), &Z.revert());
202        test_vector_equality(&r.act_on(&Y), &X.revert());
203        test_vector_equality(&r.act_on(&Z), &Y);
204
205        // given
206        let r = Rotation3D::from_euler_angles(PI / 2.0, PI /2.0, PI / 2.0);
207
208        // when & then
209        test_vector_equality(&r.act_on(&X), &X.revert());
210        test_vector_equality(&r.act_on(&Y), &Z);
211        test_vector_equality(&r.act_on(&Z), &Y);
212    }
213
214    #[test]
215    fn compare_rotations() {
216        // given
217        let r1 = Rotation3D::from_euler_angles(0.0, PI / 2.0, 0.0);
218        let r2 = Rotation3D::from_euler_angles(0.0, -PI / 2.0, 0.0).inverse();
219
220        // when & then
221        test_rotation_equality(&r1, &r2);
222    }
223
224    fn test_vector_equality(v1: &Vector3D, v2: &Vector3D) {
225        assert!(v1.almost_equals(&v2), "Comparing two vectors failed: \n {:?} \n {:?}", v1, v2);
226    }
227
228    fn test_rotation_equality(r1: &Rotation3D, r2: &Rotation3D) {
229        assert!(r1.almost_equal_to(&r2), "Comparing rotations failed: \n {:?} \n {:?}", r1, r2);
230    }
231
232}
bb_geometry/rotation3d.rs

bb_geometry/
rotation3d.rs
