re_math/
quat_ext.rs

1use glam::{Quat, Vec3};
2
3/// Extensions to [`Quat`]
4pub trait QuatExt: Sized {
5    /// Return a Quaternion that rotates -Z to the `forward` direction,
6    /// using `up` to control roll, so that +Y will approximately point in the `up` direction.
7    ///
8    /// Will return [`None`] if either argument is zero, non-finite, or if they are colinear.
9    ///
10    /// This is generally what you want to use to construct a view-rotation when -Z is forward and +Y is up (this is what Ark uses!).
11    fn rotate_negative_z_towards(forward: Vec3, up: Vec3) -> Option<Quat>;
12
13    /// Return a Quaternion that rotates +Z to the `forward` direction,
14    /// using `up` to control roll, so that +Y will approximately point in the `up` direction.
15    ///
16    /// Will return [`None`] if either argument is zero, non-finite, or if they are colinear.
17    ///
18    /// This is generally what you want to use to construct a view-rotation when +Z is forward and +Y is up.
19    fn rotate_positive_z_towards(forward: Vec3, up: Vec3) -> Option<Quat>;
20}
21
22impl QuatExt for Quat {
23    fn rotate_negative_z_towards(forward: Vec3, up: Vec3) -> Option<Quat> {
24        let forward = forward.normalize_or_zero();
25        let side = forward.cross(up).normalize_or_zero(); // `side` is right in a right-handed system
26        let up = side.cross(forward);
27
28        if forward != Vec3::ZERO && side != Vec3::ZERO && up != Vec3::ZERO {
29            Some(Self::from_mat3(&glam::Mat3::from_cols(side, up, -forward)))
30        } else {
31            None
32        }
33    }
34
35    fn rotate_positive_z_towards(forward: Vec3, up: Vec3) -> Option<Quat> {
36        let forward = forward.normalize_or_zero();
37        let side = up.cross(forward).normalize_or_zero(); // `side` is left in a right-handed system
38        let up = forward.cross(side);
39
40        if forward != Vec3::ZERO && side != Vec3::ZERO && up != Vec3::ZERO {
41            Some(Self::from_mat3(&glam::Mat3::from_cols(side, up, forward)))
42        } else {
43            None
44        }
45    }
46}
47
48#[cfg(test)]
49mod test {
50    use super::*;
51
52    #[test]
53    fn test_rotate_negative_z_towards() {
54        #![allow(clippy::disallowed_methods)] // normalize
55
56        let desired_fwd = Vec3::new(1.0, 2.0, 3.0).normalize();
57        let desired_up = Vec3::new(4.0, 5.0, 6.0).normalize();
58
59        let rot = Quat::rotate_negative_z_towards(desired_fwd, desired_up).unwrap();
60
61        let rotated_z = rot * -Vec3::Z;
62        assert!(
63            (rotated_z - desired_fwd).length() < 1e-5,
64            "Expected to rotate -Z to {desired_fwd}, but got {rotated_z}"
65        );
66
67        let rotated_y = rot * Vec3::Y;
68        assert!(
69            rotated_y.dot(desired_up) >= 0.0,
70            "Expected to rotate +Y to point approximately towards {desired_up}, but got {rotated_y}",
71        );
72    }
73
74    #[test]
75    fn test_rotate_positive_z_towards() {
76        #![allow(clippy::disallowed_methods)] // normalize
77
78        let desired_fwd = Vec3::new(1.0, 2.0, 3.0).normalize();
79        let desired_up = Vec3::new(4.0, 5.0, 6.0).normalize();
80
81        let rot = Quat::rotate_positive_z_towards(desired_fwd, desired_up).unwrap();
82
83        let rotated_z = rot * Vec3::Z;
84        assert!(
85            (rotated_z - desired_fwd).length() < 1e-5,
86            "Expected to rotate +Z to {desired_fwd}, but got {rotated_z}",
87        );
88
89        let rotated_y = rot * Vec3::Y;
90        assert!(
91            rotated_y.dot(desired_up) >= 0.0,
92            "Expected to rotate +Y to point approximately towards {desired_up}, but got {rotated_y}",
93        );
94    }
95}
re_math/quat_ext.rs

re_math/
quat_ext.rs
