spatialrust_math/
quat.rs

1use crate::{Mat3, Real, Vec3};
2
3/// Unit quaternion representing a 3D rotation.
4#[derive(Clone, Copy, Debug, PartialEq)]
5#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
6pub struct Quat<T: Real> {
7    /// X component.
8    pub x: T,
9    /// Y component.
10    pub y: T,
11    /// Z component.
12    pub z: T,
13    /// W (scalar) component.
14    pub w: T,
15}
16
17impl<T: Real> Quat<T> {
18    /// Creates a quaternion from components.
19    #[must_use]
20    pub const fn new(x: T, y: T, z: T, w: T) -> Self {
21        Self { x, y, z, w }
22    }
23}
24
25impl Quat<f32> {
26    /// Identity rotation for `f32`.
27    #[must_use]
28    pub fn identity() -> Self {
29        Self::new(0.0, 0.0, 0.0, 1.0)
30    }
31
32    /// Normalizes the quaternion.
33    #[must_use]
34    pub fn normalize(self) -> Self {
35        let len = (self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w).sqrt();
36        if len == 0.0 {
37            return Self::identity();
38        }
39        Self { x: self.x / len, y: self.y / len, z: self.z / len, w: self.w / len }
40    }
41
42    /// Creates a quaternion from an axis-angle representation.
43    #[must_use]
44    pub fn from_axis_angle(axis: Vec3<f32>, angle: f32) -> Self {
45        let axis = axis.normalize();
46        let half = angle * 0.5;
47        let s = half.sin();
48        Self::new(axis.x * s, axis.y * s, axis.z * s, half.cos()).normalize()
49    }
50
51    /// Converts the quaternion to a rotation matrix.
52    #[must_use]
53    pub fn to_mat3(self) -> Mat3<f32> {
54        let q = self.normalize();
55        let xx = q.x * q.x;
56        let yy = q.y * q.y;
57        let zz = q.z * q.z;
58        let xy = q.x * q.y;
59        let xz = q.x * q.z;
60        let yz = q.y * q.z;
61        let wx = q.w * q.x;
62        let wy = q.w * q.y;
63        let wz = q.w * q.z;
64
65        Mat3::from_rows(
66            [1.0 - 2.0 * (yy + zz), 2.0 * (xy + wz), 2.0 * (xz - wy)],
67            [2.0 * (xy - wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz + wx)],
68            [2.0 * (xz + wy), 2.0 * (yz - wx), 1.0 - 2.0 * (xx + yy)],
69        )
70        .transpose()
71    }
72
73    /// Hamilton product.
74    #[must_use]
75    #[allow(clippy::should_implement_trait)]
76    pub fn mul(self, other: Self) -> Self {
77        Self {
78            x: self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
79            y: self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
80            z: self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w,
81            w: self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
82        }
83        .normalize()
84    }
85}
86
87impl Quat<f64> {
88    /// Identity rotation for `f64`.
89    #[must_use]
90    pub fn identity() -> Self {
91        Self::new(0.0, 0.0, 0.0, 1.0)
92    }
93
94    /// Normalizes the quaternion.
95    #[must_use]
96    pub fn normalize(self) -> Self {
97        let len = (self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w).sqrt();
98        if len == 0.0 {
99            return Self::identity();
100        }
101        Self { x: self.x / len, y: self.y / len, z: self.z / len, w: self.w / len }
102    }
103
104    /// Converts the quaternion to a rotation matrix.
105    #[must_use]
106    pub fn to_mat3(self) -> Mat3<f64> {
107        let q = self.normalize();
108        let xx = q.x * q.x;
109        let yy = q.y * q.y;
110        let zz = q.z * q.z;
111        let xy = q.x * q.y;
112        let xz = q.x * q.z;
113        let yz = q.y * q.z;
114        let wx = q.w * q.x;
115        let wy = q.w * q.y;
116        let wz = q.w * q.z;
117
118        Mat3::from_rows(
119            [1.0 - 2.0 * (yy + zz), 2.0 * (xy + wz), 2.0 * (xz - wy)],
120            [2.0 * (xy - wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz + wx)],
121            [2.0 * (xz + wy), 2.0 * (yz - wx), 1.0 - 2.0 * (xx + yy)],
122        )
123        .transpose()
124    }
125}
126
127#[cfg(test)]
128mod tests {
129    use super::{Quat, Vec3};
130    use crate::tolerance::{approx_eq, f32_eps};
131
132    #[test]
133    fn quat_identity_to_mat3() {
134        let m = Quat::<f32>::identity().to_mat3();
135        let v = Vec3::new(1.0, 2.0, 3.0);
136        let out = m.mul_vec3(v);
137        assert!(approx_eq(out.x, v.x, f32_eps()));
138        assert!(approx_eq(out.y, v.y, f32_eps()));
139        assert!(approx_eq(out.z, v.z, f32_eps()));
140    }
141
142    #[test]
143    fn quat_axis_angle_90deg_z() {
144        let q = Quat::from_axis_angle(Vec3::new(0.0, 0.0, 1.0), std::f32::consts::FRAC_PI_2);
145        let m = q.to_mat3();
146        let out = m.mul_vec3(Vec3::new(1.0, 0.0, 0.0));
147        assert!(approx_eq(out.x, 0.0, 1e-5));
148        assert!(approx_eq(out.y, 1.0, 1e-5));
149    }
150}
spatialrust_math/quat.rs

spatialrust_math/
quat.rs
