gfx_maths/
quaternion.rs

1use std::fmt::Display;
2
3use crate::Vec3;
4
5use auto_ops::impl_op_ex;
6
7#[derive(Debug, Clone, Copy, PartialEq)]
8#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
9#[repr(C)]
10pub struct Quaternion {
11    pub x: f32,
12    pub y: f32,
13    pub z: f32,
14    pub w: f32,
15}
16
17impl Default for Quaternion {
18    /// Creates an identity rotation
19    fn default() -> Self {
20        Self::identity()
21    }
22}
23
24impl Display for Quaternion {
25    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
26        let Self { x, y, z, w } = self;
27        write!(f, "({x}, {y}, {z}, {w})")
28    }
29}
30
31impl Quaternion {
32    pub const fn new(x: f32, y: f32, z: f32, w: f32) -> Self {
33        Self { x, y, z, w }
34    }
35
36    /// Creates (0, 0, 0, 1), which represents no rotation
37    pub const fn identity() -> Self {
38        Self {
39            x: 0.0,
40            y: 0.0,
41            z: 0.0,
42            w: 1.0,
43        }
44    }
45
46    /// Creates a rotation of `radians` radians around `axis`.
47    ///
48    /// The rotation will be counter clock wise when looking along the direction of `axis`.
49    pub fn axis_angle(mut axis: Vec3, radians: f32) -> Self {
50        axis.normalize();
51        axis *= (radians * 0.5).sin();
52
53        Self {
54            x: axis.x,
55            y: axis.y,
56            z: axis.z,
57            w: (radians * 0.5).cos(),
58        }
59    }
60
61    /// Returns the vector (1, 0, 0) rotated by `self`
62    pub fn right(&self) -> Vec3 {
63        Vec3 {
64            x: self.x * self.x - self.y * self.y - self.z * self.z + self.w * self.w,
65            y: 2.0 * (self.z * self.w + self.x * self.y),
66            z: 2.0 * (self.x * self.z - self.y * self.w),
67        }
68    }
69
70    /// Returns the vector (0, 1, 0) rotated by `self`
71    pub fn up(&self) -> Vec3 {
72        Vec3 {
73            x: 2.0 * (self.x * self.y - self.z * self.w),
74            y: -self.x * self.x + self.y * self.y - self.z * self.z + self.w * self.w,
75            z: 2.0 * (self.x * self.w + self.y * self.z),
76        }
77    }
78
79    /// Returns the vector (0, 0, 1) rotated by `self`
80    pub fn forward(&self) -> Vec3 {
81        Vec3 {
82            x: 2.0 * (self.x * self.z + self.y * self.w),
83            y: 2.0 * (self.y * self.z - self.x * self.w),
84            z: -self.x * self.x - self.y * self.y + self.z * self.z + self.w * self.w,
85        }
86    }
87
88    /// Creates a Quaternion from euler angles in radians
89    ///
90    /// The rotation order is Z -> Y -> X
91    pub fn from_euler_radians_zyx(euler: &Vec3) -> Self {
92        let cx = (euler.x * 0.5).cos();
93        let cy = (euler.y * 0.5).cos();
94        let cz = (euler.z * 0.5).cos();
95
96        let sx = (euler.x * 0.5).sin();
97        let sy = (euler.y * 0.5).sin();
98        let sz = (euler.z * 0.5).sin();
99
100        Self {
101            x: sx * cy * cz - cx * sy * sz,
102            y: cx * sy * cz + sx * cy * sz,
103            z: cx * cy * sz - sx * sy * cz,
104            w: cx * cy * cz + sx * sy * sz,
105        }
106    }
107
108    /// Creates a Quaternion from euler angles in degrees
109    ///
110    /// The rotation order is Z -> Y -> X
111    pub fn from_euler_angles_zyx(euler: &Vec3) -> Self {
112        Self::from_euler_radians_zyx(&Vec3::new(
113            euler.x.to_radians(),
114            euler.y.to_radians(),
115            euler.z.to_radians(),
116        ))
117    }
118
119    /// Converts this Quaternion to euler angles in radians
120    ///
121    /// The rotation order is Z -> Y -> X
122    pub fn to_euler_radians_zyx(&self) -> Vec3 {
123        Vec3 {
124            x: f32::atan2(
125                2.0 * (self.w * self.x + self.y * self.z),
126                1.0 - 2.0 * (self.x * self.x + self.y * self.y),
127            ),
128            y: f32::asin(2.0 * (self.w * self.y - self.z * self.x)),
129            z: f32::atan2(
130                2.0 * (self.w * self.z + self.x * self.y),
131                1.0 - 2.0 * (self.y * self.y + self.z * self.z),
132            ),
133        }
134    }
135
136    /// Converts this Quaternion to euler angles in degrees
137    ///
138    /// The rotation order is Z -> Y -> X
139    pub fn to_euler_angles_zyx(&self) -> Vec3 {
140        let rad = self.to_euler_radians_zyx();
141        Vec3::new(rad.x.to_degrees(), rad.y.to_degrees(), rad.z.to_degrees())
142    }
143}
144
145impl_op_ex!(*|a: &Quaternion, b: &Quaternion| -> Quaternion {
146    Quaternion {
147        x: a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
148        y: a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z,
149        z: a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x,
150        w: a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
151    }
152});
153
154impl_op_ex!(*|a: &Quaternion, b: &Vec3| -> Vec3 {
155    let x2 = a.x * a.x;
156    let y2 = a.y * a.y;
157    let z2 = a.z * a.z;
158    let w2 = a.w * a.w;
159
160    let xx = a.x * b.x;
161    let yy = a.y * b.y;
162    let zz = a.z * b.z;
163
164    Vec3 {
165        x: b.x * (x2 - y2 - z2 + w2)
166            + 2.0 * (a.x * yy + a.x * zz + a.w * a.y * b.z - a.w * a.z * b.y),
167        y: b.y * (-x2 + y2 - z2 + w2)
168            + 2.0 * (a.y * xx + a.y * zz + a.w * a.z * b.x - a.w * a.x * b.z),
169        z: b.z * (-x2 - y2 + z2 + w2)
170            + 2.0 * (a.z * xx + a.z * yy + a.w * a.x * b.y - a.w * a.y * b.x),
171    }
172});
173
174impl_op_ex!(-|a: &Quaternion| -> Quaternion {
175    Quaternion {
176        x: -a.x,
177        y: -a.y,
178        z: -a.z,
179        w: a.w,
180    }
181});
182
183impl From<[f32; 4]> for Quaternion {
184    fn from(d: [f32; 4]) -> Self {
185        Self {
186            x: d[0],
187            y: d[1],
188            z: d[2],
189            w: d[3],
190        }
191    }
192}
gfx_maths/quaternion.rs

gfx_maths/
quaternion.rs
