bb_geometry/
rotation4d.rs

1//! This is the module covering anything related to rotations in 4d.
2//! So far this is quite limited in scope.
3
4
5use rand::Rng;
6use std::f64::consts::PI as PI;
7use crate::vector4d::Vector4D;
8use crate::ALMOST_ZERO;
9
10#[derive(Debug)]
11pub struct Rotation4D {
12    r_xx: f64,
13    r_xy: f64,
14    r_xz: f64,
15    r_xt: f64,
16    r_yx: f64,
17    r_yy: f64,
18    r_yz: f64,
19    r_yt: f64,
20    r_zx: f64,
21    r_zy: f64,
22    r_zz: f64,
23    r_zt: f64,
24    r_tx: f64,
25    r_ty: f64,
26    r_tz: f64,
27    r_tt: f64,
28}
29
30impl Rotation4D {
31    /// This creates the unit rotation, which does not rotate anything.
32    ///
33    /// # Example:
34    /// ```
35    /// use bb_geometry::vector4d::Vector4D;
36    /// use bb_geometry::rotation4d::Rotation4D;
37    /// let r = Rotation4D::unit();
38    /// let v = Vector4D::random_vector();
39    ///
40    /// assert!(r.act_on(&v).almost_equals(&v));
41    /// ```
42    pub fn unit() -> Rotation4D {
43        Rotation4D {
44            r_xx: 1.0,
45            r_xy: 0.0,
46            r_xz: 0.0,
47            r_xt: 0.0,
48            r_yx: 0.0,
49            r_yy: 1.0,
50            r_yz: 0.0,
51            r_yt: 0.0,
52            r_zx: 0.0,
53            r_zy: 0.0,
54            r_zz: 1.0,
55            r_zt: 0.0,
56            r_tx: 0.0,
57            r_ty: 0.0,
58            r_tz: 0.0,
59            r_tt: 1.0,
60        }
61    }
62
63    /// This method creates a 4d rotation using a specific parameterisation in terms of Euler angles.
64    /// Since only 4 angles are used, this is not sufficient to generate every rotation in the 6d space SO(4).
65    /// Still, it is enough to generate those needed for a 4d camera, which assert that a 4d head is never tilted.
66    ///
67    /// # Examples:
68    /// ```
69    /// use std::f64::consts::PI;
70    /// use bb_geometry::rotation4d::Rotation4D;
71    /// use bb_geometry::vector4d::*;
72    /// let r = Rotation4D::from_euler_angles(0.0, PI/2.0, 0.0, 0.0);
73    ///
74    /// assert!(&r.act_on(&E_X).almost_equals(&E_X));
75    /// assert!(&r.act_on(&E_Y).almost_equals(&E_Y));
76    /// assert!(&r.act_on(&E_Z).almost_equals(&E_T.revert()));
77    /// assert!(&r.act_on(&E_T).almost_equals(&E_Z));
78    /// ```
79    pub fn from_euler_angles(psi: f64, theta: f64, phi1: f64, phi2: f64) -> Rotation4D {
80        let cos_psi = f64::cos(psi);
81        let sin_psi = f64::sin(psi);
82        let cos_theta = f64::cos(theta);
83        let sin_theta = f64::sin(theta);
84        let cos_phi1 = f64::cos(phi1);
85        let sin_phi1 = f64::sin(phi1);
86        let cos_phi2 = f64::cos(phi2);
87        let sin_phi2 = f64::sin(phi2);
88        Rotation4D {
89            r_xx: cos_phi1 * cos_psi + sin_phi1 * sin_phi2 * sin_psi,
90            r_xy: cos_phi1 * sin_psi - sin_phi1 * sin_phi2 * cos_psi,
91            r_xz: sin_phi1 * cos_phi2 * cos_theta,
92            r_xt: sin_phi1 * cos_phi2 * sin_theta,
93            r_yx: -cos_phi2 * sin_psi,
94            r_yy: cos_phi2 * cos_psi,
95            r_yz: sin_phi2 * cos_theta,
96            r_yt: sin_phi2 * sin_theta,
97            r_zx: -sin_phi1 * cos_psi + cos_phi1 * sin_phi2 * sin_psi,
98            r_zy: -sin_phi1 * sin_psi - cos_phi1 * sin_phi2 * cos_psi,
99            r_zz: cos_phi1 * cos_phi2 * cos_theta,
100            r_zt: cos_phi1 * cos_phi2 * sin_theta,
101            r_tx: 0.0,
102            r_ty: 0.0,
103            r_tz: -sin_theta,
104            r_tt: cos_theta,
105        }
106    }
107
108    fn almost_equal_to(&self, other: &Rotation4D) -> bool {
109        vec![
110            self.r_tt - other.r_tt,
111            self.r_tz - other.r_tz,
112            self.r_tx - other.r_tx,
113            self.r_ty - other.r_ty,
114            self.r_xx - other.r_xx,
115            self.r_xy - other.r_xy,
116            self.r_xz - other.r_xz,
117            self.r_xt - other.r_xt,
118            self.r_yx - other.r_yx,
119            self.r_yy - other.r_yy,
120            self.r_yz - other.r_yz,
121            self.r_yt - other.r_yt,
122            self.r_zt - other.r_zt,
123            self.r_zx - other.r_zx,
124            self.r_zy - other.r_zy,
125            self.r_zz - other.r_zz,
126        ]
127        .iter()
128        .map(|c| c.abs())
129        .reduce(f64::max)
130        .unwrap()
131            < ALMOST_ZERO
132    }
133
134    pub fn inverse(&self) -> Rotation4D {
135        self.transpose()
136    }
137
138    pub fn transpose(&self) -> Rotation4D {
139        Rotation4D {
140            r_xx: self.r_xx,
141            r_xy: self.r_yx,
142            r_xz: self.r_zx,
143            r_xt: self.r_tx,
144            r_yx: self.r_xy,
145            r_yy: self.r_yy,
146            r_yz: self.r_zy,
147            r_yt: self.r_ty,
148            r_zx: self.r_xz,
149            r_zy: self.r_yz,
150            r_zz: self.r_zz,
151            r_zt: self.r_tz,
152            r_tx: self.r_xt,
153            r_ty: self.r_yt,
154            r_tz: self.r_zt,
155            r_tt: self.r_tt,
156        }
157    }
158
159    pub fn act_on(&self, v: &Vector4D) -> Vector4D {
160        Vector4D::new(
161            self.r_xx * v.x + self.r_xy * v.y + self.r_xz * v.z + self.r_xt * v.t,
162            self.r_yx * v.x + self.r_yy * v.y + self.r_yz * v.z + self.r_yt * v.t,
163            self.r_zx * v.x + self.r_zy * v.y + self.r_zz * v.z + self.r_zt * v.t,
164            self.r_tx * v.x + self.r_ty * v.y + self.r_tz * v.z + self.r_tt * v.t,
165        )
166    }
167
168    pub fn generate_random_rotations(n: usize) -> Vec<Rotation4D> {
169        let mut rng = rand::rng();
170        (0..n)
171            .map(|_| {
172                Rotation4D::from_euler_angles(
173                    rng.random_range(-3.14..=3.14),
174                    rng.random_range(0.0..=3.14),
175                    rng.random_range(-3.14..=3.14),
176                    rng.random_range(-3.14..=3.14),
177                )
178            })
179            .collect()
180    }
181
182}
183
184#[cfg(test)]
185mod test {
186    use crate::vector4d::{E_X, E_Y, E_Z, E_T};
187    use super::*;
188
189    #[test]
190    fn identity_does_not_map() {
191        // given
192        let unit = Rotation4D::unit();
193        let vectors = Vector4D::generate_random_vectors(10);
194
195        // when
196        let mapped_vectors = vectors
197            .iter()
198            .map(|v| unit.act_on(v))
199            .collect::<Vec<Vector4D>>();
200
201        // then
202        for (x, y) in vectors.iter().zip(mapped_vectors.iter()) {
203            assert!(x.almost_equals(y));
204        }
205    }
206
207    #[test]
208    fn transpose_is_inverse() {
209        // given
210        let vectors = Vector4D::generate_random_vectors(10);
211        let rotations = Rotation4D::generate_random_rotations(10);
212
213        for i in 0..10 {
214            // when
215            let inverse_rotation = rotations[i].inverse();
216            let mapped_vector = inverse_rotation.act_on(&rotations[i].act_on(&vectors[i]));
217
218            // then
219            assert!(
220                mapped_vector.almost_equals(&vectors[i]),
221                "Vector {:?} got mapped to {:?}",
222                mapped_vector,
223                vectors[i]
224            );
225        }
226    }
227
228    #[test]
229    fn directions_tests() {
230
231        // given
232        let r = Rotation4D::from_euler_angles(0.0, PI / 2.0, PI / 2.0, 0.0);
233
234        // when & then
235        test_vector_equality(&r.act_on(&E_X), &E_Z.revert());
236        test_vector_equality(&r.act_on(&E_Y), &E_Y);
237        test_vector_equality(&r.act_on(&E_Z), &E_T.revert());
238        test_vector_equality(&r.act_on(&E_T), &E_X);
239
240        // given
241        let r = Rotation4D::from_euler_angles(0.0, PI / 2.0, 0.0, PI / 2.0);
242
243        // when & then
244        test_vector_equality(&r.act_on(&E_X), &E_X);
245        test_vector_equality(&r.act_on(&E_Y), &E_Z.revert());
246        test_vector_equality(&r.act_on(&E_Z), &E_T.revert());
247        test_vector_equality(&r.act_on(&E_T), &E_Y);
248
249        // given
250        let r = Rotation4D::from_euler_angles(0.0, PI / 2.0, PI /2.0, PI / 2.0);
251
252        // when & then
253        test_vector_equality(&r.act_on(&E_X), &E_Z.revert());
254        test_vector_equality(&r.act_on(&E_Y), &E_X.revert());
255        test_vector_equality(&r.act_on(&E_Z), &E_T.revert());
256        test_vector_equality(&r.act_on(&E_T), &E_Y);
257    }
258
259    #[test]
260    fn compare_rotations() {
261        // given
262        let r1 = Rotation4D::from_euler_angles(0.0, PI / 2.0, 0.0, 0.0);
263        let r2 = Rotation4D::from_euler_angles(0.0, -PI / 2.0, 0.0, 0.0).inverse();
264
265        // when & then
266        test_rotation_equality(&r1, &r2);
267
268        // given
269        let r1 = Rotation4D::from_euler_angles(0.0, PI / 2.0, 0.0, 0.0);
270        let r2 = Rotation4D::from_euler_angles(0.0, PI / 2.0, 2.0 * PI, 0.0);
271        let r3 = Rotation4D::from_euler_angles(0.0, PI / 2.0, 0.0, PI * 2.0);
272
273        // when & then
274        test_rotation_equality(&r1, &r2);
275        test_rotation_equality(&r1, &r3);
276    }
277
278    fn test_vector_equality(v1: &Vector4D, v2: &Vector4D) {
279        assert!(v1.almost_equals(&v2), "Comparing two vectors failed: \n {:?} \n {:?}", v1, v2);
280    }
281
282    fn test_rotation_equality(r1: &Rotation4D, r2: &Rotation4D) {
283        assert!(r1.almost_equal_to(&r2), "Comparing rotations failed: \n {:?} \n {:?}", r1, r2);
284    }
285
286}
bb_geometry/rotation4d.rs

bb_geometry/
rotation4d.rs
